adding comments in project

ch13/include/myslam/algorithm.h

@@ -31,6 +31,7 @@ inline bool triangulation(const std::vector<SE3> &poses,
     pt_world = (svd.matrixV().col(3) / svd.matrixV()(3, 3)).head<3>();
 
     if (svd.singularValues()[3] / svd.singularValues()[2] < 1e-2) {
+        // 解质量不好，放弃
         return true;
     }
     return false;

ch13/include/myslam/backend.h

@@ -12,27 +12,39 @@
 namespace myslam {
 class Map;
 
+/**
+ * 后端
+ * 有单独优化线程，在Map更新时启动优化
+ * Map更新由前端触发
+ */ 
 class Backend {
    public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
     typedef std::shared_ptr<Backend> Ptr;
+
+    /// 构造函数中启动优化线程并挂起
     Backend();
 
+    // 设置左右目的相机，用于获得内外参
     void SetCameras(Camera::Ptr left, Camera::Ptr right) {
         cam_left_ = left;
         cam_right_ = right;
     }
 
+    /// 设置地图
     void SetMap(std::shared_ptr<Map> map) { map_ = map; }
 
+    /// 触发地图更新，启动优化
     void UpdateMap();
 
-    // Stop backend thread
+    /// 关闭后端线程
     void Stop();
 
    private:
+    /// 后端线程
     void BackendLoop();
 
+    /// 对给定关键帧和路标点进行优化
     void Optimize(Map::KeyframesType& keyframes, Map::LandmarksType& landmarks);
 
     std::shared_ptr<Map> map_;

ch13/include/myslam/camera.h

@@ -7,19 +7,24 @@
 
 namespace myslam {
 
-// Pinhole RGBD camera model
+// Pinhole stereo camera model
 class Camera {
-public:
+   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
     typedef std::shared_ptr<Camera> Ptr;
-    double fx_ = 0, fy_ = 0, cx_ = 0, cy_ = 0, baseline_ = 0;  // Camera intrinsics
-    SE3 pose_;   // extrinsic, from stereo camera to single camera
-    SE3 pose_inv_;   // inverse of extrinsics
+
+    double fx_ = 0, fy_ = 0, cx_ = 0, cy_ = 0,
+           baseline_ = 0;  // Camera intrinsics
+    SE3 pose_;             // extrinsic, from stereo camera to single camera
+    SE3 pose_inv_;         // inverse of extrinsics
 
     Camera();
 
-    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(); }
+    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() const { return pose_; }
 
@@ -42,8 +47,7 @@ class Camera {
     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);
-
 };
 
-}
-#endif // MYSLAM_CAMERA_H
+}  // namespace myslam
+#endif  // MYSLAM_CAMERA_H

ch13/include/myslam/common_include.h

@@ -3,19 +3,19 @@
 #define MYSLAM_COMMON_INCLUDE_H
 
 // std
-#include <vector>
-#include <typeinfo>
+#include <atomic>
+#include <condition_variable>
+#include <iostream>
 #include <list>
+#include <map>
 #include <memory>
 #include <mutex>
-#include <atomic>
-#include <string>
-#include <iostream>
 #include <set>
-#include <unordered_map>
-#include <condition_variable>
-#include <map>
+#include <string>
 #include <thread>
+#include <typeinfo>
+#include <unordered_map>
+#include <vector>
 
 // define the commonly included file to avoid a long include list
 #include <Eigen/Core>

ch13/include/myslam/config.h

@@ -5,24 +5,30 @@
 #include "myslam/common_include.h"
 
 namespace myslam {
+
+/**
+ * 配置类，使用SetParameterFile确定配置文件
+ * 然后用Get得到对应值
+ * 单例模式
+ */
 class Config {
-private:
+   private:
     static std::shared_ptr<Config> config_;
     cv::FileStorage file_;
 
-    Config() {} // private constructor makes a singleton
-public:
+    Config() {}  // private constructor makes a singleton
+   public:
     ~Config();  // close the file when deconstructing
 
     // set a new config file
     static bool SetParameterFile(const std::string &filename);
 
     // access the parameter values
-    template<typename T>
+    template <typename T>
     static T Get(const std::string &key) {
         return T(Config::config_->file_[key]);
     }
 };
-}
+}  // namespace myslam
 
