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

Question

给定从 Kinect 深度图获得的两个连续的 3D 点云 1 和 2（不是整个云，例如使用 OpenCV 的 GoodFeaturesToMatch 从云中选择的 100 个点），我想计算相机从 1 到 2 的单应性。我知道这是一个投影变换，并且已经有很多人完成了：此处（幻灯片 12），此处（幻灯片 30） 和 这里似乎是经典论文。我的问题是，虽然我是一名称职的程序员，但我没有数学或三角技能将其中一种方法转化为代码。由于这不是一个简单的问题，我为解决以下问题的代码提供了大量赏金：

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

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

旋转整个场景，使相机回到原来的位置，让我能够确定顶点的位置2：

用于准备此内容的 3DS Max 文件为 最大 1, 最大限度2 和 max 3

这是之前和之后的顶点位置，内在函数等：

请注意，camera2 的顶点并非 100% 准确，存在一些故意的噪声。

这是 Excel 文件中的数字

我需要的代码，必须易于翻译进入 VB.Net 或 C#，在必要时使用 EMGUCV 和 OpenCV，获取 2 组顶点和内在函数并生成以下输出：

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 赏金报价“取代”了我向 提供的赏金，这非常相似问题，我在那里添加了一条指向这个问题的评论。

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

Answer 1

用于计算 2D 或 3D 点云的两个快照之间的差异的“正确”算法称为 ICP（迭代最近点）。 解决 问题

该算法以人类可读格式 ：对于给定的点集 P1 和 P2，找到旋转矩阵 R 和平移矩阵 T将 P1 转换为 P2。只要确保它们围绕其起源进行标准化即可。

该算法概念简单，通常用于实时。它迭代地修改最小化两个原始扫描点之间的距离所需的变换（平移、旋转）。

对于那些感兴趣的人来说，这是计算几何处理中的一个主题

Answer 2

对于那些有类似需求的人，这里有一个使用 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°

但我仍然不知道如何确定相机的位置。

Answer 3

迭代最近点算法 (ICP) 现在是用于 C#/VB 的官方 Kinect SDK 1.7 的一部分，

那么在 VB 中恢复相机姿势就非常容易了。

http://www.microsoft.com/en-us/kinectforwindows/develop /new.aspx