Извлечь проективную гомографию из двух карт глубины Kinect

Ответ 1

"Правильный" алгоритм, используемый для вычисления разницы между двумя моментальными снимками 2D или 3D точечных облаков, называется ICP (Iterative Closest Point), Алгоритм решает

В человеко-читаемом формате: для заданных точек P1 и P2 найти матрицу поворота R и трансляцию T, которая преобразует P1 в P2. Просто убедитесь, что они нормализованы вокруг их происхождения.

Алгоритм концептуально прост и обычно используется в режиме реального времени. Он итеративно пересматривает преобразование (перевод, поворот), необходимое для минимизации расстояния между точками двух необработанных сканирований.

Для тех, кого это интересует, это тема в области обработки вычислительной геометрии

Ответ 2

Для тех, у кого схожие потребности, здесь частичное решение с использованием алгоритма Кабша для определения перевода и оптимального вращения части 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 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°

но я все еще не могу понять, как определить положение камеры.

Ответ 3

Итеративный алгоритм "Ближайшая точка" (ICP) теперь является частью официального Kinect SDK 1.7 для С#/VB

Восстановление позы камеры в VB довольно легко.

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