位姿估计_3

这里是位姿估计的第三讲，利用灭点（vanishing point）来估计相机位姿。

灭点 vanishing point

灭点的介绍，wiki百科，简单的说，就是在真是物理世界中相互平行的两条直线，在相机的2d投影中，会汇聚相交到一点，该点就是灭点或者消失点（vanishing point），抽象描述物理世界中的无穷远处。

如图所示

图中的灰色部分表示一个十字路口，其中在Z方向的无穷远处有一点Z_inf，其在真实世界中的坐标为Z_inf = [0, 0, 1, 0]。我的理解是，在Z方向的无穷远处，X和Y是远小于Z的，可以忽略为零，而且在该坐标的表达方式上，w=0，也表示该点位于Z的无穷远处。
在十字路口在相机的2D投影中，可以看到无穷远处的路两边是相交到一点了，这就是灭点，其中Zv对应Z_inf点

这里旋转矩阵R=[r1,r2,r3]，内参矩阵为K。
根据相机成像原理有
z*Vz = K[r1, r2, r3 | t]Z_inf, 即 z*Vz = K[r1, r2, r3 | t][0, 0, 1, 0]T，如下

化简可得
z*Vz = Kr3, ==> r3 = (k逆）Vz / ||(k逆）Vz||，如下

利用沿着z方向的灭点可以求出旋转矩阵的r3，同理，利用沿着x方向的灭点可以求出旋转矩阵的r1，沿着y方向的灭点可以求出旋转矩阵的r2，但是在求旋转矩阵的时候只需要知道两个方向的r即可，第三个方向上的r可以通过已知两个方向上的r叉乘得到
如下

利用灭点求解位姿步骤

1. 准备一张比较好的图片
2. 分别找出两组平行线上的两个点，计算得出灭点
3. 结合相机内参矩阵，计算分别得到两个方向上的r，并归一化
4. 两个互相垂直的r叉乘得到另外一个维度上的r

至此就得到了旋转矩阵R，但是这里依然得不到平移向量t，因为仅靠旋转是没有办法得出的

结果

1. 教程上的求解灭点展示
   

2. 自己求解灭点展示

疑惑

使用灭点求解的旋转矩阵R怎么和使用arcuo库函数得到的结果不一样呐，理解的朋友帮忙指点一下吧，谢谢！！！

代码

    //这里的cornerReturn是vector<Point2f>类型的，包含四个元素，分为两组，是两条垂直直线上的点

    Point2f vanish_y;
    float ky1=0, ky2=0;
    ky1 = (cornerReturn[0].y - cornerReturn[3].y)/(cornerReturn[0].x - cornerReturn[3].x);
    ky2 = (cornerReturn[1].y - cornerReturn[2].y)/(cornerReturn[1].x - cornerReturn[2].x);
    cout<<"ky  is "<<ky1<<" "<<ky2<<endl;
    vanish_y.x = (ky1*cornerReturn[0].x - cornerReturn[0].y - ky2*cornerReturn[1].x +cornerReturn[1].y)/(ky1 - ky2);
    vanish_y.y = ky1*(vanish_y.x - cornerReturn[0].x) + cornerReturn[0].y;
    line(frame, cornerReturn[0], vanish_y, Scalar(0, 0, 255), 2);
    line(frame, cornerReturn[1], vanish_y, Scalar(0, 0, 255), 2);
    cout<<"y vanishing point is "<<vanish_y<<endl;

    //x vanishing point
    Point2f vanish_x;
    float kx1=0, kx2=0;
    kx1 = (cornerReturn[0].y - cornerReturn[1].y)/(cornerReturn[0].x - cornerReturn[1].x);
    kx2 = (cornerReturn[2].y - cornerReturn[3].y)/(cornerReturn[2].x - cornerReturn[3].x);
    cout<<"kx  is "<<kx1<<" "<<kx2<<endl;
    vanish_x.x = (kx1*cornerReturn[0].x - cornerReturn[0].y - kx2*cornerReturn[3].x +cornerReturn[3].y)/(kx1 - kx2);
    vanish_x.y = kx1*(vanish_x.x - cornerReturn[1].x) + cornerReturn[1].y;
    line(frame, cornerReturn[0], vanish_x, Scalar(255, 0, 0), 2);
    line(frame, cornerReturn[3], vanish_x, Scalar(255, 0, 0), 2);
    cout<<"x vanishing point is "<<vanish_x<<endl;

    Mat Vx = (Mat_<float>(3, 1) << vanish_x.x, vanish_x.y, 1.f);
    Mat Vy = (Mat_<float>(3, 1) << vanish_y.x, vanish_y.y, 1.f);
    Mat r1 = Mat(3,1,CV_32F);
    Mat r2 = Mat(3,1,CV_32F);
    Mat kInv = intrinsic_matrixCommon.inv();
    cout<<"kInv  is "<<kInv<<endl;

    float normX;
    Mat rTemp1 = kInv*Vx;
    normX = abs(rTemp1.at<float>(0,0)) + abs(rTemp1.at<float>(1,0)) + abs(rTemp1.at<float>(2,0));
    cout<<"rTemp1  is "<<rTemp1<<endl<<"normX  is "<<normX<<endl;
    //第一个方向上的r，并归一化
    r1 = rTemp1/normX;
    cout<<"r1  is "<<r1<<endl;
    cout<<"******************* "<<endl;

    float normY;
    Mat rTemp2 = kInv*Vy;
    normY = abs(rTemp2.at<float>(0,0)) + abs(rTemp2.at<float>(1,0)) + abs(rTemp2.at<float>(2,0));
    cout<<"rTemp2  is "<<rTemp2<<endl<<"normY  is "<<normY<<endl;
    //第二个方向上的r，并归一化
    r2 = rTemp2/normY;
    cout<<"r2  is "<<r2<<endl;

    Mat r3 = Mat(3,1,CV_32F);
    //叉乘得到第三个方向上的r
    r3 = r1.cross(r2);      
    cout<<"r3  is "<<r3<<endl;
    cout<<"******************* "<<endl;

    //这是得到的旋转矩阵
    Mat rotateMatrix = Mat(3,3,CV_32F);
    rotateMatrix.at<float>(0,0) = r1.at<float>(0,0);//x
    rotateMatrix.at<float>(1,0) = r1.at<float>(1,0);
    rotateMatrix.at<float>(2,0) = r1.at<float>(2,0);
    rotateMatrix.at<float>(0,1) = r2.at<float>(0,0);//y
    rotateMatrix.at<float>(1,1) = r2.at<float>(1,0);
    rotateMatrix.at<float>(2,1) = r2.at<float>(2,0);
    rotateMatrix.at<float>(0,2) = r3.at<float>(0,0);//z
    rotateMatrix.at<float>(1,2) = r3.at<float>(1,0);
    rotateMatrix.at<float>(2,2) = r3.at<float>(2,0);
    cout<<"rotateMatrix  is "<<endl<<rotateMatrix<<endl;