第七章 采用AAM和POSIT的3D头部姿态估计(5)

投入到POSIT——Diving into POSIT

为了使POSIT工作，我们至少需要4个非共面模型3D点和它们各自的2D图像中的匹配。我们增加一个结束标准，因为POSIT是一个迭代算法——一般地它是迭代的次数或者一个距离参数。然后，我们调用cvPOSIT函数，这个函数产生旋转矩阵和平移矢量。

作为一个例子，我们将跟随着JavierBrandianran编写的手册，它用POSIT来获得一个立方体的姿态。模型用4个点创建。用下面的代码初始化：

float cubeSize = 10.0;

std::vector modelPoints;

modelPoints.push_back(cvPoint3D32f(0.0f, 0.0f, 0.0f));

modelPoints.push_back(cvPoint3D32f(0.0f, 0.0f, cubeSize)); modelPoints.push_back(cvPoint3D32f(cubeSize, 0.0f, 0.0f)); modelPoints.push_back(cvPoint3D32f(0.0f, cubeSize, 0.0f));

CvPOSITObject *positObject = cvCreatePOSITObject( &modelPoints[0], static_cast(modelPoints.size()) );

注意模型本身用cvCreatePOSITObject方法创建，它返回一个将在cvPOSIT函数中使用的一个CvPOSITObject方法。注意，姿态将参考第一个模型点来计算，这使得将它放在原点是个好的主意。

然而，我们需要将2D图像点放入到另外一个vector中。记住，这些点必须以同样的顺序放置在数组中，模型的点插入到这个数组中。这样，第i个2D图像点匹配第i个3D模型点。 这里，一个问题是2D图像点的原点位于图像的中心，这可能需要你平移它们。你可以插入下面的2D图像点（当然，它们将根据用户的匹配变化）。

std::vector srcImagePoints;

srcImagePoints.push_back( cvPoint2D32f( -48, -224 ) ); srcImagePoints.push_back( cvPoint2D32f( -287, -174 ) ); srcImagePoints.push_back( cvPoint2D32f( 132, -153 ) ); srcImagePoints.push_back( cvPoint2D32f( -52, 149 ) );

现在，你仅需要为矩阵分配内存和创建终止标准，接着调用cvPOSIT，就像下面的代码片段展示的那样： //Estimate the pose

CvMatr32f rotation_matrix = new float[9]; CvVect32f translation_vector = new float[3];

CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_EPS | CV_