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]： camera position 1

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

旋转整个场景以使相机回到原始位置，这样我就可以确定2处的顶点位置： enter image description here

用于准备此内容的3DS Max文件为max 1，max 2和max 3

以下是顶点之前和之后的位置，内在函数等： vertices and intrinsics

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

here are the numbers in an Excel file

我需要的代码，必须可以很容易地翻译成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等;这就是你必须找到的＆gt ;; - ）

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°

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

Answer 2

用于计算2D或3D点云的两个快照之间差异的“正确”算法称为ICP（Iterative Closest Point）。该算法解决了 ICP

以人类可读格式：对于给定的点集，P1和P2找到将P1转换为P2的旋转矩阵R和平移T.只要确保它们的来源正常化。

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

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

Answer 3

Iterative Closest Point算法（ICP）现在是C＃/ VB官方Kinect SDK 1.7的一部分

在VB中恢复相机姿势非常简单。

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

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

3 个答案: