视觉SLAM ch13 设计SLAM系统
目录
一、SLAM系统
二、工程框架
三、框架流程
四、具体实现
五、VO整体流程
六、显示整体建图效果
一、SLAM系统
实现一个精简版的双目视觉里程计，前端使用光流法，局部使用局部BA优化。
二、工程框架
- app中 run_kitti_stereo.cpp是代码的运行入口程序
- bin存放编译好的可执行程序
- include存放头文件，include/myslam放自己写的头文件
- src存放源代码文件
- test放测试用文件
- config存放配置文件，相机参数以及数据集路径
- cmake_modules存放第三方库的cmake文件
三、框架流程
前端：插入图像帧，提取图像特征，然后与上一帧进行光流追踪，通过光流结果计算该帧的位姿。必要时补充新的特征点并作三角化。前端处理的结果作为后端的初始值。
后端：对前端处理后的数据（关键帧和路标点）进行优化，然后返回优化结果。后端应该控制优化问题的规模，不要让其对时间无限增长。
为了方便前端与后端之间的数据流动，加入地图类。前后端在分别的线程中处理数据，前端负责在地图中添加路标点，后端检测到地图更新以后优化路标点，并将旧的路标点删除掉，保持一定的规模。
需要添加辅助类：
- 相机类：管理内外参和投影函数
- 配置文件管理类：方便从配置文件中读取内容，配置文件中记录一些重要参数，方便我们调整。（数据集序列）
- Kitti数据集读取类：按照kitti的存储数据格式读取图像数据
- 可视化模块：观察系统状态
四、具体实现
1.实现基本数据结构
使用结构体struct实现帧、特征和路标点三个类。考虑到这些数据可能被多个线程访问，必须加上线程锁。
帧 Frame类
/**
* 帧
* 每一帧分配独立id，关键帧分配关键帧ID
*/
struct Frame {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
typedef std::shared_ptr Ptr;
unsigned long id_ = 0; // 帧id
unsigned long keyframe_id_ = 0; // 关键帧id
bool is_keyframe_ = false; // 是否为关键帧
double time_stamp_; // 时间戳，暂不使用
SE3 pose_; // Tcw 形式位姿，用李代数表示()
std::mutex pose_mutex_; // Pose数据锁
cv::Mat left_img_, right_img_; // stereo images
//装填左图特征点指针的容器
std::vector> features_left_;
//左图特征点在右图中的对应特征点，如果没有相应的特征，则设置为nullptr
std::vector> features_right_;
public: // data members
Frame() {}
Frame(long id, double time_stamp, const SE3 &pose, const Mat &left,
const Mat &right);
// 帧的位姿会被前端后端同时设置或者访问，为了避免竞争问题，在set和get函数中加锁
//取出帧的位姿，并保证线程安全
SE3 Pose() {
std::unique_lock lck(pose_mutex_);
return pose_;
}
//设置帧的位姿
void SetPose(const SE3 &pose) {
std::unique_lock lck(pose_mutex_);
pose_ = pose;
}
// 设置关键帧并分配并键帧id
void SetKeyFrame();
// 工厂构建模式，分配id
static std::shared_ptr CreateFrame();
}; Frame可以由静态函数构建，在静态函数中自动分配id。(frame.cpp中实现）
特征Feature
/**
* 2D 特征点
* 在三角化之后会被关联一个地图点
*/
struct Feature {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
typedef std::shared_ptr Ptr;
//weak_ptr不能直接定义智能指针对象，因此这里是用weak_ptr持有这个frame的shared_ptr智能指针的值
std::weak_ptr frame_; // 持有该feature的frame
cv::KeyPoint position_; // 2D提取位置
std::weak_ptr map_point_; // 关联地图点
bool is_outlier_ = false; // 是否为异常点
bool is_on_left_image_ = true; // 标识是否提在左图，false为右图
public:
Feature() {}
Feature(std::shared_ptr frame, const cv::KeyPoint &kp)
: frame_(frame), position_(kp) {}
}; 路标点MapPoint
/**
* 路标点类
* 特征点在三角化之后形成路标点
*/
struct MapPoint {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
typedef std::shared_ptr Ptr;
unsigned long id_ = 0; // 路标点ID
bool is_outlier_ = false; // 是否是异常点
Vec3 pos_ = Vec3::Zero(); //路标点世界坐标，初始值为0
std::mutex data_mutex_;
int observed_times_ = 0; //记录路标点被帧观测的次数
// 用链表存储 观测到这个路标点的所有特征点
std::list> observations_;
MapPoint() {}
MapPoint(long id, Vec3 position);
Vec3 Pos() {
std::unique_lock lck(data_mutex_);
return pos_;
}
void SetPos(const Vec3 &pos) {
std::unique_lock lck(data_mutex_);
pos_ = pos;
};
// 增加新的观测到这个路标点，并且特征点数量+1
void AddObservation(std::shared_ptr feature) {
std::unique_lock lck(data_mutex_);
observations_.push_back(feature);
observed_times_++;
}
//移除异常特征点
void RemoveObservation(std::shared_ptr feat);
/*
std::unique_lock lck(data_mutex_);
for (auto iter = observations_.begin(); iter != observations_.end();
iter++) {
if (iter->lock() == feat) {
observations_.erase(iter);
feat->map_point_.reset();
observed_times_--;
break;
}
}
*/
// 取出特征点存储的链表
std::list> GetObs() {
std::unique_lock lck(data_mutex_);
return observations_;
}
// 工厂模式构建路标点
static MapPoint::Ptr CreateNewMappoint();
/*
static long factory_id = 0;
MapPoint::Ptr new_mappoint(new MapPoint);
new_mappoint->id_ = factory_id++;
return new_mappoint;
*/
}; 在前后端之间定义地图类：Frame和MapPoint类的对象归地图持有。
/**
* @brief 地图
* 和地图的交互：前端调用InsertKeyframe和InsertMapPoint插入新帧和地图点，后端维护地图的结构，判定outlier/剔除等等
*/
class Map {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
typedef std::shared_ptr地图以hash的形式记录了所有的关键帧和对应的路标点，同时维护一个被激活的关键帧和地图点，激活的意思就是窗口（七个关键帧）。
2.前端
前端本身有初始化、正常追踪和追踪丢失三种状态。
初始化：左右目的照片进行光流法匹配，并三角化得到地图点，建立初始地图。StereoInit
追踪：计算前一帧的特征点到当前帧的光流，根据光流结果计算图像位姿（只使用左目图像）。Track()
如果追踪到的点很少（说明两帧差异较大），判定该帧为关键帧。InsertKeyframe
对于关键帧：
- 提取新特征点。
- 找到这些特征点在右图的对应点，使用三角化计算新的路标点。
- 将新的关键帧和路标点加入地图，触发一次后端优化。
追踪丢失：重置前端系统，重新初始化。Reset
前端Frontend:估计当前帧Pose，在满足关键帧条件时向地图加入关键帧并触发优化
AddFrame()增加新的一帧(这个函数触发前端)
//添加一个帧并计算其定位结果
bool Frontend::AddFrame(myslam::Frame::Ptr frame) {
current_frame_ = frame;
switch (status_) {
case FrontendStatus::INITING:
StereoInit();
break;
case FrontendStatus::TRACKING_GOOD:
case FrontendStatus::TRACKING_BAD:
Track();
break;
case FrontendStatus::LOST:
Reset();
break;
}
last_frame_ = current_frame_;
return true;
}status_开始被初始化为0，因此先执行StereoInit()
bool Frontend::StereoInit() {
int num_features_left = DetectFeatures(); //检测当前帧左目图像的特征点
int num_coor_features = FindFeaturesInRight(); //在当前帧的右目图像中找到与左目相应的特征
//找到的对应特征数目不满足初始化条件
if (num_coor_features < num_features_init_) {
return false;
}
//满足初始化条件 初始地图
bool build_map_success = BuildInitMap();
if (build_map_success) {
status_ = FrontendStatus::TRACKING_GOOD; //地图初始化成功，状态变为正常跟踪
if (viewer_) {
viewer_->AddCurrentFrame(current_frame_); //在可视化类中加入当前帧
viewer_->UpdateMap(); //更新地图
}
return true;
}
return false;
}①DetectFeatures()检测当前帧左目图像的特征点
int Frontend::DetectFeatures() {
/*
创建一个掩膜 mask，大小与当前帧的左图像相同，像素值为 255。
然后，对于当前帧已有的每个特征点，将其周围 20x20 的区域在掩膜中标记为 0，可以避免在已有的特征点周围检测到新的特征点。
*/
cv::Mat mask(current_frame_->left_img_.size(), CV_8UC1, 255);
for (auto &feat : current_frame_->features_left_) {
cv::rectangle(mask, feat->position_.pt - cv::Point2f(10, 10),
feat->position_.pt + cv::Point2f(10, 10), 0, CV_FILLED); //绘制矩形
}
/*
使用 GFTT 算法在掩膜内检测新的特征点，并将它们存储在 keypoints 中
*/
std::vector keypoints;
gftt_->detect(current_frame_->left_img_, keypoints, mask); //第三个参数是用来指定特征点选取区域的
/*
将每个新的特征点封装成 Feature 对象，并将其存储在当前帧的 featuresleft 中，同时计算新检测到的特征点的数量 cntdetected
*/
int cnt_detected = 0;
for (auto &kp : keypoints) {
current_frame_->features_left_.push_back(
Feature::Ptr(new Feature(current_frame_, kp))); //特征点放到容器中
cnt_detected++;
}
LOG(INFO) << "Detect " << cnt_detected << " new features";
return cnt_detected;
} ②FindFeaturesInRight()在当前帧的右目图像中找到与左目相应的特征点
int Frontend::FindFeaturesInRight() {
// 使用LK流来估计右侧图像中的点
std::vector kps_left, kps_right;
for (auto &kp : current_frame_->features_left_) {
kps_left.push_back(kp->position_.pt); //将特征点的位置放到kps_left中，LK光流函数要求角点是Point2f类型
auto mp = kp->map_point_.lock(); //检查是否上锁
if (mp) {
// 如果地图点存在，使用投影点作为初始估计值
auto px =
camera_right_->world2pixel(mp->pos_, current_frame_->Pose()); //使用世界坐标与位姿Tcw求右目的像素点
kps_right.push_back(cv::Point2f(px[0], px[1]));
} else {
//使用左侧相机特征点坐标为初始值
kps_right.push_back(kp->position_.pt);
}
}
std::vector status; //光流跟踪成功与否的状态向量，成功则为1,否则为0
Mat error;
/*
函数参数(prevImg,nextImg,prevPts，nextPts，status，err，winSize，maxLevel，criteria，flags)
- prevPts：前一帧图像中的特征点
-nextPts：后一帧图像中的特征点的初始猜测位置
- status：输出的状态向量，指示每个特征点是否成功跟踪
- err：输出的误差向量
- winSize：金字塔中每一层的窗口大小
- maxLevel：金字塔的最大层数
- criteria：迭代终止条件
- flags：计算光流的方式，OPTFLOW_USE_INITIAL_FLOW使用初始估计，存储在nextPts中;如果未设置标志，则将prevPts复制到nextPts并将其视为初始估计
*/
cv::calcOpticalFlowPyrLK(
current_frame_->left_img_, current_frame_->right_img_, kps_left,
kps_right, status, error, cv::Size(11, 11), 3,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30,
0.01),
cv::OPTFLOW_USE_INITIAL_FLOW);
int num_good_pts = 0;
for (size_t i = 0; i < status.size(); ++i) {
if (status[i]) { //为真表示光流追踪成功
cv::KeyPoint kp(kps_right[i], 7); //右目关键点的直径为7
Feature::Ptr feat(new Feature(current_frame_, kp));
feat->is_on_left_image_ = false;
current_frame_->features_right_.push_back(feat);
num_good_pts++;
} else {
current_frame_->features_right_.push_back(nullptr);
}
}
LOG(INFO) << "Find " << num_good_pts << " in the right image.";
return num_good_pts;
} ③BuildInitMap()初始化地图
bool Frontend::BuildInitMap() {
//pose描述了固定坐标系（世界坐标系）和某一帧间的位姿变化，
std::vector poses{camera_left_->pose(), camera_right_->pose()};
//初始化的路标数目
size_t cnt_init_landmarks = 0;
for (size_t i = 0; i < current_frame_->features_left_.size(); ++i) {
//右目没有对应点
if (current_frame_->features_right_[i] == nullptr) continue;
//对于左右目配对成功的点，使用三角化
//points存储了当前帧中左右目对应的特征点的归一化坐标(z=1)
std::vector points{
camera_left_->pixel2camera(
Vec2(current_frame_->features_left_[i]->position_.pt.x,
current_frame_->features_left_[i]->position_.pt.y)),
camera_right_->pixel2camera(
Vec2(current_frame_->features_right_[i]->position_.pt.x,
current_frame_->features_right_[i]->position_.pt.y))};
Vec3 pworld = Vec3::Zero();
if (triangulation(poses, points, pworld) && pworld[2] > 0) { //三角化成功并且深度值大于0
auto new_map_point = MapPoint::CreateNewMappoint(); //工厂模式创建一个新的地图点
new_map_point->SetPos(pworld);
//为这个地图点添加观测量(特征点)
new_map_point->AddObservation(current_frame_->features_left_[i]);
new_map_point->AddObservation(current_frame_->features_right_[i]);
//为特征类Feature对象填写地图点成员
current_frame_->features_left_[i]->map_point_ = new_map_point;
current_frame_->features_right_[i]->map_point_ = new_map_point;
cnt_init_landmarks++;
//对Map类对象来说，地图里面应当多了一个地图点，所以要将这个地图点加到地图中去
map_->InsertMapPoint(new_map_point);
}
}
//作为初始化帧，可看做开始的第一帧，所以应当是一个关键帧
current_frame_->SetKeyFrame();
//对Map类对象来说，地图里面应当多了一个关键帧，所以要将这个关键帧加到地图中去
map_->InsertKeyFrame(current_frame_);
// 关键帧插入，后端需要对新纳入的关键帧进行优化处理
backend_->UpdateMap();
LOG(INFO) << "Initial map created with " << cnt_init_landmarks
<< " map points";
return true;
} status_被修改为1，执行Track() （ status是TRACKING_GOOD或TRACKING_BAD都执行Track()）
/*
Track函数是上一帧和当前帧进行匹配计算位姿的，之前的初始化是某帧图像的左右目之间的匹配
*/
bool Frontend::Track() {
if (last_frame_) { //如果存在上一帧
//当前帧位姿的初始值 = 上一帧的位姿 左乘 当前帧与上一帧的相对变换
//Tc c_w = △Tlc * Tl c_w
current_frame_->SetPose(relative_motion_ * last_frame_->Pose());
}
//使用光流法得到前后两帧之间匹配特征点
int num_track_last = TrackLastFrame();
//计算当前帧的位姿，返回追踪成功点的数量（在光流匹配成功基础上，更信任的点）
tracking_inliers_ = EstimateCurrentPose();
//根据阈值判断本次跟踪的状态：好、坏或者丢失
if (tracking_inliers_ > num_features_tracking_) {
// tracking good
status_ = FrontendStatus::TRACKING_GOOD;
} else if (tracking_inliers_ > num_features_tracking_bad_) {
// tracking bad
status_ = FrontendStatus::TRACKING_BAD;
} else {
// lost
status_ = FrontendStatus::LOST;
}
//添加关键帧，对左目中没有地图点且右目中有匹配点的特征点进行三角化
InsertKeyframe();
//当前帧位姿 = 上一帧的位姿 左乘 当前帧与上一帧的相对变换
//相对位姿变换 = 当前帧的位姿 * 上一帧位姿的逆
relative_motion_ = current_frame_->Pose() * last_frame_->Pose().inverse();
if (viewer_) viewer_->AddCurrentFrame(current_frame_);
return true;
}①TrackLastFrame() 当前帧与上一帧左目图像的匹配点
//使用光流法得到前后两帧之间匹配特征点 并返回匹配数
int Frontend::TrackLastFrame() {
std::vector kps_last, kps_current;
//遍历上一帧的特征点
for (auto &kp : last_frame_->features_left_) {
//判断特征点是否有对应的地图点。地图点是某帧左右目通过三角化计算的，不是每个特征点都有地图点
if (kp->map_point_.lock()) {
auto mp = kp->map_point_.lock();
auto px =
camera_left_->world2pixel(mp->pos_, current_frame_->Pose()); //使用投影方式计算当前帧特征点对应的像素坐标
kps_last.push_back(kp->position_.pt);
kps_current.push_back(cv::Point2f(px[0], px[1]));
} else {
//如果特征点没有对应的地图点，当前帧的像素坐标初值和上一帧相同
kps_last.push_back(kp->position_.pt);
kps_current.push_back(kp->position_.pt);
}
}
/*
之前用LK光流是 在当前帧的右目图像中找到与左目相应的特征点 （FindFeaturesInRight函数中）
本次使用LK光流是 在当前帧的左目图像中找到与上一帧左目对应的特征点
*/
std::vector status;
Mat error;
cv::calcOpticalFlowPyrLK(
last_frame_->left_img_, current_frame_->left_img_, kps_last,
kps_current, status, error, cv::Size(11, 11), 3,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30,
0.01),
cv::OPTFLOW_USE_INITIAL_FLOW);
int num_good_pts = 0;
for (size_t i = 0; i < status.size(); ++i) {
if (status[i]) {
cv::KeyPoint kp(kps_current[i], 7);
Feature::Ptr feature(new Feature(current_frame_, kp)); //每个特征点构造一个特征对象
feature->map_point_ = last_frame_->features_left_[i]->map_point_; //当期帧的特征点和上一帧特征点对应的地图点相同
current_frame_->features_left_.push_back(feature); //当前帧的特征点放到对应的容器中
num_good_pts++; //前后两帧匹配成功点
}
}
LOG(INFO) << "Find " << num_good_pts << " in the last image.";
return num_good_pts;
} ②EstimateCurrentPose()计算当前帧的位姿，返回追踪成功点的数量
//计算当前帧的位姿，返回追踪成功点的数量
int Frontend::EstimateCurrentPose() {
/// pose 维度为 6, landmark 维度为 3
typedef g2o::BlockSolver_6_3 BlockSolverType;
typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique(
g2o::make_unique()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
// //往图中添加顶点
VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
vertex_pose->setId(0); // 设置顶点ID
vertex_pose->setEstimate(current_frame_->Pose()); // 设置优化初始值
optimizer.addVertex(vertex_pose); // 向稀疏优化器添加顶点
// K
Mat33 K = camera_left_->K();
// edges
int index = 1;
std::vector edges;
std::vector features;
for (size_t i = 0; i < current_frame_->features_left_.size(); ++i) {
//没有地图点不能进行重投影误差
auto mp = current_frame_->features_left_[i]->map_point_.lock();
if (mp) {
features.push_back(current_frame_->features_left_[i]);
EdgeProjectionPoseOnly *edge =
new EdgeProjectionPoseOnly(mp->pos_, K);
edge->setId(index);
edge->setVertex(0, vertex_pose); // 设置连接的顶点
edge->setMeasurement(toVec2(current_frame_->features_left_[i]->position_.pt)); // 设置观测数值
edge->setInformation(Eigen::Matrix2d::Identity()); // 设置信息矩阵：协方差矩阵之逆 该矩阵的维度和误差项有关，如果误差是三维的 信息矩阵的就是Eigen::Matrix3d::Identity()
edge->setRobustKernel(new g2o::RobustKernelHuber); //鲁棒核函数
edges.push_back(edge);
optimizer.addEdge(edge); // 向稀疏优化器添加边
index++;
}
}
// estimate the Pose the determine the outliers
const double chi2_th = 5.991;
int cnt_outlier = 0;
for (int iteration = 0; iteration < 4; ++iteration) {
vertex_pose->setEstimate(current_frame_->Pose());
optimizer.initializeOptimization(); // 设置优化初始值
optimizer.optimize(10); // 设置优化次数
cnt_outlier = 0;
// count the outliers
for (size_t i = 0; i < edges.size(); ++i) {
auto e = edges[i];
if (features[i]->is_outlier_) { //特征点本身是outliers，重新计算重投影误差
e->computeError();
}
if (e->chi2() > chi2_th) {
//设置等级 一般情况下g2o只处理level = 0的边，设置等级为1，下次循环g2o不再优化这个边
features[i]->is_outlier_ = true;
e->setLevel(1);
cnt_outlier++;
} else {
features[i]->is_outlier_ = false;
e->setLevel(0);
};// 为什么有分号？？？
// 在第三次迭代之后，它会将所有边的鲁棒核函数设置为nullptr，以便在后续的迭代中不再考虑outlier
if (iteration == 2) {
e->setRobustKernel(nullptr);
}
}
}
LOG(INFO) << "Outlier/Inlier in pose estimating: " << cnt_outlier << "/"
<< features.size() - cnt_outlier;
// Set pose and outlier
current_frame_->SetPose(vertex_pose->estimate()); //vertex_pose->estimate() 就是要求的优化变量
LOG(INFO) << "Current Pose = \n" << current_frame_->Pose().matrix();
/*
将所有被标记为outlier的特征点的map_point重置为nullptr，并将其标记为非outlier。
虽然这些特征点在当前帧的位姿估计中被排除了，但它们仍然可能在后续的帧中被重新观察到，并被用于位姿估计。
*/
for (auto &feat : features) {
if (feat->is_outlier_) {
feat->map_point_.reset();
feat->is_outlier_ = false; // maybe we can still use it in future
}
}
return features.size() - cnt_outlier;
}
③ InsertKeyframe()添加关键帧
//添加关键帧
bool Frontend::InsertKeyframe() {
//如果追踪到的点很少（说明两帧差异较大），判定该帧为关键帧
if (tracking_inliers_ >= num_features_needed_for_keyframe_) { //当前帧不能作为关键帧
return false;
}
current_frame_->SetKeyFrame(); //当前帧为关键帧并分配关键帧id
map_->InsertKeyFrame(current_frame_); //地图中插入当前帧
LOG(INFO) << "Set frame " << current_frame_->id_ << " as keyframe"
<< current_frame_->keyframe_id_;
///将关键帧中特征点对应的地图点加入到观测容器中
SetObservationsForKeyFrame();
//关键帧重新提取特征点
DetectFeatures();
// 在关键帧的右目图像中找与左目对应的特征点
FindFeaturesInRight();
// 三角化新的地图点
TriangulateNewPoints();
//更新地图,触发后端优化
backend_->UpdateMap();
//可视化模块中更新视图
if (viewer_) viewer_->UpdateMap();
return true;
}TriangulateNewPoints对左目中没有地图点且右目中有匹配点的特征点进行三角化
//关键帧中三角化新的地图点
int Frontend::TriangulateNewPoints() {
std::vector poses{camera_left_->pose(), camera_right_->pose()};
SE3 current_pose_Twc = current_frame_->Pose().inverse(); //当前帧的位姿是Tcw
int cnt_triangulated_pts = 0;
for (size_t i = 0; i < current_frame_->features_left_.size(); ++i) {
/*
expired()函数用于检查智能指针指向的对象是否为空 ,expired()==true 存储空指针
在左图找没有关联到路标的特征点，而且存在右匹配点
*/
if (current_frame_->features_left_[i]->map_point_.expired() &&
current_frame_->features_right_[i] != nullptr) {
// 左图的特征点未关联地图点且存在右图匹配点，尝试三角化
std::vector points{
camera_left_->pixel2camera(
Vec2(current_frame_->features_left_[i]->position_.pt.x,
current_frame_->features_left_[i]->position_.pt.y)),
camera_right_->pixel2camera(
Vec2(current_frame_->features_right_[i]->position_.pt.x,
current_frame_->features_right_[i]->position_.pt.y))};
Vec3 pworld = Vec3::Zero();
// 三角化成功并且深度为正
if (triangulation(poses, points, pworld) && pworld[2] > 0) { //三角化得到的坐标是左目相机系坐标
auto new_map_point = MapPoint::CreateNewMappoint();
/*
和初始化中的triangulation区分：
这里计算出的pworld是相机系坐标，左乘Twc才是世界系坐标
初始化是第一帧，一般以第一帧为世界系，因此得到的pworld直接当世界系坐标使用
*/
pworld = current_pose_Twc * pworld;
new_map_point->SetPos(pworld);
//为新地图点添加观测量(特征点)
new_map_point->AddObservation( current_frame_->features_left_[i]);
new_map_point->AddObservation(current_frame_->features_right_[i]);
//为特征类Feature对象填写地图点成员
current_frame_->features_left_[i]->map_point_ = new_map_point;
current_frame_->features_right_[i]->map_point_ = new_map_point;
//对Map类对象来说，地图里面应当多了一个地图点，所以要将这个地图点加到地图中去
map_->InsertMapPoint(new_map_point);
cnt_triangulated_pts++;
}
}
}
LOG(INFO) << "new landmarks(关键帧中新三角化的地图点数量为): " << cnt_triangulated_pts;
return cnt_triangulated_pts;
} status_是LOST，执行Reset() （该函数没有实现）
bool Frontend::Reset() {
LOG(INFO) << "Reset is not implemented. ";
return true;
}总结：到此前端代码结束，通过AddFrame()来对status_进行判断执行初始化、正常跟踪和重置。
在正常跟踪中实现了添加关键帧功能。
关于triangulation()三角化函数的实现以及推导
//points是归一化坐标，为什么A.block中把它当像素坐标呢？？？
inline bool triangulation(const std::vector &poses,
const std::vector points, Vec3 &pt_world) {
MatXX A(2 * poses.size(), 4);
VecX b(2 * poses.size());
b.setZero();
for (size_t i = 0; i < poses.size(); ++i) {
Mat34 m = poses[i].matrix3x4(); //转化为3*4的矩阵形式
A.block<1, 4>(2 * i, 0) = points[i][0] * m.row(2) - m.row(0);
A.block<1, 4>(2 * i + 1, 0) = points[i][1] * m.row(2) - m.row(1);
}
auto svd = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);// Eigen::ComputeThinU和Eigen::ComputeThinV是两个参数，用于指定只计算矩阵的前k个奇异向量，其中k是矩阵的秩。这样可以加快计算速度并减小内存占用
/*
取SVD分解得到v矩阵的最有一列作为解：svd.matrixV().col(3)
深度值是svd.matrixV()(3, 3)
head<3>() 是取前三个值
*/
pt_world = (svd.matrixV().col(3) / svd.matrixV()(3, 3)).head<3>();
if (svd.singularValues()[3] / svd.singularValues()[2] < 1e-2) { //计算了矩阵A的第四个奇异值与第三个奇异值的比值
// 解质量不好，放弃
return true;
}
return false;
} 三角化本质上是求最小二乘解的问题。ch12单目稠密中就是使用最小二乘法求解像素深度值，这里使用SVD奇异值分解的三角化，详细推导参考：
https://blog.csdn.net/qq_42995327/article/details/118917141
3后端：
①bached.h
/**
* 后端
* 有单独优化线程，在Map更新时启动优化
* Map更新由前端触发
*/
class Backend {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
typedef std::shared_ptr Ptr;
// 构造函数中启动优化线程并挂起
Backend();
// 设置左右目的相机，用于获得内外参
void SetCameras(Camera::Ptr left, Camera::Ptr right) {
cam_left_ = left;
cam_right_ = right;
}
/// 设置地图
void SetMap(std::shared_ptr ②backed.cpp
Backend::Backend() {
/*
store(true)是一个C++11原子操作，它将true存储到backend_running_中。
原子操作是一种线程安全的操作，可以保证在多线程环境下对变量的读写操作是原子的，即不会被其他线程中断。
*/
backend_running_.store(true);
//创建一个线程，线程执行回调函数BackendLoop，并将this绑定到函数，即这是this指向的类的成员函数
backend_thread_ = std::thread(std::bind(&Backend::BackendLoop, this));
}
void Backend::UpdateMap() {
std::unique_lock lock(data_mutex_);
map_update_.notify_one();
}
void Backend::Stop() {
backend_running_.store(false);
map_update_.notify_one();
backend_thread_.join();
}
void Backend::BackendLoop() {
///load()函数返回当前存储在backend_running_中的值，以确保后端线程只在backend_running_为true时运行
//这是一个死循环，等待前段激活。每激活一次，运行此函数进行一次后端优化
while (backend_running_.load()) {
std::unique_lock lock(data_mutex_);
map_update_.wait(lock); //将后端构造函数挂起(阻塞)，解锁互斥量， 直到其他线程调用notify_one ，然后将互斥量锁住继续执行
/// 后端仅优化激活的Frames和Landmarks
Map::KeyframesType active_kfs = map_->GetActiveKeyFrames();
Map::LandmarksType active_landmarks = map_->GetActiveMapPoints();
Optimize(active_kfs, active_landmarks);
}
}
/*
typedef std::unordered_map KeyframesType
typedef std::unordered_map LandmarksType
*/
void Backend::Optimize(Map::KeyframesType &keyframes, // keyframes是map类中的哈希表
Map::LandmarksType &landmarks) {
// setup g2o
typedef g2o::BlockSolver_6_3 BlockSolverType;
typedef g2o::LinearSolverCSparse
LinearSolverType;
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique(
g2o::make_unique()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
/*
定义了一个名为vertices的std::map，它的键是unsigned long类型，值是指向位姿VertexPose对象的指针。
这个std::map用于存储关键帧的位姿顶点。
在后端优化过程中，只有激活的关键帧和路标会被优化，因此这个std::map只存储激活的关键帧的位姿顶点。
*/
std::map vertices;
unsigned long max_kf_id = 0;
//遍历关键帧 确定第一个顶点
for (auto &keyframe : keyframes) {
auto kf = keyframe.second;
VertexPose *vertex_pose = new VertexPose(); // 为每一个关键帧创建一个相机位姿顶点
vertex_pose->setId(kf->keyframe_id_);
vertex_pose->setEstimate(kf->Pose()); //Tcw
optimizer.addVertex(vertex_pose);
if (kf->keyframe_id_ > max_kf_id) {
max_kf_id = kf->keyframe_id_;
}
vertices.insert({kf->keyframe_id_, vertex_pose});
}
// 路标顶点，使用路标id索引
std::map vertices_landmarks;
// 相机内参K 和初始化后第一帧相机的左右目外参
Mat33 K = cam_left_->K();
SE3 left_ext = cam_left_->pose();
SE3 right_ext = cam_right_->pose();
// 边
int index = 1;
double chi2_th = 5.991; // robust kernel 阈值
//EdgeProjection是带有位姿顶点和路标顶点的二元边
std::map edges_and_features;
//遍历路标点
for (auto &landmark : landmarks) {
//外点不优化
if (landmark.second->is_outlier_) continue;
//路标点id
unsigned long landmark_id = landmark.second->id_;
//得到所有观测到这个路标点的feature
auto observations = landmark.second->GetObs();
//遍历所有观测到这个路标点的feature，得到第二个顶点，形成对应的点边关系
for (auto &obs : observations) {
if (obs.lock() == nullptr) continue;
auto feat = obs.lock();
if (feat->is_outlier_ || feat->frame_.lock() == nullptr) continue;
//得到该feature所在的frame
auto frame = feat->frame_.lock();
EdgeProjection *edge = nullptr;
//判断这个feature在哪个相机
if (feat->is_on_left_image_) {
edge = new EdgeProjection(K, left_ext);
} else {
edge = new EdgeProjection(K, right_ext);
}
// 如果landmark还没有被加入优化，则新加一个顶点。无论mappoint被观测到几次，只与其中一个形成关系
if (vertices_landmarks.find(landmark_id) ==
vertices_landmarks.end()) {
VertexXYZ *v = new VertexXYZ;
v->setEstimate(landmark.second->Pos());
v->setId(landmark_id + max_kf_id + 1);
v->setMarginalized(true); //边缘化
vertices_landmarks.insert({landmark_id, v});
//添加顶点
optimizer.addVertex(v);
}
edge->setId(index);
edge->setVertex(0, vertices.at(frame->keyframe_id_)); // pose
edge->setVertex(1, vertices_landmarks.at(landmark_id)); // landmark
edge->setMeasurement(toVec2(feat->position_.pt));
edge->setInformation(Mat22::Identity());
auto rk = new g2o::RobustKernelHuber(); //定义鲁棒核函数robust kernel
rk->setDelta(chi2_th); //设置阈值
edge->setRobustKernel(rk);
edges_and_features.insert({edge, feat});
//增加边
optimizer.addEdge(edge);
index++;
}
}
// do optimization and eliminate the outliers
optimizer.initializeOptimization();
optimizer.optimize(10);
int cnt_outlier = 0, cnt_inlier = 0;
int iteration = 0;
//确保内点占1/2以上，否则调整阈值，直到迭代结束
while (iteration < 5) {
cnt_outlier = 0;
cnt_inlier = 0;
// determine if we want to adjust the outlier threshold
for (auto &ef : edges_and_features) {
if (ef.first->chi2() > chi2_th) {
cnt_outlier++;
} else {
cnt_inlier++;
}
}
double inlier_ratio = cnt_inlier / double(cnt_inlier + cnt_outlier);
if (inlier_ratio > 0.5) {
break;
} else {
chi2_th *= 2;
iteration++;
}
}
//根据新的阈值，调整哪些是外点 ，并移除
for (auto &ef : edges_and_features) {
if (ef.first->chi2() > chi2_th) {
ef.second->is_outlier_ = true;
// remove the observation
ef.second->map_point_.lock()->RemoveObservation(ef.second);
} else {
ef.second->is_outlier_ = false;
}
}
LOG(INFO) << "Outlier/Inlier in optimization: " << cnt_outlier << "/"
<< cnt_inlier;
// Set pose and lanrmark position
//保存优化后的位姿和地图点
for (auto &v : vertices) {
keyframes.at(v.first)->SetPose(v.second->estimate());
}
for (auto &v : vertices_landmarks) {
landmarks.at(v.first)->SetPos(v.second->estimate());
}
}
五、VO整体流程
从run_kitti_stereo.cpp开始：
//参考gflags库使用: https://blog.csdn.net/wcy23580/article/details/89222962
//定义了一个名为config_file的字符串变量，其初始值为"./config/default.yaml"，表示配置文件的路径。第三个参数是一个字符串，用于描述这个变量的作用。
//这个变量是使用gflags库中的DEFINE_string宏定义的，它允许我们在命令行中指定配置文件的路径。在程序中，我们可以使用FLAGS_config_file来访问这个变量的值。
DEFINE_string(config_file, "./config/default.yaml", "config file path");
int main(int argc, char **argv) {
google::ParseCommandLineFlags(&argc, &argv, true);
myslam::VisualOdometry::Ptr vo(
new myslam::VisualOdometry(FLAGS_config_file));
assert(vo->Init() == true);
vo->Run();
return 0;
}
首先创建VisualOdometry实例对象，然后执行断言函数判断初始化函数Init()是否成功
bool VisualOdometry::Init() {
if (Config::SetParameterFile(config_file_path_) == false) {
return false;
}
dataset_ =
Dataset::Ptr(new Dataset(Config::Get("dataset_dir")));
//类似于assert函数
CHECK_EQ(dataset_->Init(), true);
// 创建各个模块的实例
frontend_ = Frontend::Ptr(new Frontend);
backend_ = Backend::Ptr(new Backend);
map_ = Map::Ptr(new Map);
viewer_ = Viewer::Ptr(new Viewer);
frontend_->SetBackend(backend_);
frontend_->SetMap(map_);
frontend_->SetViewer(viewer_);
frontend_->SetCameras(dataset_->GetCamera(0), dataset_->GetCamera(1));
backend_->SetMap(map_);
backend_->SetCameras(dataset_->GetCamera(0), dataset_->GetCamera(1));
viewer_->SetMap(map_);
return true;
} 初始化成功后执行Run()
void VisualOdometry::Run() {
while (1) {
LOG(INFO) << "VO is running(VO 开始运行)";
if (Step() == false) {
break;
}
}
//步进到下一帧失败,停止后端并关闭可视化窗口
backend_->Stop();
viewer_->Close();
LOG(INFO) << "VO exit";
}其中重要的是Step()函数，函数中调用AddFrame()函数执行前端代码
bool VisualOdometry::Step() {
Frame::Ptr new_frame = dataset_->NextFrame();
if (new_frame == nullptr) return false;
auto t1 = std::chrono::steady_clock::now();
bool success = frontend_->AddFrame(new_frame); //执行前端代码
auto t2 = std::chrono::steady_clock::now();
auto time_used =
std::chrono::duration_cast>(t2 - t1);
LOG(INFO) << "VO cost time(用时): " << time_used.count() << " seconds.";
return success;
} 整体程序思维导图
六、显示整体建图效果
// 注意是viewer.cpp中的函数
void Viewer::UpdateMap() {
std::unique_lock lck(viewer_data_mutex_);
assert(map_ != nullptr);
active_keyframes_ = map_->GetActiveKeyFrames();
// active_landmarks_ = map_->GetActiveMapPoints(); //当前观测到的地图点
active_landmarks_ = map_->GetAllMapPoints(); // 改为all mappoints，显示整体地图
map_updated_ = true;
}
七、c++
1.多线程中有着线程同步：线程间需要按照预定的先后次序顺序进行的行为。condition_variable https://zhuanlan.zhihu.com/p/224054283 这篇文章中的例子可以更好的理解它
2.bind函数用法:
回调函数（一种特殊的函数，它作为参数传递给另一个函数，并在被调用函数执行完毕后被调用）如果是类的成员函数，这时想把成员函数设置给一个回调函数指针往往是不行的。因为类的成员函数多了一个隐含的参数this，所以直接赋值给函数指针肯定会引起编译报错。这个时候需要使用bind，详细用法参考https://www.cnblogs.com/jialin0x7c9/p/12219239.html
参考文章：
重读《视觉SLAM十四讲》ch13实践设计SLAM系统 - 知乎 (zhihu.com)
slam14讲之设计slam系统（第二版ch13）代码注释二（后端及VO接口）_MIKingZCC的博客-CSDN博客