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