Replace g2o & Sophus codes to the latest commit version

CMakeLists.txt

@@ -45,7 +45,7 @@ set( THIRD_PARTY_LIBS
     ${OpenCV_LIBS}
     ${PCL_LIBRARIES}
     ${Sophus_LIBRARIES} # If "make install" is good, use this line
-    libSophus.so # If "make install" failed, copy files manully to usr/lib and usr/include, and use this line
+    # libSophus.so # If "make install" failed, copy files manully to usr/lib and usr/include, and use this line
     g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension
     ${CSPARSE_LIBRARY}
 )
@@ -61,4 +61,4 @@ target_link_libraries( run_vo
     geometry
     display
     vo
-)
+)

include/my_slam/basics/eigen_funcs.h

@@ -9,8 +9,8 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-#include <sophus/so3.h>
-#include <sophus/se3.h>
+#include <sophus/so3.hpp>
+#include <sophus/se3.hpp>
 
 #include <opencv2/core/eigen.hpp>
 
@@ -30,10 +30,10 @@ Eigen::Affine3d getAffine3d(double x, double y, double z, double rot_axis_x, dou
 Eigen::Affine3d transT_CVRt_to_EigenAffine3d(const cv::Mat &R, const cv::Mat &t);
 
 // -------------- CV <--> Sophus --------------
-Sophus::SE3 transT_cv2sophus(const cv::Mat &T_cv);
-cv::Mat transT_sophus2cv(const Sophus::SE3 &T_sophus);
+Sophus::SE3d transT_cv2sophus(const cv::Mat &T_cv);
+cv::Mat transT_sophus2cv(const Sophus::SE3d &T_sophus);
 
 } // namespace basics
 } // namespace my_slam
 
-#endif
+#endif

src/basics/eigen_funcs.cpp

@@ -33,19 +33,19 @@ Eigen::Affine3d transT_CVRt_to_EigenAffine3d(const cv::Mat &R0, const cv::Mat &t
 
 // -------------- CV <--> Sophus --------------
 
-Sophus::SE3 transT_cv2sophus(const cv::Mat &T_cv)
+Sophus::SE3d transT_cv2sophus(const cv::Mat &T_cv)
 {
     Eigen::Matrix3d R_eigen;
     cv::cv2eigen(T_cv(cv::Rect2d(0, 0, 3, 3)), R_eigen);
     Eigen::Vector3d t_eigen(T_cv.at<double>(0, 3), T_cv.at<double>(1, 3), T_cv.at<double>(2, 3));
-    Sophus::SE3 SE3_Rt(R_eigen, t_eigen);
+    Sophus::SE3d SE3_Rt(R_eigen, t_eigen);
     return SE3_Rt;
 }
 
-cv::Mat transT_sophus2cv(const Sophus::SE3 &T_sophus)
+cv::Mat transT_sophus2cv(const Sophus::SE3d &T_sophus)
 {
     Eigen::Vector3d eigen_t(T_sophus.translation());
-    Eigen::Matrix3d eigen_R(T_sophus.rotation_matrix());
+    Eigen::Matrix3d eigen_R(T_sophus.rotationMatrix());
 
     cv::Mat cv_t, cv_R;
     eigen2cv(eigen_t, cv_t);
@@ -55,4 +55,4 @@ cv::Mat transT_sophus2cv(const Sophus::SE3 &T_sophus)
 }
 
 } // namespace basics
-} // namespace my_slam
+} // namespace my_slam

src/optimization/g2o_ba.cpp

@@ -40,15 +40,17 @@ void optimizeSingleFrame(
 {
     const cv::Mat pose_src0 = pose_src.clone();
 
-    // Change pose format from OpenCV to Sophus::SE3
+    // Change pose format from OpenCV to Sophus::SE3d
     cv::Mat T_cam_to_world_cv = pose_src.inv();
-    Sophus::SE3 T_cam_to_world = basics::transT_cv2sophus(T_cam_to_world_cv);
+    Sophus::SE3d T_cam_to_world = basics::transT_cv2sophus(T_cam_to_world_cv);
 
     // Init g2o
-    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;                                  // dim(pose) = 6, dim(landmark) = 3
-    Block::LinearSolverType *linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // solver for linear equation
-    Block *solver_ptr = new Block(linearSolver);                                                   // solver for matrix block
-    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+    std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linear_solver;
+    linear_solver = g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>>();
+    // use LM method to minimize nonlinear least square.
+    g2o::OptimizationAlgorithmLevenberg* solver =
+        new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linear_solver)));
+    solver->setUserLambdaInit(1);
     g2o::SparseOptimizer optimizer;
     optimizer.setAlgorithm(solver);
 
