SLAM十四讲第二版项目代码总结
github地址：https://github.com/gaoxiang12/slambook2/tree/master/ch13
双目视觉里程计
头文件
所有的类都在myslam命名空间中
1.common_include.h
定义常用的头文件、EIgen矩阵格式
2.algorithm.h
common_include.h
三角化，已知位姿和归一化平面的点，和书上有点区别，使用归一化平面的点比较方便
/**
* linear triangulation with SVD
* @param poses poses, （已知）
* @param points points in normalized plane （已知）
* @param pt_world triangulated point in the world （输出）
* @return true if success
*/
inline bool triangulation(const std::vector<SE3> &poses,const std::vector<Vec3> points, Vec3 &pt_world)
将opencv的点格式变为eigen2*1矩阵形式
inline Vec2 toVec2(const cv::Point2f p)
3.Config.h
common_include.h
class Config
/**
* 配置类，使用SetParameterFile确定配置文件
* 然后用Get得到对应值
* 单例模式
*/
static std::shared_ptr<Config> config_;
cv::FileStorage file_;
//设定参数文件名 参数文件读给file_
static bool SetParameterFile(const std::string &filename);
static T Get(const std::string &key) // 根据参数名key在file_中找到该参数的值
4.dataset.h
common_include.h frame.h camera.h
class Dataset
typedef std::shared_ptr<Dataset> Ptr;
Dataset(const std::string& dataset_path);
bool Init(); //初始化
Frame::Ptr NextFrame(); //创建并返回下一帧
Camera::Ptr GetCamera(int camera_id) //根据id返回相机参数
std::string dataset_path_; //数据集路径
int current_image_index_ = 0; //当前图像id
std::vector<Camera::Ptr> cameras_; //每一帧对应的相机参数（应该都是一样的）
5.camera.h
common_include.h
class Camera
double fx_ = 0, fy_ = 0, cx_ = 0, cy_ = 0,baseline_ = 0; // Camera intrinsics
SE3 pose_; // 双目到单目位姿变换
SE3 pose_inv_; // pose_的逆
Camera(double fx, double fy, double cx, double cy, double baseline,const SE3 &pose)
: fx_(fx), fy_(fy), cx_(cx), cy_(cy), baseline_(baseline), pose_(pose) {
pose_inv_ = pose_.inverse();
}
SE3 pose() //返回位姿
Mat33 K() //返回3*3的内参矩阵
Vec3 world2camera(const Vec3 &p_w, const SE3 &T_c_w); //世界到相机
Vec3 camera2world(const Vec3 &p_c, const SE3 &T_c_w); //相机到世界
Vec2 camera2pixel(const Vec3 &p_c); //相机到像素
Vec3 pixel2camera(const Vec2 &p_p, double depth = 1); //像素到相机（注意深度）
Vec3 pixel2world(const Vec2 &p_p, const SE3 &T_c_w, double depth = 1); //像素到世界
Vec2 world2pixel(const Vec3 &p_w, const SE3 &T_c_w); //世界到像素
6.feature.h
memory features2d.hpp common_include.h
struct Frame;
struct MapPoint;
struct Feature （没用类） 2D 特征点 在三角化之后会被关联一个地图点
typedef std::shared_ptr<Feature> Ptr;
std::weak_ptr<Frame> frame_; // 持有该feature的frame
cv::KeyPoint position_; // 2D提取位置
std::weak_ptr<MapPoint> map_point_; // 关联地图点
bool is_outlier_ = false; // 是否为异常点
bool is_on_left_image_ = true; // 标识是否提在左图，false为右图
Feature(std::shared_ptr<Frame> frame, const cv::KeyPoint &kp): frame_(frame), position_(kp) {} //构造
7.frame.h
camera.h common_include.h
struct MapPoint;
struct Feature;
struct Frame 每一帧分配独立id，关键帧分配关键帧ID
typedef std::shared_ptr<Frame> Ptr;
unsigned long id_ = 0; // id of this frame
unsigned long keyframe_id_ = 0; // id of key frame
bool is_keyframe_ = false; // 是否为关键帧
double time_stamp_; // 时间戳，暂不使用
SE3 pose_; // Tcw 形式Pose
std::mutex pose_mutex_; // Pose数据锁
cv::Mat left_img_, right_img_; // stereo images
std::vector<std::shared_ptr<Feature>> features_left_; //左图像中提取的特征点
std::vector<std::shared_ptr<Feature>> features_right_; //左图像在右图像中
Frame(long id, double time_stamp, const SE3 &pose, const Mat &left,const Mat &right);
SE3 Pose(); //返回位姿，加锁
void SetPose(const SE3 &pose) //设定位姿 加锁
void SetKeyFrame(); // 设置关键帧并分配并键帧id
static std::shared_ptr<Frame> CreateFrame(); // 工厂构建模式，分配id
8.mappoint.h
common_include.h
typedef std::shared_ptr<MapPoint> Ptr;
unsigned long id_ = 0; // ID
bool is_outlier_ = false; //是否为外点
Vec3 pos_ = Vec3::Zero(); // 世界坐标
std::mutex data_mutex_; //数据锁
int observed_times_ = 0; // 特征匹配时被观测到的次数
std::list<std::weak_ptr<Feature>> observations_; //观测到该地图点的特征
MapPoint(long id, Vec3 position);
Vec3 Pos() //返回世界坐标系中位置 （加锁）
void SetPos(const Vec3 &pos) //设定地图点世界坐标 （加锁）
void AddObservation(std::shared_ptr<Feature> feature) //添加观测到该地图点的特征 （加锁）
void RemoveObservation(std::shared_ptr<Feature> feat);
std::list<std::weak_ptr<Feature>> GetObs() //获取观测到该地图点的所有特征
static MapPoint::Ptr CreateNewMappoint(); //工厂模式 创建地图点
9.map.h
common_include.h frame.h mappoint.h
class Map
typedef std::shared_ptr<Map> Ptr;
typedef std::unordered_map<unsigned long, MapPoint::Ptr> LandmarksType; //地图点
typedef std::unordered_map<unsigned long, Frame::Ptr> KeyframesType; //关键帧
void InsertKeyFrame(Frame::Ptr frame); // 增加一个关键帧
void InsertMapPoint(MapPoint::Ptr map_point); // 增加一个地图顶点
LandmarksType GetAllMapPoints() // 获取所有地图点 (加锁)
KeyframesType GetAllKeyFrames() // 获取所有关键帧 (加锁)
LandmarksType GetActiveMapPoints() // 获取激活地图点
KeyframesType GetActiveKeyFrames() // 获取激活关键帧
void CleanMap(); // 清理map中观测数量为零的点
private：
void RemoveOldKeyframe(); // 将旧的关键帧置为不活跃状态
std::mutex data_mutex_; //数据锁
LandmarksType landmarks_; // all landmarks
LandmarksType active_landmarks_; // active landmarks
KeyframesType keyframes_; // all key-frames
KeyframesType active_keyframes_; // all key-frames
Frame::Ptr current_frame_ = nullptr; //当前帧
int num_active_keyframes_ = 7; // 激活的关键帧数量
10.frontend.h
map.h frame.h common_include.h
typedef std::shared_ptr<Frontend> Ptr;
bool AddFrame(Frame::Ptr frame); // 外部接口，添加一个帧并计算其定位结果
void SetMap(Map::Ptr map) { map_ = map; } //设定地图
void SetBackend(std::shared_ptr<Backend> backend) { backend_ = backend; } //设定后端
void SetViewer(std::shared_ptr<Viewer> viewer) { viewer_ = viewer; } //可视化
FrontendStatus GetStatus() const { return status_; } //返回当前状态 初始化 丢失 正常追踪
void SetCameras(Camera::Ptr left, Camera::Ptr right) //设定左右相机
bool Track(); //追踪 成功返回true
bool Reset(); //追踪丢失之后重置 成功返回seccess
int TrackLastFrame(); // 追踪上一帧 返回追踪到的点
int EstimateCurrentPose(); //估计当前帧的位姿，返回内点个数
bool InsertKeyframe(); //将当前帧设为关键帧 并插入后端
bool StereoInit(); //双目初始化
int DetectFeatures(); //检测当前帧左图像中的特征，返回数量 keypoint保存在当前帧
int FindFeaturesInRight(); //找到对应在右图像中的特征 返回找到对应的特征数量
bool BuildInitMap(); //用单张图像构建初始化特征
int TriangulateNewPoints(); //三角化当前帧的2d点 返回三角化的点
void SetObservationsForKeyFrame(); //将关键帧中的地图点设为新的地图点
FrontendStatus status_ = FrontendStatus::INITING; //前段状态 初始化 丢失 正常追踪
Frame::Ptr current_frame_ = nullptr; // 当前帧
Frame::Ptr last_frame_ = nullptr; // 上一帧
Camera::Ptr camera_left_ = nullptr; // 左侧相机
Camera::Ptr camera_right_ = nullptr; // 右侧相机
Map::Ptr map_ = nullptr; //地图
std::shared_ptr<Backend> backend_ = nullptr; //后端
std::shared_ptr<Viewer> viewer_ = nullptr; //初始化
SE3 relative_motion_; // 当前帧与上一帧的相对运动，用于估计当前帧pose初值
int tracking_inliers_ = 0; //内点数，用于测试是否加入新的关键帧
int num_features_ = 200;
int num_features_init_ = 100;
int num_features_tracking_ = 50;
int num_features_tracking_bad_ = 20;
int num_features_needed_for_keyframe_ = 80;
cv::Ptr<cv::GFTTDetector> gftt_; //opencv特征点检测
11.backend.h
common_include.h frame.h map.h
typedef std::shared_ptr<Backend> Ptr;
Backend(); // 构造函数中启动优化线程并挂起
void SetCameras(Camera::Ptr left, Camera::Ptr right) // 设置左右目的相机，用于获得内外参
void SetMap(std::shared_ptr<Map> map) // 设置地图
void UpdateMap(); // 触发地图更新，启动优化 （唤醒线程）
void Stop(); // 关闭后端线程
private:
void BackendLoop(); // 后端线程
// 对给定关键帧和路标点进行优化
void Optimize(Map::KeyframesType& keyframes, Map::LandmarksType& landmarks);
std::shared_ptr<Map> map_; //地图
std::thread backend_thread_;
std::mutex data_mutex_; //数据锁
std::condition_variable map_update_;
std::atomic<bool> backend_running_;
Camera::Ptr cam_left_ = nullptr, cam_right_ = nullptr; //相机
12.visual_odometry.h
backend.h commom_include.h frontend.h viewer.h
class VisualOdometry
typedef std::shared_ptr<VisualOdometry> Ptr;
VisualOdometry(std::string &config_path); //参数文件用于构造
bool Init(); //初始化
void Run(); //跑数据集VO
bool Step(); //啥啥啥
FrontendStatus GetFrontendStatus() //获取前端状态
private:
bool inited_ = false; //是否初始化
std::string config_file_path_; //参数文件路径
Frontend::Ptr frontend_ = nullptr;
Backend::Ptr backend_ = nullptr;
Map::Ptr map_ = nullptr;
Viewer::Ptr viewer_ = nullptr;
Dataset::Ptr dataset_ = nullptr; //数据集
13.viewer.h
thread pangolin.h common_include.h frame.h map.h
typedef std::shared_ptr<Viewer> Ptr;
Viewer();
void SetMap(Map::Ptr map)
void Close();
void AddCurrentFrame(Frame::Ptr current_frame); // 增加一个当前帧
void UpdateMap(); // 更新地图
private:
void ThreadLoop();
void DrawFrame(Frame::Ptr frame, const float* color); //绘制帧
void DrawMapPoints(); //绘制地图点
void FollowCurrentFrame(pangolin::OpenGlRenderState& vis_camera); //跟踪当前帧
cv::Mat PlotFrameImage(); //绘制当前帧的特征点
Frame::Ptr current_frame_ = nullptr;
Map::Ptr map_ = nullptr;
std::thread viewer_thread_;
bool viewer_running_ = true;
std::unordered_map<unsigned long, Frame::Ptr> active_keyframes_;
std::unordered_map<unsigned long, MapPoint::Ptr> active_landmarks_;
bool map_updated_ = false;
std::mutex viewer_data_mutex_;
代码流程
初始化
run_kitti_stereo.cpp 初始化一个VisualOdmetry类（记为VO），传入参数文件，并调用VO类中的初始化函数Init()（记为voInit），voInit函数建立一个Dataset类，并调用Dataset类中的初始化函数Init()（记为dataInit）,dataInit读取相机参数和标定的参数。随后voInit创建前端、后端、map、viewer类（生成对应的智能指针，调用一些set函数）。初始化完成后，run_kitti_stereo.cpp调用VO类中的启动函数run()（记为vorun）。vorun循环调用VO类中的step()函数（除非step()函数范围0，则系统停止），该step函数通过Dataset类中的NextFrame()函数，读取下一帧，调整了图片的大小，返回新的帧，随后调用前端类（frontend）中的AddFrame函数添加该关键帧。
前端
Frontend() 初始化设定特征点个数的参数
AddFrame函数中根据当前状态status_分为初始化StereoInit()、好的追踪（当前帧设为下一帧，return继续追踪）、坏的追踪都进行Track()，丢失Reset()。最后将当前帧设为上一帧。
StereoInit()
检测左边图像特征DetectFeatures()，在右边图像找到左边图像对应的特征FindFeaturesInRight()，如果对应的特征点找的太少，返回false，这会使状态仍是初始化，下一帧依然进行初始化。特征点足够则建立初始地图，BuildInitMap()，并将状态改为好的追踪。并向viewer类中增减当前帧，且更新viewer类中的地图。BuildInitMap函数中进行三角化，初始化地图点：左右对应的特征点已知，而初始化的左相机位置为原点，且右相机可以通过标定的参数求出，因此能进行三角化。初始化地图点后，则插入地图中map->InsertMapPoint()，并且将第一帧作为关键帧，由于更新了关键帧，后端则需要更新地图了backend_->UpdateMap()。
Track()
追踪时假设的是恒速模型，作为位姿估计的初始值，然后使用光流法进行追踪TrackLastFrame，求出前后两帧的对应特征点并返回追踪到的特征点数，随后估计当前帧的位姿EstimateCurrentPose，采用了直接法求解当前位姿，仅优化位姿，返回追踪到的内点，如果追踪到的内点太少则设为丢失，Reset()，这里重新初始化没看到？？随后判断是否插入关键帧InsertKeyframe()。最后计算相对上一帧的位姿用于下一帧估计的初始化，并向viewer中加入当前帧。
Frontend::InsertKeyframe()
追踪的内点数大于阈值时将当前帧设为关键帧，并且调用map_->InsertKeyFrame，这里判断关键帧是不是太多,太多需要删除旧的关键帧，对于关键帧，首先为地图点增加观测到该点的特征点SetObservationsForKeyFrame()，然后提取新的特征点，找到这些点在右图的对应点，使用三角化建立新的路标点，将新的特征点和路标点加入地图触发一次后端优化（更新地图）backend->UpdateMap，viewer也更新地图。
后端
构造函数初始化一个线程，回调函数BackendLoop
BackendLoop： 仅优化激活的Frames和Landmarks：map->GetActiveKeyFrames()，map_->GetActiveMapPoints(); g2o优化Optimize()。
总结
三个线程：前端、后端、可视化，没有回环检测。双目算法的流程比较简单,只追踪左边图像的位姿(groundtruth给的也是左边相机的位姿)。初始化时，检测左边图像的特征，在右图像中找到与左图像对应的特征，如果找到的点太少，则下一帧继续进行初始化。由于左图像初始位姿为原点，而右图像可以通过标定参数求得，下面进行三角化，建立初始地图，第一帧作为关键帧（每次更新关键帧后端都进行一次优化）。假设恒速模型（最近两帧位姿差恒定），作为估计初始值，主要用于光流法追踪，得到前后两个左边图像帧匹配的特征点。随后用直接法切结位姿，如果追踪的点太少则reset。随后判断是否加入关键帧，当追踪的内点太少，则当前帧设为关键帧。如果关键帧太多需要进行剔除，这里直接去掉旧的关键帧和地图点，使关键帧和地图点维持一定的数量。添加关键帧继续提取新的特征点，并找到右图的对应点，三角化新的地图点，并更新地图，触发后端优化。
实现细节
待补
注意：
kitti中给定的groundtruth是左边相机的位姿。
丢失Reset好像什么都没做
github地址：https://github.com/gaoxiang12/slambook2/tree/master/ch13
双目视觉里程计
头文件
所有的类都在myslam命名空间中
1.common_include.h
定义常用的头文件、EIgen矩阵格式
2.algorithm.h
common_include.h
三角化，已知位姿和归一化平面的点，和书上有点区别，使用归一化平面的点比较方便
/**
* linear triangulation with SVD
* @param poses poses, （已知）
* @param points points in normalized plane （已知）
* @param pt_world triangulated point in the world （输出）
* @return true if success
*/
inline bool triangulation(const std::vector<SE3> &poses,const std::vector<Vec3> points, Vec3 &pt_world)
将opencv的点格式变为eigen2*1矩阵形式
inline Vec2 toVec2(const cv::Point2f p)
3.Config.h
common_include.h
class Config
/**
* 配置类，使用SetParameterFile确定配置文件
* 然后用Get得到对应值
* 单例模式
*/
static std::shared_ptr<Config> config_;
cv::FileStorage file_;
//设定参数文件名 参数文件读给file_
static bool SetParameterFile(const std::string &filename);
static T Get(const std::string &key) // 根据参数名key在file_中找到该参数的值
4.dataset.h
common_include.h frame.h camera.h
class Dataset
typedef std::shared_ptr<Dataset> Ptr;
Dataset(const std::string& dataset_path);
bool Init(); //初始化
Frame::Ptr NextFrame(); //创建并返回下一帧
Camera::Ptr GetCamera(int camera_id) //根据id返回相机参数
std::string dataset_path_; //数据集路径
int current_image_index_ = 0; //当前图像id
std::vector<Camera::Ptr> cameras_; //每一帧对应的相机参数（应该都是一样的）
5.camera.h
common_include.h
class Camera
double fx_ = 0, fy_ = 0, cx_ = 0, cy_ = 0,baseline_ = 0; // Camera intrinsics
SE3 pose_; // 双目到单目位姿变换
SE3 pose_inv_; // pose_的逆
Camera(double fx, double fy, double cx, double cy, double baseline,const SE3 &pose)
: fx_(fx), fy_(fy), cx_(cx), cy_(cy), baseline_(baseline), pose_(pose) {
pose_inv_ = pose_.inverse();
}
SE3 pose() //返回位姿
Mat33 K() //返回3*3的内参矩阵
Vec3 world2camera(const Vec3 &p_w, const SE3 &T_c_w); //世界到相机
Vec3 camera2world(const Vec3 &p_c, const SE3 &T_c_w); //相机到世界
Vec2 camera2pixel(const Vec3 &p_c); //相机到像素
Vec3 pixel2camera(const Vec2 &p_p, double depth = 1); //像素到相机（注意深度）
Vec3 pixel2world(const Vec2 &p_p, const SE3 &T_c_w, double depth = 1); //像素到世界
Vec2 world2pixel(const Vec3 &p_w, const SE3 &T_c_w); //世界到像素
6.feature.h
memory features2d.hpp common_include.h
struct Frame;
struct MapPoint;
struct Feature （没用类） 2D 特征点 在三角化之后会被关联一个地图点
typedef std::shared_ptr<Feature> Ptr;
std::weak_ptr<Frame> frame_; // 持有该feature的frame
cv::KeyPoint position_; // 2D提取位置
std::weak_ptr<MapPoint> map_point_; // 关联地图点
bool is_outlier_ = false; // 是否为异常点
bool is_on_left_image_ = true; // 标识是否提在左图，false为右图
Feature(std::shared_ptr<Frame> frame, const cv::KeyPoint &kp): frame_(frame), position_(kp) {} //构造
7.frame.h
camera.h common_include.h
struct MapPoint;
struct Feature;
struct Frame 每一帧分配独立id，关键帧分配关键帧ID
typedef std::shared_ptr<Frame> Ptr;
unsigned long id_ = 0; // id of this frame
unsigned long keyframe_id_ = 0; // id of key frame
bool is_keyframe_ = false; // 是否为关键帧
double time_stamp_; // 时间戳，暂不使用
SE3 pose_; // Tcw 形式Pose
std::mutex pose_mutex_; // Pose数据锁
cv::Mat left_img_, right_img_; // stereo images
std::vector<std::shared_ptr<Feature>> features_left_; //左图像中提取的特征点
std::vector<std::shared_ptr<Feature>> features_right_; //左图像在右图像中
Frame(long id, double time_stamp, const SE3 &pose, const Mat &left,const Mat &right);
SE3 Pose(); //返回位姿，加锁
void SetPose(const SE3 &pose) //设定位姿 加锁
void SetKeyFrame(); // 设置关键帧并分配并键帧id
static std::shared_ptr<Frame> CreateFrame(); // 工厂构建模式，分配id
8.mappoint.h
common_include.h
typedef std::shared_ptr<MapPoint> Ptr;
unsigned long id_ = 0; // ID
bool is_outlier_ = false; //是否为外点
Vec3 pos_ = Vec3::Zero(); // 世界坐标
std::mutex data_mutex_; //数据锁
int observed_times_ = 0; // 特征匹配时被观测到的次数
std::list<std::weak_ptr<Feature>> observations_; //观测到该地图点的特征
MapPoint(long id, Vec3 position);
Vec3 Pos() //返回世界坐标系中位置 （加锁）
void SetPos(const Vec3 &pos) //设定地图点世界坐标 （加锁）
void AddObservation(std::shared_ptr<Feature> feature) //添加观测到该地图点的特征 （加锁）
void RemoveObservation(std::shared_ptr<Feature> feat);
std::list<std::weak_ptr<Feature>> GetObs() //获取观测到该地图点的所有特征
static MapPoint::Ptr CreateNewMappoint(); //工厂模式 创建地图点
9.map.h
common_include.h frame.h mappoint.h
class Map
typedef std::shared_ptr<Map> Ptr;
typedef std::unordered_map<unsigned long, MapPoint::Ptr> LandmarksType; //地图点
typedef std::unordered_map<unsigned long, Frame::Ptr> KeyframesType; //关键帧
void InsertKeyFrame(Frame::Ptr frame); // 增加一个关键帧
void InsertMapPoint(MapPoint::Ptr map_point); // 增加一个地图顶点
LandmarksType GetAllMapPoints() // 获取所有地图点 (加锁)
KeyframesType GetAllKeyFrames() // 获取所有关键帧 (加锁)
LandmarksType GetActiveMapPoints() // 获取激活地图点
KeyframesType GetActiveKeyFrames() // 获取激活关键帧
void CleanMap(); // 清理map中观测数量为零的点
private：
void RemoveOldKeyframe(); // 将旧的关键帧置为不活跃状态
std::mutex data_mutex_; //数据锁
LandmarksType landmarks_; // all landmarks
LandmarksType active_landmarks_; // active landmarks
KeyframesType keyframes_; // all key-frames
KeyframesType active_keyframes_; // all key-frames
Frame::Ptr current_frame_ = nullptr; //当前帧
int num_active_keyframes_ = 7; // 激活的关键帧数量
10.frontend.h
map.h frame.h common_include.h
typedef std::shared_ptr<Frontend> Ptr;
bool AddFrame(Frame::Ptr frame); // 外部接口，添加一个帧并计算其定位结果
void SetMap(Map::Ptr map) { map_ = map; } //设定地图
void SetBackend(std::shared_ptr<Backend> backend) { backend_ = backend; } //设定后端
void SetViewer(std::shared_ptr<Viewer> viewer) { viewer_ = viewer; } //可视化
FrontendStatus GetStatus() const { return status_; } //返回当前状态 初始化 丢失 正常追踪
void SetCameras(Camera::Ptr left, Camera::Ptr right) //设定左右相机
bool Track(); //追踪 成功返回true
bool Reset(); //追踪丢失之后重置 成功返回seccess
int TrackLastFrame(); // 追踪上一帧 返回追踪到的点
int EstimateCurrentPose(); //估计当前帧的位姿，返回内点个数
bool InsertKeyframe(); //将当前帧设为关键帧 并插入后端
bool StereoInit(); //双目初始化
int DetectFeatures(); //检测当前帧左图像中的特征，返回数量 keypoint保存在当前帧
int FindFeaturesInRight(); //找到对应在右图像中的特征 返回找到对应的特征数量
bool BuildInitMap(); //用单张图像构建初始化特征
int TriangulateNewPoints(); //三角化当前帧的2d点 返回三角化的点
void SetObservationsForKeyFrame(); //将关键帧中的地图点设为新的地图点
FrontendStatus status_ = FrontendStatus::INITING; //前段状态 初始化 丢失 正常追踪
Frame::Ptr current_frame_ = nullptr; // 当前帧
Frame::Ptr last_frame_ = nullptr; // 上一帧
Camera::Ptr camera_left_ = nullptr; // 左侧相机
Camera::Ptr camera_right_ = nullptr; // 右侧相机
Map::Ptr map_ = nullptr; //地图
std::shared_ptr<Backend> backend_ = nullptr; //后端
std::shared_ptr<Viewer> viewer_ = nullptr; //初始化
SE3 relative_motion_; // 当前帧与上一帧的相对运动，用于估计当前帧pose初值
int tracking_inliers_ = 0; //内点数，用于测试是否加入新的关键帧
int num_features_ = 200;
int num_features_init_ = 100;
int num_features_tracking_ = 50;
int num_features_tracking_bad_ = 20;
int num_features_needed_for_keyframe_ = 80;
cv::Ptr<cv::GFTTDetector> gftt_; //opencv特征点检测
11.backend.h
common_include.h frame.h map.h
typedef std::shared_ptr<Backend> Ptr;
Backend(); // 构造函数中启动优化线程并挂起
void SetCameras(Camera::Ptr left, Camera::Ptr right) // 设置左右目的相机，用于获得内外参
void SetMap(std::shared_ptr<Map> map) // 设置地图
void UpdateMap(); // 触发地图更新，启动优化 （唤醒线程）
void Stop(); // 关闭后端线程
private:
void BackendLoop(); // 后端线程
// 对给定关键帧和路标点进行优化
void Optimize(Map::KeyframesType& keyframes, Map::LandmarksType& landmarks);
std::shared_ptr<Map> map_; //地图
std::thread backend_thread_;
std::mutex data_mutex_; //数据锁
std::condition_variable map_update_;
std::atomic<bool> backend_running_;
Camera::Ptr cam_left_ = nullptr, cam_right_ = nullptr; //相机
12.visual_odometry.h
backend.h commom_include.h frontend.h viewer.h
class VisualOdometry
typedef std::shared_ptr<VisualOdometry> Ptr;
VisualOdometry(std::string &config_path); //参数文件用于构造
bool Init(); //初始化
void Run(); //跑数据集VO
bool Step(); //啥啥啥
FrontendStatus GetFrontendStatus() //获取前端状态
private:
bool inited_ = false; //是否初始化
std::string config_file_path_; //参数文件路径
Frontend::Ptr frontend_ = nullptr;
Backend::Ptr backend_ = nullptr;
Map::Ptr map_ = nullptr;
Viewer::Ptr viewer_ = nullptr;
Dataset::Ptr dataset_ = nullptr; //数据集
13.viewer.h
thread pangolin.h common_include.h frame.h map.h
typedef std::shared_ptr<Viewer> Ptr;
Viewer();
void SetMap(Map::Ptr map)
void Close();
void AddCurrentFrame(Frame::Ptr current_frame); // 增加一个当前帧
void UpdateMap(); // 更新地图
private:
void ThreadLoop();
void DrawFrame(Frame::Ptr frame, const float* color); //绘制帧
void DrawMapPoints(); //绘制地图点
void FollowCurrentFrame(pangolin::OpenGlRenderState& vis_camera); //跟踪当前帧
cv::Mat PlotFrameImage(); //绘制当前帧的特征点
Frame::Ptr current_frame_ = nullptr;
Map::Ptr map_ = nullptr;
std::thread viewer_thread_;
bool viewer_running_ = true;
std::unordered_map<unsigned long, Frame::Ptr> active_keyframes_;
std::unordered_map<unsigned long, MapPoint::Ptr> active_landmarks_;
bool map_updated_ = false;
std::mutex viewer_data_mutex_;
代码流程
初始化
run_kitti_stereo.cpp 初始化一个VisualOdmetry类（记为VO），传入参数文件，并调用VO类中的初始化函数Init()（记为voInit），voInit函数建立一个Dataset类，并调用Dataset类中的初始化函数Init()（记为dataInit）,dataInit读取相机参数和标定的参数。随后voInit创建前端、后端、map、viewer类（生成对应的智能指针，调用一些set函数）。初始化完成后，run_kitti_stereo.cpp调用VO类中的启动函数run()（记为vorun）。vorun循环调用VO类中的step()函数（除非step()函数范围0，则系统停止），该step函数通过Dataset类中的NextFrame()函数，读取下一帧，调整了图片的大小，返回新的帧，随后调用前端类（frontend）中的AddFrame函数添加该关键帧。
前端
Frontend() 初始化设定特征点个数的参数
AddFrame函数中根据当前状态status_分为初始化StereoInit()、好的追踪（当前帧设为下一帧，return继续追踪）、坏的追踪都进行Track()，丢失Reset()。最后将当前帧设为上一帧。
StereoInit()
检测左边图像特征DetectFeatures()，在右边图像找到左边图像对应的特征FindFeaturesInRight()，如果对应的特征点找的太少，返回false，这会使状态仍是初始化，下一帧依然进行初始化。特征点足够则建立初始地图，BuildInitMap()，并将状态改为好的追踪。并向viewer类中增减当前帧，且更新viewer类中的地图。BuildInitMap函数中进行三角化，初始化地图点：左右对应的特征点已知，而初始化的左相机位置为原点，且右相机可以通过标定的参数求出，因此能进行三角化。初始化地图点后，则插入地图中map->InsertMapPoint()，并且将第一帧作为关键帧，由于更新了关键帧，后端则需要更新地图了backend_->UpdateMap()。
Track()
追踪时假设的是恒速模型，作为位姿估计的初始值，然后使用光流法进行追踪TrackLastFrame，求出前后两帧的对应特征点并返回追踪到的特征点数，随后估计当前帧的位姿EstimateCurrentPose，采用了直接法求解当前位姿，仅优化位姿，返回追踪到的内点，如果追踪到的内点太少则设为丢失，Reset()，这里重新初始化没看到？？随后判断是否插入关键帧InsertKeyframe()。最后计算相对上一帧的位姿用于下一帧估计的初始化，并向viewer中加入当前帧。
Frontend::InsertKeyframe()
追踪的内点数大于阈值时将当前帧设为关键帧，并且调用map_->InsertKeyFrame，这里判断关键帧是不是太多,太多需要删除旧的关键帧，对于关键帧，首先为地图点增加观测到该点的特征点SetObservationsForKeyFrame()，然后提取新的特征点，找到这些点在右图的对应点，使用三角化建立新的路标点，将新的特征点和路标点加入地图触发一次后端优化（更新地图）backend->UpdateMap，viewer也更新地图。
后端
构造函数初始化一个线程，回调函数BackendLoop
BackendLoop： 仅优化激活的Frames和Landmarks：map->GetActiveKeyFrames()，map_->GetActiveMapPoints(); g2o优化Optimize()。
总结
三个线程：前端、后端、可视化，没有回环检测。双目算法的流程比较简单,只追踪左边图像的位姿(groundtruth给的也是左边相机的位姿)。初始化时，检测左边图像的特征，在右图像中找到与左图像对应的特征，如果找到的点太少，则下一帧继续进行初始化。由于左图像初始位姿为原点，而右图像可以通过标定参数求得，下面进行三角化，建立初始地图，第一帧作为关键帧（每次更新关键帧后端都进行一次优化）。假设恒速模型（最近两帧位姿差恒定），作为估计初始值，主要用于光流法追踪，得到前后两个左边图像帧匹配的特征点。随后用直接法切结位姿，如果追踪的点太少则reset。随后判断是否加入关键帧，当追踪的内点太少，则当前帧设为关键帧。如果关键帧太多需要进行剔除，这里直接去掉旧的关键帧和地图点，使关键帧和地图点维持一定的数量。添加关键帧继续提取新的特征点，并找到右图的对应点，三角化新的地图点，并更新地图，触发后端优化。
实现细节
待补
注意：
kitti中给定的groundtruth是左边相机的位姿。
丢失Reset好像什么都没做
SLAM十四讲第二版项目代码总结的更多相关文章
- 视觉slam十四讲ch5 joinMap.cpp 代码注释（笔记版）
#include <iostream> #include <fstream> using namespace std; #include <opencv2/core/co ...
- 视觉SLAM十四讲：从理论到实践 两版 PDF和源码
视觉SLAM十四讲:从理论到实践 第一版电子版PDF 链接:https://pan.baidu.com/s/1SuuSpavo_fj7xqTYtgHBfw提取码:lr4t 源码github链接:htt ...
- 浅读《视觉SLAM十四讲：从理论到实践》--操作1--初识SLAM
下载<视觉SLAM十四讲:从理论到实践>源码:https://github.com/gaoxiang12/slambook 第二讲:初识SLAM 2.4.2 Hello SLAM(书本P2 ...
- 《SLAM十四讲》个人学习知识点梳理
0.引言 从六月末到八月初大概一个月时间一直在啃SLAM十四讲[1]这本书,这本书把SLAM中涉及的基本知识点都涵盖了,所以在这里做一个复习,对这本书自己学到的东西做一个梳理. 书本地址:http:/ ...
- 视觉slam十四讲第七章课后习题6
版权声明:本文为博主原创文章,转载请注明出处: http://www.cnblogs.com/newneul/p/8545450.html 6.在PnP优化中,将第一个相机的观测也考虑进来,程序应如何 ...
- 视觉slam十四讲第七章课后习题7
版权声明:本文为博主原创文章,转载请注明出处:http://www.cnblogs.com/newneul/p/8544369.html 7.题目要求:在ICP程序中,将空间点也作为优化变量考虑进来 ...
- 高博-《视觉SLAM十四讲》
0 讲座 (1)SLAM定义 对比雷达传感器和视觉传感器的优缺点(主要介绍视觉SLAM) 单目:不知道尺度信息 双目:知道尺度信息,但测量范围根据预定的基线相关 RGBD:知道深度信息,但是深度信息对 ...
- SLAM十四讲中Sophus库安装
Sophus截止目前有很多版本,其中大体分为两类,一种是用模板实现的方法,一种是用非模板类实现的,SLAM十四讲中使用的是非模板类库,clone Sophus: git clone http://gi ...
- 《视觉SLAM十四讲》第2讲
目录 一 视觉SLAM中的传感器 二 经典视觉SLAM框架 三 SLAM问题的数学表述 注:原创不易,转载请务必注明原作者和出处,感谢支持! 本讲主要内容: (1) 视觉SLAM中的传感器 (2) 经 ...
随机推荐
- 使用JavaScript获取url(request)中的参数
这次是使用JavaScript来获取url(request)中的参数 在日常页面编写的过程中为了方便操作在<script>中通过使用window.location.href="要 ...
- Vue packages version mismatch的解决方法 初来乍到，踩坑日常
初来乍到,踩坑日常 这个问题也是我也是接受别人项目,出现的问题,在下载好依赖后运行的时候报这样的错误 它上面显示两个版本一个vue的版本,一个vue-template-compiler版本,我这边忘了 ...
- Pytorch CNN网络MNIST数字识别 [超详细记录] 学习笔记(三)
目录 1. 准备数据集 1.1 MNIST数据集获取: 1.2 程序部分 2. 设计网络结构 2.1 网络设计 2.2 程序部分 3. 迭代训练 4. 测试集预测部分 5. 全部代码 1. 准备数据集 ...
- 三、WPF入门教程——布局和常用Panel学习
布局和常用Panel学习 一.简介 所有的WPF布局容器都派生自System.Windows.Controls.Panel.Panel继承自FrameworkElement. 在Panel中有一个比较 ...
- MediaStore 与Media.EXTERNAL_CONTENT_URI
MediaStore这个类是Android系统提供的一个多媒体数据库,android中多媒体信息都可以从这里提取. 这个MediaStore包括了多媒体数据库的所有信息,包括音频,视频和图像,andr ...
- Vue项目的开发流程
我先安装的node.js 1.确认已安装了node.js,可在cmd中输入( node -v和npm -v),如显示出版号,说明安装成功 2.安装webpack 和webpack-cli 在全局下安装 ...
- 2、SpringBoot整合之SpringBoot整合servlet
SpringBoot整合servlet 一.创建SpringBoot项目,仅选择Web模块即可 二.在POM文件中添加依赖 <!-- 添加servlet依赖模块 --> <depen ...
- 乘风破浪，.Net Core遇见MAUI(.NET Multi-platform App UI)，进击现代化跨设备应用框架
什么是MAUI https://github.com/dotnet/maui .NET Multi-platform App UI (MAUI) 的前身是Xamarin.Forms(适用于Androi ...
- Zoho Books十年发展历程
十年前,我们推出Zoho Books的时候,是为了全面解决企业面临的财务和会计方面的挑战.我们逐渐地从一开始的易用的中小企业在线会计工具,发展成为现在的解决企业复杂的财务挑战的解决方案,其中经历了很多 ...
- 增强采样软件PLUMED的安装与使用
技术背景 增强采样(Enhanced Sampling)是一种在分子动力学模拟中常用的技术,其作用是帮助我们更加快速的在时间轴上找到尽可能多的体系结构及其对应的能量.比如一个氢气的燃烧反应,在中间过程 ...