Pour ceux qui ont des besoins similaires, voici une solution partielle en utilisant l'algorithme de Kabsch pour déterminer la translation et de rotation optimale d'un morceau de géométrie 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
sortie:
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°
mais je ne peux toujours pas comprendre comment déterminer la position de l'appareil.
Salut, moi encore, j'ai eu un coup d'oeil et oui je pense que cela est douable dans OpenCV et, espérons-EMGU J'essaie d'obtenir le code de travail, mais sa prenant un certain temps sous position. CalibrateCamera2 dans opencv devrait être ce qui est nécessaire la documentation est ici: http://opencv.willowgarage.com/documentation/python/calib3d_camera_calibration_and_3d_reconstruction.html#calibratecamera2 J'ai trouvé un exemple dans opencv ici http://www.neuroforge.co .uk/index.php/77-tutorials/78-camera-calibration-python-opencv Tout ce qui est maintenant requis est de le transposer en quelque chose que vous pouvez utiliser. Regardez ça, espérons que ça aide – Chris