kinect - 从两个 Kinect 深度图中提取投影单应性

Question

给定从 Kinect 深度图获得的两个连续 3D 点云 1 和 2(不是整个云，比如说使用 OpenCV 的 GoodFeaturesToMatch 从云中选择的 100 个点)，我想从 1 到 2 计算相机的单应性。我知道这是一个投影改造，已经有很多人做过了:here (slide 12) , here (slide 30)和 here in what seems to be the classic paper .我的问题是，虽然我是一名称职的程序员，但我没有将其中一种方法转化为代码的数学或触发技能。由于这不是一个简单的问题，我为解决以下问题的代码提供了大量奖励:

相机位于原点，从 Z 方向看，不规则五面体 [A,B,C,D,E,F]:


相机向左移动 -90mm (X)，向上移动 +60mm (Y)，向前移动 +50mm (Z) 并向下旋转 5°，向右旋转 10°，逆时针旋转 -3°:


旋转整个场景使相机回到其原始位置，这样我就可以确定顶点的位置为 2:


用于准备的 3DS Max 文件是 max 1 , max 2和 max 3

以下是顶点前后的位置、内在函数等:


请注意，camera2 的顶点不是 100% 准确，有一些故意的噪音。

here are the numbers in an Excel file

我需要的代码必须很容易翻译成 VB.Net 或 C#，在必要时使用 EMGUCV 和 OpenCV，采用两组顶点和内在函数并产生以下输出:

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

我不知道齐次坐标的单应性是 3X3 还是 3X4，但它必须允许我将顶点从 1 转换为 2。

我也不知道 a1、a2 等值；这就是你必须找到的 >;-)

500 赏金优惠“取代”了我提供给 this very similar question 的赏金，我在那里添加了一条评论，指向这个问题。

EDIT2:我想知道我问这个问题的方式是否具有误导性。在我看来，问题更多是点云拟合而不是相机几何(如果您知道如何将 A 转换和旋转到 B，您就知道相机变换，反之亦然)。如果是这样，那么也许可以使用 Kabsch 算法或类似的方法获得解决方案

Answer 1

对于那些有类似需求的人，这里有一个使用 Kabsch 算法来确定一块 3D 几何体的平移和最佳旋转的部分解决方案:

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

输出:

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°

但我仍然无法弄清楚如何确定相机的位置。