Opencv:SolvePNP

背景介绍

由于实验室项目的原因，最近学习了基于PNP方法的绝对位姿测量。
如果场景的三维结构已知，利用多个控制点在三维场景中的坐标及其在图像中的透视投影坐标即可求解出摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系，包括绝对平移向量t以及旋转矩阵R，该类求解方法统称为N点透视位姿求解（Perspective-N-Point，PNP问题）。这里的控制点是指准确知道三维空间坐标位置，同时也知道对应图像平面坐标的点。对于透视投影来说，要使得PNP问题有确定解，需要至少三组控制点。

image.png

经典的P3P问题可以转化为一个四面体形状的确定问题，如图所示。即已知条件为知道控制点 A,B,C的位置以及在摄像机中的投影坐标求棱长边a,b,c的问题。通过余弦定理，再利用点云配准方法可以得到摄像机坐标系相对于世界坐标系的平移以及旋转。图中的P点相当于相机的光心，A,B,C相当于世界坐标系下已知相对位置关系的三个控制点，A',B',C'为图像坐标系中对应的三个点。PNP解决的是纯数学问题，数学证明在此处省略。

Opencv中PNP的求解函数

void solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags = CV_ITERATIVE)

Parameters:

objectPoints - 世界坐标系下的控制点的坐标，vector<Point3f>的数据类型在这里可以使用
imagePoints - 在图像坐标系下对应的控制点的坐标。vector<Point2f>在这里可以使用
cameraMatrix - 相机的内参矩阵
distCoeffs - 相机的畸变系数
以上两个参数通过相机标定可以得到。相机的内参数的标定参见：http://www.cnblogs.com/star91/p/6012425.html
rvec - 输出的旋转向量。使坐标点从世界坐标系旋转到相机坐标系
tvec - 输出的平移向量。使坐标点从世界坐标系平移到相机坐标系
flags - 默认使用CV_ITERATIV迭代法

算法使用

由于opencv2以上版本已经提供了pnp算法的api，所以使用pnp的难点变成了如何构造场景使得能使用PNP算法。目前我们使用的最简单的方法就是使用四个点，使用物体方法使得场景中只出现这四个控制点。
代码如下：

     #将控制点在世界坐标系的坐标压入容器
    vector<Point3f> objP;
    Mat objM;
    objP.clear();
    objP.push_back(Point3f(0, 0, 0));
    objP.push_back(Point3f(150, 0, 0));
    objP.push_back(Point3f(150, 150, 0));
    objP.push_back(Point3f(0, 150, 0));
    Mat(objP).convertTo(objM, CV_32F);

GaussianBlur(src, src, Size(5, 5), 1.5);
imshow("滤波后的图", src);
//二值化
cvtColor(src, src, CV_BGR2GRAY);
threshold(src, src, 180, 255, THRESH_BINARY);
imshow("二值化后的图", src);

//边缘检测
Canny(src, out, thresh, thresh * 3, 3);
imshow("边缘检测后的图", out);

//查找轮廓
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
findContours(out, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
if (contours.size() == 0)     //没找到任何轮廓
{
    cout << "未找到任何轮廓！" << endl;
    continue;
}
else if (contours.size() != 4)   //找到的轮廓不是4个，说明之前图像未处理好，有干扰
{
    cout << "找到的轮廓不是4个！" << endl;
    continue;
}

//计算轮廓矩       
vector<Moments> mu(contours.size());
for (int i = 0; i < contours.size(); i++)
{
    mu[i] = moments(contours[i], false);
}

//计算轮廓的质心     
vector<Point2f> mc(contours.size());
for (int i = 0; i < contours.size(); i++)
{
    mc[i] = Point2d(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
    //points[0].push_back(mc[i]);
    dis[i] = sqrt(double(mc[i].x  * mc[i].x + mc[i].y * mc[i].y));
    dis_x[i] = mc[i].x;
    dis_y[i] = mc[i].y;
    //cout << "第" << i << "个轮廓中心为" << mc[i].x << "\t" << mc[i].y << endl;
    
}

//对四个小灯的位置排序
double min = dis[0];
double max = dis[0];
double max_x = 0;
double max_y = 0;
int min_id = 0;
int max_id = 0;
int max_x_id = 0;
int max_y_id = 0;

for (int m = 1; m < contours.size(); m++)
{

    if (dis[m] <= min)
    {
        min = dis[m];
        min_id = m;

    }
    if (dis[m] >= max)
    {
        max = dis[m];
        max_id = m;
    }
}
for (int m = 0; m < contours.size(); m++)
{
    if ((m != min_id) && (m != max_id))
    {
        if (dis_x[m] >= max_x)
        {
            max_x = dis_x[m];
            max_x_id = m;
        }
        if (dis_y[m] >= max_y)
        {
            max_y = dis_y[m];
            max_y_id = m;
        }
    }
}
//目标四个点按顺时针顺序压入points中，左上角是第一个点
points.clear();
points.push_back(mc[min_id]);
points.push_back(mc[max_x_id]);
points.push_back(mc[max_id]);
points.push_back(mc[max_y_id]);

//使用pnp解算求出相机矩阵和畸变系数矩阵
    Rodrigues(rotM, rvec);  //将旋转矩阵变换成旋转向量
    solvePnP(objM, Mat(points), camera_matrix, distortion_coefficients, rvec, tvec);
    Rodrigues(rvec, rotM);  //将旋转向量变换成旋转矩阵
    Rodrigues(tvec, rotT);

根据旋转矩阵和平移矩阵求出旋转角度和深度信息

根据旋转矩阵求出坐标旋转角

//根据旋转矩阵求出坐标旋转角
    theta_x = atan2(rotM.at<double>(2, 1), rotM.at<double>(2, 2));
    theta_y = atan2(-rotM.at<double>(2, 0),
    sqrt(rotM.at<double>(2, 1)*rotM.at<double>(2, 1) + rotM.at<double>(2, 2)*rotM.at<double>(2, 2)));
    theta_z = atan2(rotM.at<double>(1, 0), rotM.at<double>(0, 0));

    //将弧度转化为角度
    theta_x = theta_x * (180 / PI);
    theta_y = theta_y * (180 / PI);
    theta_z = theta_z * (180 / PI);

求解坐标系旋转角度的求解方法参考：https://stackoverflow.com/questions/15022630/how-to-calculate-the-angle-from-roational-matrix

根据旋转矩阵和平移矩阵求出深度信息