手搓视觉SLAM定位算法：第三章

最新推荐文章于 2025-04-19 16:33:14 发布

zengy5

最新推荐文章于 2025-04-19 16:33:14 发布

阅读量1k

点赞数 12

分类专栏：从0开始的简易视觉定位算法文章标签：算法 c++ 人工智能自动驾驶

本文链接：https://blog.youkuaiyun.com/weixin_48013375/article/details/144175227

版权

从0开始的简易视觉定位算法专栏收录该内容

3 篇文章

订阅专栏

手搓视觉SLAM定位算法：第三章

默认读者对视觉slam和视觉定位有一定的基础
没有的话可以参考《视觉十四讲》或者作者的另外一个专栏
有一点基础就行

提示：这一章开始作者也开始晕了，所以说是教程，更应该说是一个学习记录和谈论平台

前言

要讲BA，就必须讲地图点了

PS：前排提示，这里的localBA还不涉及滑窗，只是根据当前帧的观测多拿了几帧

但是流程就是这么个流程，能走通能明白了，再加滑窗

一、理论基础

要做局部BA联合几帧去优化的话，就需要一个地图点管理模块

每个地图点应该包括：3D坐标、哪些帧可以看到它、它在这些帧里的投影坐标

那么管理模块就可以基于一个unordered_map去管理，给每个地图点加一个id，用id去搜索

所以整个流程就是：

新来的图像做LK光流法追踪，知道哪些2D点有对应的地图点，哪些没有
没有对应地图点的2D点进行三角化，作为新的地图点加入地图
用这一帧能够观测到的地图点去做BA
遍历这些地图点能够被观测到的帧，以及它们的位姿，加入BA问题求解

二、代码实现

2.1 地图点

每个点包含自身的坐标，能够看到它的帧的序号，以及在这些帧里面它的像素坐标

struct MapPoint
{
    double point[3];
    std::vector<int> framesAbleToObserve;
    std::vector<Eigen::Vector2d> projectionPixels;
};
std::unordered_map<int, MonoLocal::MapPoint> globalMap;

2.2 第一帧的处理

第一帧只提取角点，初始化一下位姿即可

        if(!init_flag)
        {
            pre_img = cv::imread(img_queue[0], cv::IMREAD_UNCHANGED);
            cv::goodFeaturesToTrack(pre_img, points_pre, track_num, 0.01, 10);
            for(size_t i = 0; i < points_pre.size(); i ++)
            {
                pointIDs.push_back(freature_id);
                freature_id ++;
            }
            init_flag = true;
            frame_id ++;
            double state[7] = {1,0,0,0,0,0,0};
            double* state_copy = new double[7];
            std::memcpy(state_copy, state, sizeof(state));  
            camera_states.push_back(state_copy);
            continue;
        }

2.3 第二帧以及以后的处理

先LK光流跟踪，获得跟踪上的点对

再根据status来筛去没跟踪上的，并且获取这些特征点的id

用id去搜地图点，搜到了的话就给现有的地图点进行更新

没搜到的话就进行三角化，创建新的地图点加入地图

同时用curMapPoints来记录这一帧能够看到的地图点

        cv::calcOpticalFlowPyrLK(
            pre_img, cur_img, points_pre, points_cur, status, err,
            cv::Size(21, 21),
            3
        );

        cv::cvtColor(cur_img, result, cv::COLOR_GRAY2BGR);
        std::vector<cv::Point2f> pre_pts;
        std::vector<cv::Point2f> cur_pts;
        std::vector<int>    curPointIDs;
        for (size_t i = 0; i < points_pre.size(); i++) {
            if (status[i]) { 
                cv::circle(result, points_cur[i], 3, cv::Scalar(0, 255, 0), -1);   
                cv::line(result, points_pre[i], points_cur[i], cv::Scalar(255, 0, 0), 2); 
                pre_pts.push_back(points_pre[i]);
                cur_pts.push_back(points_cur[i]);
                curPointIDs.push_back(pointIDs[i]);
            } 
        }
        pointIDs = curPointIDs;
        points_pre = pre_pts;
        points_cur = cur_pts;
        std::vector<cv::Point2f> to_tri_pre_pts;
        std::vector<cv::Point2f> to_tri_cur_pts;
        std::vector<MonoLocal::MapPoint> curMapPoints;
        for(size_t i = 0; i < pointIDs.size(); i ++)
        {
            if(globalMap.find(pointIDs[i]) == globalMap.end())
            {
                to_tri_pre_pts.push_back(points_pre[i]);
                to_tri_cur_pts.push_back(points_cur[i]);
            }
            else
            {
                globalMap[pointIDs[i]].framesAbleToObserve.push_back(frame_id);
                globalMap[pointIDs[i]].projectionPixels.push_back(Eigen::Vector2d(cur_pts[i].x,cur_pts[i].y));
                curMapPoints.push_back(globalMap[pointIDs[i]]);
            } 
        }

2.4 新增地图点的处理

这里需要注意的是，不同于两帧之间的BA，这里已经是有个globalMap了，所以任何一个地图点的3D坐标都应该是地图坐标系下的

那么在三角化这里就需要额外的处理

因为三角化后的点的坐标是基于前一帧的坐标系，所以我们需要用前一帧的坐标进行变换

在机器人坐标系中，如果我们记坐标系A到坐标系B的变换为 $T_A^B$

那么在定位过程中，第k帧的位姿就是 $T_K^0$ ，将点从第k帧坐标系变换到地图坐标系中

这里就正常变换一下就可以了

            cv::triangulatePoints(P1, P2, pre_pts_norm, cur_pts_norm, points_4d);

            std::vector<cv::Point3f> points_3d;
            for (int i = 0; i < points_4d.cols; i++) {
                cv::Mat point = points_4d.col(i);
                point /= point.at<float>(3); 
                points_3d.emplace_back(point.at<float>(0), point.at<float>(1), point.at<float>(2));
                
                auto transform = camera_states[frame_id-1];
                Eigen::Quaterniond rotationq(transform[0], transform[1], transform[2], transform[3]); 
                Eigen::Vector3d translation(transform[4], transform[5], transform[6]);    
                Eigen::Vector3d local_point(point.at<float>(0), point.at<float>(1), point.at<float>(2)); 
                Eigen::Vector3d global_point = rotationq * local_point + translation;

                MonoLocal::MapPoint newPoint;
                newPoint.point[0] = global_point[0];
                newPoint.point[1] = global_point[1];
                newPoint.point[2] = global_point[2];
                newPoint.framesAbleToObserve.push_back(frame_id);
                newPoint.projectionPixels.push_back(Eigen::Vector2d(cur_pts_norm[i].x, cur_pts_norm[i].y));
                globalMap.insert({freature_id, newPoint});
                curMapPoints.push_back(newPoint);
                freature_id ++;
            }

2.5 局部BA

经过这些处理后，遍历curMapPoints，依次添加残差块即可

for(auto & mapPoint : curMapPoints)
{
    for(size_t i = 0; i < mapPoint.framesAbleToObserve.size(); i ++)
    {
        ceres::CostFunction* cost_function = ReprojectionError::Create(
            mapPoint.projectionPixels[i][0],mapPoint.projectionPixels[i][1], K);
        problem.AddResidualBlock(cost_function, nullptr, camera_states[mapPoint.framesAbleToObserve[i]], 
                mapPoint.point);
    }
}