從兩個Kinect深度圖提取投影單應圖

Question

從Kinect深度圖中獲取兩個連續的三維點雲1和2（不是整個雲，例如從雲中選擇的100個點，從OpenCV的GoodFeaturesToMatch中選擇），我想計算相機的單應性從1到2.我明白這是一個投影變換，它已經被很多人完成了：here (slide 12)，here (slide 30)和here in what seems to be the classic paper。我的問題是，雖然我是一名稱職的程序員，但我還沒有掌握數學或觸發技能來將其中一種方法轉化爲代碼。由於這不是一個簡單的問題，所以我提供瞭解決以下問題的大量代碼：

相機位於原點，在Z方向上看不規則的五面體[A，B，C，D ，E，F]：

相機向左（X），+ 60mm向上（Y），+ 50mm向前（Z）移動90mm並向下旋轉5°，向右旋轉10°，向下旋轉-3°逆時針：

旋轉整個場景，使相機回到原來的位置讓我確定在2頂點的位置：

用來製備本是max 1，max 2和max 3

這裏3ds Max的文件之前和之後的頂點的位置，內部函數等：

注意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，等等;這就是你必須找到的;-)

500賞金提議'取代'我提供給this very similar question的賞金，我已經添加了一條評論，指出這個問題。

編輯2：我想知道如果我問這個問題的方式是誤導。在我看來，問題更多的是點雲擬合而不是相機幾何（如果你知道如何平移和旋轉A到B，你知道相機變換，反之亦然）。如果是這樣的話，或許該解決方案可以與Kabsch的算法或類似

Answer 1

用於計算2D或3D點雲的兩個快照之間差異的「正確」算法稱爲ICP（Iterative Closest Point）。該算法求解

以人可讀的格式：對於給定的點集P1和P2找到旋轉矩陣R和將P1轉換爲P2的平移T.只要確保它們在原點周圍正常化。

該算法在概念上很簡單並且通常實時使用。它迭代地修改爲最小化兩個原始掃描點之間的距離所需的變換（平移，旋轉）。

對於那些有興趣，這是相機的計算幾何處理

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）現在是官方的Kinect SDK 1.7的對C＃中的一部分/ VB

恢復中的話題然後在VB中構成很容易。

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