diff --git a/CMakeLists.txt b/CMakeLists.txt index a224d25..def691f 100644 --- a/CMakeLists.txt +++ b/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 -) \ No newline at end of file +) diff --git a/include/my_slam/basics/eigen_funcs.h b/include/my_slam/basics/eigen_funcs.h index 97146c2..b51a5d0 100644 --- a/include/my_slam/basics/eigen_funcs.h +++ b/include/my_slam/basics/eigen_funcs.h @@ -9,8 +9,8 @@ #include #include -#include -#include +#include +#include #include @@ -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 \ No newline at end of file +#endif diff --git a/src/basics/eigen_funcs.cpp b/src/basics/eigen_funcs.cpp index 35d9039..e66d3b7 100644 --- a/src/basics/eigen_funcs.cpp +++ b/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(0, 3), T_cv.at(1, 3), T_cv.at(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 \ No newline at end of file +} // namespace my_slam diff --git a/src/optimization/g2o_ba.cpp b/src/optimization/g2o_ba.cpp index 651a773..dafb766 100644 --- a/src/optimization/g2o_ba.cpp +++ b/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> Block; // dim(pose) = 6, dim(landmark) = 3 - Block::LinearSolverType *linearSolver = new g2o::LinearSolverCSparse(); // 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 linear_solver; + linear_solver = g2o::make_unique>(); + // use LM method to minimize nonlinear least square. + g2o::OptimizationAlgorithmLevenberg* solver = + new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique(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,10 +181,10 @@ 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> v_T_cam_to_world; - vector v_T_cam_to_world; + // vector> v_T_cam_to_world; + vector v_T_cam_to_world; for (int i = 0; i < num_frames; i++) { v_T_cam_to_world.push_back( @@ -190,12 +192,12 @@ void bundleAdjustment( } // Init g2o - typedef g2o::BlockSolver> Block; // dim(pose) = 6, dim(landmark) = 3 - Block::LinearSolverType *linearSolver; - // linearSolver = new g2o::LinearSolverCSparse(); // solver for linear equation - linearSolver = new g2o::LinearSolverDense(); - Block *solver_ptr = new Block(linearSolver); // solver for matrix block - g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); + std::unique_ptr linear_solver; + linear_solver = g2o::make_unique>(); + // use LM method to minimize nonlinear least square. + g2o::OptimizationAlgorithmLevenberg* solver = + new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique(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 \ No newline at end of file +} // namespace my_slam