opencv - 在OpenCV中使用POSIT算法

我遵循http://opencv.willowgarage.com/wiki/Posit教程在OpenCV中练习POSIT算法

我的目标是估计3D对象的姿势，尤其是解决未知的深度问题。在本教程中，它的立方体边为10mm。

部分代码:

std::vector<CvPoint2D32f> projectedPoints;
...
CvMat poseMatrix = cvMat( 4, 4, CV_32F, pose );  // pose moves object coord. to camera coord.
for ( size_t  p=0; p<modelPoints.size(); p++ )
{
    float modelPoint[] =  { modelPoints[p].x, modelPoints[p].y, modelPoints[p].z, 1.0f };
    CvMat modelPointMatrix = cvMat( 4, 1, CV_32F, modelPoint );
    float point3D[4];
    CvMat point3DMatrix = cvMat( 4, 1, CV_32F, point3D )
    //Transform the points from model space coordinates to camera space
    //The pose must be transposed because is in OpenGL format
    cvGEMM( &poseMatrix, &modelPointMatrix, 1.0, NULL, 0.0, &point3DMatrix, CV_GEMM_A_T );
    //Project the transformed 3D points
    CvPoint2D32f point2D = cvPoint2D32f( 0.0, 0.0 );
    if ( point3D[2] != 0 )
    {
            point2D.x = cvmGet( intrinsics, 0, 0 ) * point3D[0] / point3D[2]; // this is fx * X/Z
            point2D.y = cvmGet( intrinsics, 1, 1 ) * point3D[1] / point3D[2]; // this is fy * Y/Z
    }
    projectedPoints.push_back( point2D );
}

根据等式，Z是已知的。 POSIT如何解决未知深度？或在本教程中解决？

任何想法，请!

最佳答案

cvPOSIT估计摄像机和模型的坐标系之间的平移(X，Y，Z)和旋转 vector 。您发布的代码使用位姿矩阵计算模型点的投影。在本教程中，该函数与模型的实际姿势(在实际场景中未知)和POSIT估算的姿势一起使用，以便比较两个投影。
cvGEMS仅以一个矩阵乘法执行点变换(R * p + T)。请注意，模型点p具有4个坐标(x，y，z，1)。