Extraer homografía proyectiva de dos mapas de profundidad Kinect

Question

Dadas dos nubes de puntos 3D consecutivas 1 y 2 (no toda la nube, digamos 100 puntos seleccionados de la nube con GoodFeaturesToMatch de OpenCV), obtenida de un mapa de profundidad Kinect, quiero calcular la homografía de la cámara de 1 a 2. Entiendo que esto es una transformación proyectiva, y ya lo han hecho muchas personas: here (slide 12), here (slide 30) y here in what seems to be the classic paper. Mi problema es que, aunque soy un programador competente, no tengo las habilidades matemáticas o trigonométricas para convertir uno de esos métodos en código. Como este no es un problema fácil, ofrezco una gran recompensa por el código que resuelve el siguiente problema:

La cámara está en el origen, mirando en la dirección Z, en el pentaedro irregular [A, B, C, D , e, F]:

la cámara se mueve -90mm a la izquierda (X), + 60 mm hacia arriba (y), + de 50 mm hacia adelante (Z) y gira 5 ° hacia abajo, a 10 ° a la derecha y -3 ° en sentido antihorario:

que gira toda la escena para que la cámara está de nuevo en su posición original me permite determinar las ubicaciones de los vértices a las 2:

Los archivos 3DS Max utilizados para preparar este son max 1, max 2 y max 3

Aquí están las posiciones de los vértices antes y después, los intrínsecos, etc .:

Tenga en cuenta que los vértices de la Cámara 2 estén no es 100% exacto, hay un poco de ruido deliberado.

here are the numbers in an Excel file

El código que necesito, que debe ser fácilmente traducible en VB.Net o C#, usando EMGUCV y OpenCV en caso necesario, lleva a los 2 conjuntos de vértices y los intrínsecos y produce esta salida:

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 

no sé si el homografía es 3X3 o 3X4 de coordenadas homogéneas, pero debe permitir que traduzca los vértices de 1 a 2.

Asimismo, no sé los valores a1, a2, etc .; eso es lo que tienes que encontrar> ;-)

La oferta de 500 recompensas 'reemplaza' la recompensa que ofrecí a this very similar question, he agregado un comentario que apunta a esta pregunta.

EDIT2 EDIT2: Me pregunto si la forma en que hago esta pregunta es engañosa. Me parece que el problema radica más en la adaptación de nubes de puntos que en la geometría de la cámara (si sabes cómo traducir y rotar de A a B, conoces la transformación de la cámara y viceversa). Si es así, entonces tal vez la solución podría ser obtenida con el algoritmo de Kabsch o algo similar

Answer 1

El algoritmo "correcto" para calcular la diferencia entre dos instantáneas de nubes de puntos 2D o 3D se denomina ICP (Iterative Closest Point). El algoritmo resuelve

en formato legible para humanos: Para los conjuntos de puntos P1 y P2 dados, encuentre la matriz de rotación R y la traslación T que transforma P1 en P2. Solo asegúrate de que estén normalizados en torno a su origen.

El algoritmo es conceptualmente simple y se usa comúnmente en tiempo real. Revisa iterativamente la transformación (traducción, rotación) necesaria para minimizar la distancia entre los puntos de dos escaneos sin formato.

Para los interesados ​​este es un tema dentro computacional de procesamiento geométrico

Answer 2

Para aquellos con necesidades similares, he aquí una solución parcial usando el algoritmo de Kabsch para determinar la traslación y rotación óptima de una pieza de geometría 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 

salida:

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° 

pero todavía no puedo encontrar la manera de determinar la posición de la cámara.

Answer 3

El iterativo algoritmo punto más cercano (ICP) es ahora una parte de la Kinect SDK oficial de 1,7 para C#/VB

Recuperación de la cámara posar en VB es bastante fácil entonces.

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