SLAM十四讲-前端设计

最新推荐文章于 2025-05-16 16:43:23 发布

Maccy37

最新推荐文章于 2025-05-16 16:43:23 发布

阅读量368

点赞数

CC 4.0 BY-SA版权

分类专栏： SLAM 文章标签： slam 十四讲

本文链接：https://blog.youkuaiyun.com/qq_37611824/article/details/90707067

SLAM 专栏收录该内容

6 篇文章

订阅专栏

博客围绕视觉SLAM中两两帧之间的VO展开，以参考帧为坐标系，将当前帧与参考帧进行特征匹配来估计运动。待估计运动和两帧变换矩阵构成左乘关系，其中T_c_r_estimated_通过cv::solvePnPRansac估计得出，参考了《视觉SLAM十四讲从理论到实践》。

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

两两帧之间的VO:书中主要进行参考帧(Reference)和当前帧(Current)之间的变换,参考帧为坐标系,当前帧与参考帧进行特征匹配,估计运动.参考帧相对世界坐标系变换矩阵,当前帧与世界坐标系变换矩阵为,待估计运动和两帧变换矩阵构成左乘关系:

T_c_r_estimated_是通过cv::solvePnPRansac估计的来

void VisualOdometry::poseEstimationPnP()
{
    // construct the 3d 2d observations
    vector<cv::Point3f> pts3d;
    vector<cv::Point2f> pts2d;
    
    for ( cv::DMatch m:feature_matches_ )
    {
        pts3d.push_back( pts_3d_ref_[m.queryIdx] );
        pts2d.push_back( keypoints_curr_[m.trainIdx].pt );
    }
    
    Mat K = ( cv::Mat_<double>(3,3)<<
        ref_->camera_->fx_, 0, ref_->camera_->cx_,
        0, ref_->camera_->fy_, ref_->camera_->cy_,
        0,0,1
    );
    Mat rvec, tvec, inliers;
    cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );  ///pnp求解法求解 旋转矩阵和平移

    cout<<"pnp inliers: "<<num_inliers_<<endl;
    T_c_r_estimated_ = SE3(
        SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)), 
        Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
    );  ///从R,t构造SE(3);从旋转向量构造SO(3)
}

参考:<<视觉SLAM十四讲从理论到实践>>