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

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

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

文章目录

手搓视觉SLAM定位算法：第三章
前言
一、理论基础
二、代码实现
- 2.1 地图点
- 2.2 第一帧的处理
- 2.3 第二帧以及以后的处理
- 2.4 新增地图点的处理
- 2.5 局部BA
总结

前言

要讲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);}
}