For those with similar needs, here's a partial solution using Kabsch's algorithm to determine the translation and optimal rotation of a piece of 3D geometry:
Imports Emgu
Imports Emgu.CV
Imports Emgu.CV.Structure
Imports Emgu.CV.CvInvoke
Imports Emgu.CV.CvEnum
Imports System.Math
Module Module1
' A 2*2 cube, centred on the origin
Dim matrixA(,) As Double = {{-1, -1, -1},
{1, -1, -1},
{-1, 1, -1},
{1, 1, -1},
{-1, -1, 1},
{1, -1, 1},
{-1, 1, 1},
{1, 1, 1}
}
Dim matrixB(,) As Double
Function Translate(ByVal mat As Matrix(Of Double), ByVal translation As Matrix(Of Double)) As Matrix(Of Double)
Dim tx As New Matrix(Of Double)({{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{translation(0, 0), translation(1, 0), translation(2, 0), 1}})
Dim mtx As New Matrix(Of Double)(mat.Rows, mat.Cols + 1)
' Convert from Nx3 to Nx4
For i As Integer = 0 To mat.Rows - 1
For j As Integer = 0 To mat.Cols - 1
mtx(i, j) = mat(i, j)
Next
mtx(i, mat.Cols) = 1
Next
mtx = mtx * tx
Dim result As New Matrix(Of Double)(mat.Rows, mat.Cols)
For i As Integer = 0 To mat.Rows - 1
For j As Integer = 0 To mat.Cols - 1
result(i, j) = mtx(i, j)
Next
Next
Return result
End Function
Function Rotate(ByVal mat As Matrix(Of Double), ByVal rotation As Matrix(Of Double)) As Matrix(Of Double)
Dim sinx As Double = Sin(rotation(0, 0))
Dim siny As Double = Sin(rotation(1, 0))
Dim sinz As Double = Sin(rotation(2, 0))
Dim cosx As Double = Cos(rotation(0, 0))
Dim cosy As Double = Cos(rotation(1, 0))
Dim cosz As Double = Cos(rotation(2, 0))
Dim rm As New Matrix(Of Double)(3, 3)
rm(0, 0) = cosy * cosz
rm(0, 1) = -cosx * sinz + sinx * siny * cosz
rm(0, 2) = sinx * sinz + cosx * siny * cosz
rm(1, 0) = cosy * sinz
rm(1, 1) = cosx * cosz + sinx * siny * sinz
rm(1, 2) = -sinx * cosz + cosx * siny * sinz
rm(2, 0) = -siny
rm(2, 1) = sinx * cosy
rm(2, 2) = cosx * cosy
Return mat * rm
End Function
Public Sub Main()
Dim ma As Matrix(Of Double)
Dim mb As Matrix(Of Double)
ma = New Matrix(Of Double)(matrixA)
' Make second matrix by rotating X=5°, Y=6°, Z=7° and translating X+2, Y+3, Z+4
mb = ma.Clone
mb = Rotate(mb, New Matrix(Of Double)({radians(5), radians(6), radians(7)}))
mb = Translate(mb, New Matrix(Of Double)({2, 3, 4}))
Dim tx As Matrix(Of Double) = Nothing
Dim rx As Matrix(Of Double) = Nothing
Dim ac As Matrix(Of Double) = Nothing
Dim bc As Matrix(Of Double) = Nothing
Dim rotation As Matrix(Of Double) = Nothing
Dim translation As Matrix(Of Double) = Nothing
Dim xr As Double, yr As Double, zr As Double
Kabsch(ma, mb, ac, bc, translation, rotation, xr, yr, zr)
ShowMatrix("A centroid", ac)
ShowMatrix("B centroid", bc)
ShowMatrix("Translation", translation)
ShowMatrix("Rotation", rotation)
console.WriteLine(degrees(xr) & "° " & degrees(yr) & "° " & degrees(zr) & "°")
System.Console.ReadLine()
End Sub
Function radians(ByVal a As Double)
Return a * Math.PI / 180
End Function
Function degrees(ByVal a As Double)
Return a * 180 / Math.PI
End Function
''' <summary>
''' Compute translation and optimal rotation between 2 matrices using Kabsch's algorithm
''' </summary>
''' <param name="p">Starting matrix</param>
''' <param name="q">Rotated and translated matrix</param>
''' <param name="pcentroid">returned (3,1), centroid(p)</param>
''' <param name="qcentroid">returned (3,1), centroid(q)</param>
''' <param name="translation">returned (3,1), translation to get q from p</param>
''' <param name="rotation">returned (3,3), rotation to get q from p</param>
''' <param name="xr">returned, X rotation in radians</param>
''' <param name="yr">returned, Y rotation in radians</param>
''' <param name="zr">returned, Z rotation in radians</param>
''' <remarks>nomeclature as per http://en.wikipedia.org/wiki/Kabsch_algorithm</remarks>
Sub Kabsch(ByVal p As Matrix(Of Double), ByVal q As Matrix(Of Double),
ByRef pcentroid As Matrix(Of Double), ByRef qcentroid As Matrix(Of Double),
ByRef translation As Matrix(Of Double), ByRef rotation As Matrix(Of Double),
ByRef xr As Double, ByRef yr As Double, ByRef zr As Double)
Dim zero As New Matrix(Of Double)({0, 0, 0})
Dim a As Matrix(Of Double)
Dim v As New Matrix(Of Double)(3, 3)
Dim s As New Matrix(Of Double)(3, 3)
Dim w As New Matrix(Of Double)(3, 3)
Dim handed As Matrix(Of Double)
Dim d As Double
pcentroid = Centroid(p)
qcentroid = Centroid(q)
translation = qcentroid - pcentroid
p = Translate(p, zero - pcentroid) ' move p to the origin
q = Translate(q, zero - qcentroid) ' and q too
a = p.Transpose * q ' 3x3 covariance
cvSVD(a, s, v, w, SVD_TYPE.CV_SVD_DEFAULT)
d = System.Math.Sign(a.Det)
handed = New Matrix(Of Double)({{1, 0, 0}, {0, 1, 0}, {0, 0, 1}})
handed.Data(2, 2) = d
rotation = v * handed * w.Transpose ' optimal rotation matrix, U
' Extract X,Y,Z angles from rotation matrix
yr = Asin(-rotation(2, 0))
xr = Asin(rotation(2, 1) / Cos(yr))
zr = Asin(rotation(1, 0) / Cos(yr))
End Sub
Function Centroid(ByVal m As Matrix(Of Double)) As Matrix(Of Double)
Dim result As New Matrix(Of Double)(3, 1)
Dim ui() As Double = {0, 0, 0}
For i As Integer = 0 To m.Rows - 1
For j As Integer = 0 To 2
ui(j) = ui(j) + m(i, j)
Next
Next
For i As Integer = 0 To 2
result(i, 0) = ui(i) / m.Rows
Next
Return result
End Function
Sub ShowMatrix(ByVal name As String, ByVal m As Matrix(Of Double))
console.WriteLine(name)
For i As Integer = 0 To m.Rows - 1
For j As Integer = 0 To m.Cols - 1
console.Write(m(i, j) & " ")
Next
console.WriteLine("")
Next
End Sub
End Module
Output:
A centroid
0
0
0
B centroid
2
3
4
Translation
2
3
4
Rotation
0.987108879970813 -0.112363244371414 0.113976139595516
0.121201730390574 0.989879474775675 -0.0738157569097856
-0.104528463267653 0.0866782944696306 0.990737439302028
5° 6° 7°
but I still can't figure out how to determine the camera's position.
