backend optimization done

ch13/CMakeLists.txt

@@ -40,15 +40,21 @@ include_directories(${GTEST_INCLUDE_DIRS})
 find_package(GFlags REQUIRED)
 include_directories(${GFLAGS_INCLUDE_DIRS})
 
+# csparse
+find_package(CSparse REQUIRED)
+include_directories(${CSPARSE_INCLUDE_DIR})
+
 set(THIRD_PARTY_LIBS
         ${OpenCV_LIBS}
         ${Sophus_LIBRARIES}
         ${Pangolin_LIBRARIES} GL GLU GLEW glut
-        g2o_core g2o_stuff g2o_types_sba
+        g2o_core g2o_stuff g2o_types_sba g2o_solver_csparse g2o_csparse_extension
         ${GTEST_BOTH_LIBRARIES}
         ${GLOG_LIBRARIES}
         ${GFLAGS_LIBRARIES}
-        pthread)
+        pthread
+        ${CSPARSE_LIBRARY}
+        )
 
 enable_testing()
 

ch13/cmake_modules/FindCSparse.cmake

@@ -1,5 +1,5 @@
 # Look for csparse; note the difference in the directory specifications!
-FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
+find_path(CSPARSE_INCLUDE_DIR NAMES cs.h
   PATHS
   /usr/include/suitesparse
   /usr/include
@@ -10,9 +10,11 @@ FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
   /opt/local/include/ufsparse
   /usr/local/include/ufsparse
   /sw/include/ufsparse
+  PATH_SUFFIXES
+  suitesparse
   )
 
-FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
+find_library(CSPARSE_LIBRARY NAMES cxsparse libcxsparse
   PATHS
   /usr/lib
   /usr/local/lib

ch13/include/myslam/algorithm.h

@@ -17,7 +17,7 @@ namespace myslam {
  * @param pt_world  triangulated point in the world
  * @return true if success
  */
-bool triangulation(const std::vector<SE3> &poses,
+inline bool triangulation(const std::vector<SE3> &poses,
                    const std::vector<Vec3> points, Vec3 &pt_world) {
     MatXX A(2 * poses.size(), 4);
     VecX b(2 * poses.size());

ch13/include/myslam/backend.h

@@ -7,6 +7,7 @@
 
 #include "myslam/common_include.h"
 #include "myslam/frame.h"
+#include "myslam/map.h"
 
 namespace myslam {
 class Map;
@@ -15,14 +16,33 @@ class Backend {
    public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
     typedef std::shared_ptr<Backend> Ptr;
-    Backend() {}
+    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_;
+    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;
 };
 
 }  // namespace myslam

ch13/include/myslam/common_include.h

@@ -8,11 +8,14 @@
 #include <list>
 #include <memory>
 #include <mutex>
+#include <atomic>
 #include <string>
 #include <iostream>
 #include <set>
 #include <unordered_map>
+#include <condition_variable>
 #include <map>
+#include <thread>
 
 // define the commonly included file to avoid a long include list
 #include <Eigen/Core>

ch13/include/myslam/feature.h

@@ -16,21 +16,22 @@ struct Frame;
 struct MapPoint;
 
 struct Feature {
-public:
+   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;
-    bool is_on_left_image_ = true;
+    bool is_on_left_image_ = true;  // 标识是否提在左图，false为右图
 
-public:
+   public:
     Feature() {}
 
-    Feature(std::shared_ptr<Frame> frame, const cv::KeyPoint &kp) : frame_(frame), position_(kp) {}
+    Feature(std::shared_ptr<Frame> frame, const cv::KeyPoint &kp)
+        : frame_(frame), position_(kp) {}
 };
-}
+}  // namespace myslam
 
-#endif //MYSLAM_FEATURE_H
+#endif  // MYSLAM_FEATURE_H

ch13/include/myslam/g2o_types.h

@@ -7,6 +7,7 @@
 
 #include "myslam/common_include.h"
 
+#include <g2o/core/base_binary_edge.h>
 #include <g2o/core/base_unary_edge.h>
 #include <g2o/core/base_vertex.h>
 #include <g2o/core/block_solver.h>
@@ -15,6 +16,7 @@
 #include <g2o/core/robust_kernel_impl.h>
 #include <g2o/core/solver.h>
 #include <g2o/core/sparse_optimizer.h>
+#include <g2o/solvers/csparse/linear_solver_csparse.h>
 #include <g2o/solvers/dense/linear_solver_dense.h>
 
 namespace myslam {
@@ -38,6 +40,22 @@ class VertexPose : public g2o::BaseVertex<6, SE3> {
     virtual bool write(std::ostream &out) const override { return true; }
 };
 
+class VertexXYZ : public g2o::BaseVertex<3, Vec3> {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    virtual void setToOriginImpl() override { _estimate = Vec3::Zero(); }
+
+    virtual void oplusImpl(const double *update) override {
+        _estimate[0] += update[0];
+        _estimate[1] += update[1];
+        _estimate[2] += update[2];
+    }
+
+    virtual bool read(std::istream &in) override { return true; }
+
+    virtual bool write(std::ostream &out) const override { return true; }
+};
+
 class EdgeProjectionPoseOnly : public g2o::BaseUnaryEdge<2, Vec2, VertexPose> {
    public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -79,6 +97,56 @@ class EdgeProjectionPoseOnly : public g2o::BaseUnaryEdge<2, Vec2, VertexPose> {
     Mat33 _K;
 };
 
+class EdgeProjection
+    : public g2o::BaseBinaryEdge<2, Vec2, VertexPose, VertexXYZ> {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+    /// 构造时传入相机内外参
+    EdgeProjection(const Mat33 &K, const SE3 &cam_ext) : _K(K) {
+        _cam_ext = cam_ext;
+    }
+
+    virtual void computeError() override {
+        const VertexPose *v0 = static_cast<VertexPose *>(_vertices[0]);
+        const VertexXYZ *v1 = static_cast<VertexXYZ *>(_vertices[1]);
+        SE3 T = v0->estimate();
+        Vec3 pos_pixel = _K * (_cam_ext * (T * v1->estimate()));
+        pos_pixel /= pos_pixel[2];
+        _error = _measurement - pos_pixel.head<2>();
+    }
+
+    virtual void linearizeOplus() override {
+        const VertexPose *v0 = static_cast<VertexPose *>(_vertices[0]);
+        const VertexXYZ *v1 = static_cast<VertexXYZ *>(_vertices[1]);
+        SE3 T = v0->estimate();
+        Vec3 pw = v1->estimate();
+        Vec3 pos_cam = _cam_ext * T * pw;
+        double fx = _K(0, 0);
+        double fy = _K(1, 1);
+        double X = pos_cam[0];
+        double Y = pos_cam[1];
+        double Z = pos_cam[2];
+        double Zinv = 1.0 / (Z + 1e-18);
+        double Zinv2 = Zinv * Zinv;
+        _jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2,
+            -fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv,
+            fy * Y * Zinv2, fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2,
+            -fy * X * Zinv;
+
+        _jacobianOplusXj = _jacobianOplusXi.block<2, 3>(0, 0) *
+                           _cam_ext.rotationMatrix() * T.rotationMatrix();
+    }
+
+    virtual bool read(std::istream &in) override { return true; }
+
+    virtual bool write(std::ostream &out) const override { return true; }
+
+   private:
+    Mat33 _K;
+    SE3 _cam_ext;
+};
+
 }  // namespace myslam
 
 #endif  // MYSLAM_G2O_TYPES_H

ch13/include/myslam/map.h

@@ -9,7 +9,8 @@
 namespace myslam {
 
 /**
- * @brief 和地图的交互：前端调用InsertKeyframe和InsertMapPoint插入新帧和地图点，后端维护地图的结构，判定outlier/剔除等等
+ * @brief
+ * 和地图的交互：前端调用InsertKeyframe和InsertMapPoint插入新帧和地图点，后端维护地图的结构，判定outlier/剔除等等
  */
 class Map {
    public:
@@ -28,26 +29,36 @@ class Map {
         std::unique_lock<std::mutex> lck(data_mutex_);
         return landmarks_;
     }
-    LandmarksType GetActiveMapPoints() {
+    KeyframesType GetAllKeyFrames() {
         std::unique_lock<std::mutex> lck(data_mutex_);
-        return active_landmarks_;
+        return keyframes_;
     }
 
-    KeyframesType GetAllKeyFrames() {
+    LandmarksType GetActiveMapPoints() {
         std::unique_lock<std::mutex> lck(data_mutex_);
-        return keyframes_;
+        return active_landmarks_;
     }
+
     KeyframesType GetActiveKeyFrames() {
         std::unique_lock<std::mutex> lck(data_mutex_);
         return active_keyframes_;
     }
 
+    /// 清理map中观测数量为零的点
+    void CleanMap();
+
    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
+
+    // settings
+    int num_active_keyframes_ = 10;  // 激活的关键帧数量
 };
 }  // namespace myslam
 

ch13/include/myslam/mappoint.h

@@ -2,22 +2,24 @@
 #ifndef MYSLAM_MAPPOINT_H
 #define MYSLAM_MAPPOINT_H
 
+#include "myslam/common_include.h"
+
 namespace myslam {
 
 struct Frame;
 
 struct Feature;
 
 struct MapPoint {
-public:
+   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
     typedef std::shared_ptr<MapPoint> Ptr;
-    unsigned long id_ = 0; // ID
+    unsigned long id_ = 0;  // ID
     bool is_outlier_ = false;
-    Vec3 pos_ = Vec3::Zero();       // Position in world
+    Vec3 pos_ = Vec3::Zero();  // Position in world
 
     std::mutex data_mutex_;
-    int observed_times_ = 0;    // being observed by feature matching algo.
+    int observed_times_ = 0;  // being observed by feature matching algo.
     std::list<std::weak_ptr<Feature>> observations_;
 
     MapPoint() {}
@@ -39,9 +41,15 @@ struct MapPoint {
         observations_.push_back(feature);
     }
 
+    std::list<std::weak_ptr<Feature>> GetObs() {
+        std::unique_lock<std::mutex> lck(data_mutex_);
+        return observations_;
+    }
+
+    void RemoveObservation(std::shared_ptr<Feature> feat);
     // factory function
     static MapPoint::Ptr CreateNewMappoint();
 };
-}
+}  // namespace myslam
 
-#endif // MYSLAM_MAPPOINT_H
+#endif  // MYSLAM_MAPPOINT_H