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好像什么都没做