使用POSIT算法估计物体角度

POSIT算法(DeMenthon & Davis 1995) 能够根据2D图像中的若干特征点位置，估计出物体在3D上角度（比如人脸）。下面是使用OpenCV的POSIT算法的使用示例，引自：opencv posit sample

1、对象声明

CvPOSITObject* positObject;

就是这个简单的CvPOSITObject的结构体。

2、POSIT模型的初始化

std::vector modelPoints;
modelPoints.push_back(cvPoint3D32f(0.0f, 0.0f, 0.0f)); //The first must be (0,0,0)
modelPoints.push_back(cvPoint3D32f(0.0f, 0.0f, CUBE_SIZE));
modelPoints.push_back(cvPoint3D32f(CUBE_SIZE, 0.0f, 0.0f));
modelPoints.push_back(cvPoint3D32f(0.0f, CUBE_SIZE, 0.0f));

//Create the POSIT object with the model points
positObject = cvCreatePOSITObject( &modelPoints[0], (int)modelPoints.size() );

这里的CUBE_SIZE=10。这一步主要是用要输入物体在3D空间的坐标(x,y,z)，POSIT算法就是参考这些3D的信息，再与后面要输入的2D坐标信息进行相应的计算，估计得出最终的POSE。如果物体是一个人脸，那就需要用一些特定点（如鼻子、嘴、眼角等位置）的3D位置信息进行初始化。

3、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 ) );

当前输入的即是物体在2D图像中的位置信息（x,y)。一般情况下，需要对这些坐标信息使用集距和中心点进行归一化处理。

4、角度估计

//Estimate the pose
CvMatr32f rotation_matrix = new float[9];
CvVect32f translation_vector = new float[3];	
CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 100, 1.0e-4f);
cvPOSIT( positObject, &srcImagePoints[0], FOCAL_LENGTH, criteria, rotation_matrix, translation_vector );

FOCAL_LENGTH为焦距，criteria为指定的迭代次数和迭代误差，rotation_matrix为所要求的旋转矩阵，translation_vector为所要求的平移矩阵。

其中rotation_matrix包含了物体的角度信息。

5、计算各个欧拉角

旋转矩阵是一个3*3的矩阵，可以使用OpenGL等将物体角度显示出来。如果要计算物体相对于X/Y/Z轴的角度，还需要再将矩阵转换为三个角度，见：点击打开链接

如果能够自动检测物体（比如人脸）指定特征点的2D位置，又能够预先知道这些特征点在3D模型的坐标，即可对物体的pose进行自动的角度估计。可参考：点击打开链接，它是使用函数solvePnP来实现姿态的估计，但是流程基本类似，并且它包括使用OpenGL显示的问题。