OpenCV立体匹配结果求三维坐标代码及解释

OpenCV立体匹配结果求三维坐标代码

在OpenCV源码的258-261行，代码是将输出输出转化为8位的图像输出，原来输出的disp是16位float类型的，是无法作为RGB图像输出的，现在转成8位单通道图像才能imwrite输出。 源码262-278行是输出对应视差图像。
最后主要是通过矩阵Q转化为三维坐标，所用的主要是reprojectImageTo3D,看下图中的第四个参数释义，如果是没有匹配上的点，也就是视差图中黑色的点它的Z值会被置成10000，当我们看点云的三维坐标时，就可以由此看到漏匹配点的个数。

之后我们可以得到xyz这个Mat矩阵，他是一个三通道的Mat，其中每一个像素点（姑且将其理解为类似RGB形式的Mat图像，只不过每个通道是16位）的值有三个通道，分别代表了该像素点的空间上X,Y,Z的坐标值，坐标原点是左摄像头的光心。xyz之后是输出为Point3D文件，在另外的地方要用的话，就读入它为Mat再用.at读数据，但这样用源码往往结果不对，下文会讲到如何修改。

但是在这里建议是自己再输出一个yml文件，这样子就能看到三维坐标和点云了。
我的代码如下：

if(!point_cloud_filename.empty())//点入点云数据
                {
                    printf("storing the point cloud...");
                    fflush(stdout);//强制输出
                    Mat tmp;
//                    disp=depth_fill(disp);
                    disp.convertTo(tmp, CV_32FC1, 1 / 16.0);
                    reprojectImageTo3D(tmp, xyz, Q, true);
//                    cout<

这里用filestorage输出yml文件，而最重要的是这一句：

 disp.convertTo(tmp, CV_32FC1, 1 / 16.0);

将disp转成32位，同时除16，关键在于除16，具体原因我也不是特别清楚了，好像是数据往前移了4位，要移回来吧。但是就是要除上16才能得到正确值，如果你只是看看点云长啥样那可以不除，但你要是想算距离就必须要除上。
但是输出为yml文件有个坏处，就是你没办法用源码的筛选函数，你要自己重写写一个，或者可以和我一样，改写saveXYZ为输出yml格式文件，再或者就是，输出yml格式文件看效果，后续计算还是用saveXYZ出来的文件。

查看点云如图：


有了基本平面的样子，但矫正矩阵效果不好，还要重新标定，如果说得到的点云是一个很锥形的云，如果之前你已经除了16，那就是你标定矩阵不好，你要进行重新进行摄像机标定。

计算面积和距离的函数可以自己写，我这边就讲一种计算空间直线距离的最简单方法。主要用到的函数是triangulatePoints，这里感谢一波学长。

官网源码释义，前面两个参数就是之前相机标定里extrinsics文件中的P1和P2，三四两个参数就是输入的左右图像上的一对对应特征点（此时的左右图要是极线对准的，参数是通过图像识别算得的像素坐标值），但是要注意，必须是以规定格式输入，我用的是vector，而且要求是float类型的，最后一个参数也要注意，输出的是对应点的齐次坐标，是一个四维向量，要先转换到标准坐标系下，这里可以用convertPointsFromHomogeneous函数，但是要注意的是两者格式不同，triangulatePoints函数得到的坐标中每个点的坐标是以列向量的形式表示的，而convertPointsFromHomogeneous是以行向量表示的，所以，在转换到标准坐标系之前要先转置一下结果矩阵，这里用transpose

我的计算代码是：

float cal_max_distance(vector obj_point1,vector obj_point2,Mat P1,Mat P2)
{


    int num=4;
    vector input1;
    input1.resize(num);
    vector input2;
    input2.resize(num);

    vector top_point1;
    top_point1.resize(num);
    vector top_point2;
    top_point2.resize(num);
    top_point1=get_max_distance_points(obj_point1);
    top_point2=get_max_distance_points(obj_point2);

    input1=points_int_to_float(top_point1);
    input2=points_int_to_float(top_point2);

    Mat Point3D;
    cv::triangulatePoints(P1,P2,input1,input2,Point3D);
//    for(int i=0;i(i,j);
//            cout<<" "<(0,0);
    Vec3f p2=b.at(1,0);
    double distances=sqrt(pow((p1.val[0]-p2.val[0]),2)+pow((p1.val[1]-p2.val[1]),2)+pow((p1.val[2]-p2.val[2]),2));
    cout<<"distance=    "<

里面部分函数我是另外写的，作用和函数名一样。

效果图（点的对应在图右上角）




可以看到误差不大，基本准确。