《SLAM十四讲》7.6 三角测量 triangulation.cpp

最新推荐文章于 2024-03-29 20:03:10 发布

sxxblogs

最新推荐文章于 2024-03-29 20:03:10 发布

阅读量2.4k

点赞数 7

CC 4.0 BY-SA版权

分类专栏： SLAM学习文章标签： slam opencv

本文链接：https://blog.csdn.net/sunnyblogs/article/details/107701493

本文详细解读了三角测量在SLAM中的应用，通过OpenCV的triangulatePoints函数实现3D坐标获取。核心步骤包括利用对极几何求解R和t，将相机外参转化为投影矩阵，以及将像素坐标转换为相机坐标。文中还探讨了像素到相机坐标的转换公式，并强调了数据类型的一致性对函数正确运行的重要性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

三角测量是为了解决单目图像无法获取深度的问题，本博客是代码解释所以不解释三角测量的原理。先上全部代码：

#include <iostream>


int main ( int argc, char** argv )
{
   
    if ( argc != 3 )
    {
   
        cout<<"usage: triangulation img1 img2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );//这部分完成特征点的提取与匹配
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    //-- 估计两张图像间运动,2d-2d函数可以求解R与t
    Mat R,t;
    pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );

    //-- 三角化
    vector<Point3d> points;//将三角测量之后的3d坐标存储在points中
    triangulation( keypoints_1, keypoints_2, matches, R, t, points );//完成特征点之间的三角测量获取深度
    
    //-- 验证三角化点与特征点的重投影关系
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );//内参K
    for ( int i=0; i<matches.size(); i++ )
    {
   
        Point2d pt1_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K );
        Point2d pt1_cam_3d(
            points[i].x/points[i].z, //三角化得到的3d坐标除以其深度信息来计算其归一化坐标
            points[i].y/points[i].z 
        );
        
        cout<<"point in the first camera frame: "<<pt1_cam<<endl;
        cout<<"point projected from 3D "<<pt1_cam_3d<<", d="<<points[i].z<<endl;
        
        // 第二个图
        Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K );
        Mat pt2_trans = R*( Mat_<double>(3,1) << points[i].x, points[i]

最低0.47元/天解锁文章