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

最新推荐文章于 2024-04-17 11:21:21 发布

wswtyy

最新推荐文章于 2024-04-17 11:21:21 发布

阅读量3.6k

点赞数 4

本文链接：https://blog.csdn.net/qq_41764032/article/details/100080762

版权

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<<endl<<xyz.type()<<endl;
                    FileStorage f("point3d.yml", FileStorage::WRITE);
//                    f.open("point3d.yml", FileStorage::WRITE);
                    if( f.isOpened() )
                    {
                        f<<"xyz"<<xyz;
                        f.release();
                    }
                    imshow("img1",img1);
//                    imwrite("obj.jpg",img1);
                    cout<<img1.cols<<"   "<<img1.rows<<endl;
                    cout<<disp8.cols<<"    "<<disp8.rows<<endl;
//                    cout<<"type:    "<<disp8.type()<<endl;

                    saveXYZ(point_cloud_filename.c_str(), xyz);
                    printf("\n");

这里用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<Point> obj_point1,vector<Point> obj_point2,Mat P1,Mat P2)
{


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

    vector<Point> top_point1;
    top_point1.resize(num);
    vector<Point> 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<Point3D.rows;i++)
//    {
//        for(int j=0;j<Point3D.cols;j++)
//        {
//            float a=Point3D.at<float>(i,j);
//            cout<<" "<<a<<"          ";
//        }
//        cout<<endl;
//    }
//    cout<<Point3D.rows<<"     "<<Point3D.cols<<endl;
    Mat b;
    Mat Point3D_transposition=Mat(4,4,CV_32F);
    cv::transpose(Point3D,Point3D_transposition);
    convertPointsFromHomogeneous(Point3D_transposition,b);
//    cout<<b.rows<<"     "<<b.cols<<endl<<b.type()<<endl;
    Vec3f p1=b.at<Vec3f>(0,0);
    Vec3f p2=b.at<Vec3f>(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=    "<<distances<<endl;

    return distances;
}

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

效果图（点的对应在图右上角）
在这里插入图片描述

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