Extract projective homography from two Kinect depth maps

Question

Given two consecutive 3D point clouds 1 and 2 (not the whole cloud, say 100 points selected from the cloud with OpenCV's GoodFeaturesToMatch), obtained from a Kinect depthmap, I want to compute camera's homography from 1 to 2. I understand that this a projective transform, and it has already been done by many people: here (slide 12), here (slide 30) and here in what seems to be the classic paper. My problem is that whilst I'm a competent programmer, I haven't got the math or trig skills to turn one of those methods into code. As this is not an easy problem, I offer a large bounty for the code that solves the following problem:

The camera is at the origin, looking in the Z direction, at irregular pentahedron [A,B,C,D,E,F]:

The camera moves -90mm to the left (X), +60mm up (Y), +50mm forwards (Z) and rotates 5° down, 10° right and -3° anticlockwise:

Rotating the entire scene so that the camera is back at its original position allow me to determine the vertices' locations at 2:

The 3DS Max files used to prepare this are max 1, max 2 and max 3

Here are the vertices' positions before and after, the intrinsics, etc.:

Note that camera2's vertices are not 100% accurate, there's a bit of deliberate noise.

here are the numbers in an Excel file

The code I need, which must be readily translatable into VB.Net or C#, using EMGUCV and OpenCV where necessary, takes the 2 sets of vertices and the intrinsics and produces this output:

Camera 2 is at -90 X, +60 Y, +50 Z rotated -5 Y, 10 X, -3 Z.
The homography matrix to translate points in A to B is:
a1, a2, a3
b1, b2, b3
c1, c2, c3

I don't know if the homography is 3X3 or 3X4 for homogenous coordinates, but it must allow me to translate the vertices from 1 to 2.

I also don't know the values a1, a2, etc; that's what you have to find >;-)

The 500 bounty offer 'replaces' the bounty I offered to this very similar question, I've added a comment there pointing to this question.

EDIT2: I'm wondering if the way I'm asking this question is misleading. It seems to me that the problem is more of point-cloud fitting than of camera geometry (if you know how to translate and rotate A to B, you know the camera transform and vice-versa). If so, then perhaps the solution could be obtained with Kabsch's algorithm or something similar

Answer 1

"The correct" algorithm to use for computing difference between two snapshots of 2D or 3D point clouds is called ICP (Iterative Closest Point). The algorithm solves

In human-readable-format: For given point sets P1 and P2 find the rotation matrix R and translation T that transforms P1 to P2. Just make sure they are normalized around their origin.

The algorithm is conceptually simple and is commonly used in real-time. It iteratively revises the transformation (translation, rotation) needed to minimize the distance between the points of two raw scans.

For those interested this is a topic within Computational Geometry Processing

Answer 2

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.