-#endif // MYSLAM_CONFIG_H
+#endif  // MYSLAM_CONFIG_H

ch13/include/myslam/dataset.h

@@ -6,11 +6,18 @@
 
 namespace myslam {
 
+/**
+ * 数据集读取
+ * 构造时传入配置文件路径，配置文件的dataset_dir为数据集路径
+ * Init之后可获得相机和下一帧图像
+ */
 class Dataset {
    public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
     typedef std::shared_ptr<Dataset> Ptr;
     Dataset(const std::string& dataset_path);
+
+    /// 初始化，返回是否成功
     bool Init();
 
     /// create and return the next frame containing the stereo images

ch13/include/myslam/feature.h

@@ -15,15 +15,20 @@ namespace myslam {
 struct Frame;
 struct MapPoint;
 
+/**
+ * 2D 特征点
+ * 在三角化之后会被关联一个地图点
+ */
 struct Feature {
    public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
     typedef std::shared_ptr<Feature> Ptr;
-    std::weak_ptr<Frame> frame_;
-    cv::KeyPoint position_;
-    std::weak_ptr<MapPoint> map_point_;
 
-    bool is_outlier_ = false;
+    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为右图
 
    public:

ch13/include/myslam/frame.h

@@ -3,40 +3,42 @@
 #ifndef MYSLAM_FRAME_H
 #define MYSLAM_FRAME_H
 
-#include "myslam/common_include.h"
 #include "myslam/camera.h"
+#include "myslam/common_include.h"
 
 namespace myslam {
 
-// forward declare 
+// forward declare
 struct MapPoint;
 struct Feature;
 
+/**
+ * 帧
+ * 每一帧分配独立id，关键帧分配关键帧ID
+ */
 struct Frame {
-public:
+   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
     typedef std::shared_ptr<Frame> Ptr;
 
-    unsigned long id_ = 0;         // id of this frame
+    unsigned long id_ = 0;           // id of this frame
     unsigned long keyframe_id_ = 0;  // id of key frame
-    bool is_keyframe_ = false;
-    double time_stamp_;        // when it is recorded
-    SE3 pose_;                  // Tcw, transform from world to camera
-    std::mutex pose_mutex_;
-    cv::Mat left_img_, right_img_; // stereo images
+    bool is_keyframe_ = false;       // 是否为关键帧
+    double time_stamp_;              // 时间戳，暂不使用
+    SE3 pose_;                       // Tcw 形式Pose
+    std::mutex pose_mutex_;          // Pose数据锁
+    cv::Mat left_img_, right_img_;   // stereo images
 
     // extracted features in left image
     std::vector<std::shared_ptr<Feature>> features_left_;
-
-    // corresponding features in right image, set to nullptr if no corresponding feature is found
+    // corresponding features in right image, set to nullptr if no corresponding
     std::vector<std::shared_ptr<Feature>> features_right_;
 
-public: // data members 
+   public:  // data members
     Frame() {}
 
-    Frame(long id, double time_stamp, const SE3 &pose, const Mat &left, const Mat &right);
-
-    ~Frame();
+    Frame(long id, double time_stamp, const SE3 &pose, const Mat &left,
+          const Mat &right);
 
     // set and get pose, thread safe
     SE3 Pose() {
@@ -49,13 +51,13 @@ struct Frame {
         pose_ = pose;
     }
 
-    /// set this frame as a keyframe and allocate keyframe id
+    /// 设置关键帧并分配并键帧id
     void SetKeyFrame();
 
-    // factory function
+    /// 工厂构建模式，分配id 
     static std::shared_ptr<Frame> CreateFrame();
 };
 
-}
+}  // namespace myslam
 
-#endif // MYSLAM_FRAME_H
+#endif  // MYSLAM_FRAME_H

ch13/include/myslam/frontend.h

@@ -15,6 +15,10 @@ class Viewer;
 
 enum class FrontendStatus { INITING, TRACKING_GOOD, TRACKING_BAD, LOST };
 
+/**
+ * 前端
+ * 估计当前帧Pose，在满足关键帧条件时向地图加入关键帧并触发优化
+ */
 class Frontend {
    public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -24,6 +28,7 @@ class Frontend {
 
     bool AddFrame(Frame::Ptr frame);
 
+    // 
     void SetMap(Map::Ptr map) { map_ = map; }
 
     void SetBackend(std::shared_ptr<Backend> backend) { backend_ = backend; }

ch13/src/backend.cpp

@@ -23,17 +23,18 @@ void Backend::UpdateMap() {
 
 void Backend::Stop() {
     backend_running_.store(false);
+    map_update_.notify_one();
     backend_thread_.join();
 }
 
 void Backend::BackendLoop() {
     while (backend_running_.load()) {
         std::unique_lock<std::mutex> lock(data_mutex_);
         map_update_.wait(lock);
+
         /// 后端仅优化激活的Frames和Landmarks
         Map::KeyframesType active_kfs = map_->GetActiveKeyFrames();
         Map::LandmarksType active_landmarks = map_->GetActiveMapPoints();
-
         Optimize(active_kfs, active_landmarks);
     }
 }
@@ -50,7 +51,7 @@ void Backend::Optimize(Map::KeyframesType &keyframes,
     g2o::SparseOptimizer optimizer;
     optimizer.setAlgorithm(solver);
 
-    // pose 顶点，id使用Keyframe id
+    // pose 顶点，使用Keyframe id
     std::map<unsigned long, VertexPose *> vertices;
     unsigned long max_kf_id = 0;
     for (auto &keyframe : keyframes) {
@@ -66,17 +67,17 @@ void Backend::Optimize(Map::KeyframesType &keyframes,
         vertices.insert({kf->keyframe_id_, vertex_pose});
     }
 
-    // landmarks, will be created when adding new edges
+    // 路标顶点，使用路标id索引
     std::map<unsigned long, VertexXYZ *> vertices_landmarks;
 
-    // K
+    // K 和左右外参
     Mat33 K = cam_left_->K();
     SE3 left_ext = cam_left_->pose();
     SE3 right_ext = cam_right_->pose();
 
     // edges
     int index = 1;
-    double chi2_th = 5.991;
+    double chi2_th = 5.991; // robust kernel 阈值
     std::map<EdgeProjection *, Feature::Ptr> edges_and_features;
 
     for (auto &landmark : landmarks) {
@@ -96,6 +97,7 @@ void Backend::Optimize(Map::KeyframesType &keyframes,
                 edge = new EdgeProjection(K, right_ext);
             }
 
+            // 如果landmark还没有被加入优化，则新加一个顶点
             if (vertices_landmarks.find(landmark_id) ==
                 vertices_landmarks.end()) {
                 VertexXYZ *v = new VertexXYZ;
@@ -168,8 +170,6 @@ void Backend::Optimize(Map::KeyframesType &keyframes,
     for (auto &v : vertices_landmarks) {
         landmarks.at(v.first)->SetPos(v.second->estimate());
     }
-
-    map_->CleanMap();
 }
 
 }  // namespace myslam

ch13/src/frame.cpp

@@ -24,8 +24,6 @@ namespace myslam {
 Frame::Frame(long id, double time_stamp, const SE3 &pose, const Mat &left, const Mat &right)
         : id_(id), time_stamp_(time_stamp), pose_(pose), left_img_(left), right_img_(right) {}
 
-Frame::~Frame() {}
-
 Frame::Ptr Frame::CreateFrame() {
     static long factory_id = 0;
     Frame::Ptr new_frame(new Frame);

ch13/test/CMakeLists.txt

@@ -1,7 +1,7 @@
 SET(TEST_SOURCES test_triangulation)
 
 FOREACH (test_src ${TEST_SOURCES})
-    ADD_EXECUTABLE(${test_src} ${test_src}.cpp test_triangulation.cpp)
+    ADD_EXECUTABLE(${test_src} ${test_src}.cpp)
     TARGET_LINK_LIBRARIES(${test_src} ${THIRD_PARTY_LIBS} myslam)
     ADD_TEST(${test_src} ${test_src})
 ENDFOREACH (test_src)