@@ -58,7 +60,7 @@ void optimizeSingleFrame(
     g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap(); // camera pose
     pose->setId(0);
     pose->setEstimate(g2o::SE3Quat(
-        T_cam_to_world.rotation_matrix(),
+        T_cam_to_world.rotationMatrix(),
         T_cam_to_world.translation()));
 
     optimizer.addVertex(pose);
@@ -121,7 +123,7 @@ void optimizeSingleFrame(
     // -- Final: get the result from solver
 
     // 1. Camera pose
-    T_cam_to_world = Sophus::SE3(
+    T_cam_to_world = Sophus::SE3d(
         pose->estimate().rotation(),
         pose->estimate().translation());
     // Eigen::Matrix4d T_cam_to_world = Eigen::Isometry3d(pose->estimate()).matrix();
@@ -179,23 +181,23 @@ void bundleAdjustment(
     bool is_fix_map_pts, bool is_update_map_pts)
 {
 
-    // Change pose format from OpenCV to Sophus::SE3
+    // Change pose format from OpenCV to Sophus::SE3d
     int num_frames = v_camera_g2o_poses.size();
-    // vector<Sophus::SE3, aligned_allocator<Sophus::SE3>> v_T_cam_to_world;
-    vector<Sophus::SE3> v_T_cam_to_world;
+    // vector<Sophus::SE3d, aligned_allocator<Sophus::SE3d>> v_T_cam_to_world;
+    vector<Sophus::SE3d> v_T_cam_to_world;
     for (int i = 0; i < num_frames; i++)
     {
         v_T_cam_to_world.push_back(
             basics::transT_cv2sophus((*v_camera_g2o_poses[i]).inv()));
     }
 
     // Init g2o
-    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block; // dim(pose) = 6, dim(landmark) = 3
-    Block::LinearSolverType *linearSolver;
-    // linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // solver for linear equation
-    linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
-    Block *solver_ptr = new Block(linearSolver); // solver for matrix block
-    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+    std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linear_solver;
+    linear_solver = g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>>();
+    // use LM method to minimize nonlinear least square.
+    g2o::OptimizationAlgorithmLevenberg* solver =
+        new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linear_solver)));
+    solver->setUserLambdaInit(1);
     g2o::SparseOptimizer optimizer;
     optimizer.setAlgorithm(solver);
 
@@ -210,7 +212,7 @@ void bundleAdjustment(
         // if (num_frames > 1 && ith_frame == num_frames - 1)
         // pose->setFixed(true); // Fix the last one -- which is the earliest frame
         pose->setEstimate(g2o::SE3Quat(
-            v_T_cam_to_world[ith_frame].rotation_matrix(),
+            v_T_cam_to_world[ith_frame].rotationMatrix(),
             v_T_cam_to_world[ith_frame].translation()));
         optimizer.addVertex(pose);
         g2o_poses.push_back(pose);
@@ -297,7 +299,7 @@ void bundleAdjustment(
     // 1. Camera pose
     for (int i = 0; i < num_frames; i++)
     {
-        Sophus::SE3 T_cam_to_world = Sophus::SE3(
+        Sophus::SE3d T_cam_to_world = Sophus::SE3d(
             g2o_poses[i]->estimate().rotation(),
             g2o_poses[i]->estimate().translation());
         cv::Mat pose_src = basics::transT_sophus2cv(T_cam_to_world).inv(); // Change data format back to OpenCV
@@ -317,4 +319,4 @@ void bundleAdjustment(
 }
 
 } // namespace optimization
-} // namespace my_slam
+} // namespace my_slam