diff --git a/backend/cost_function/impl/se3_left_pgo_factor_impl.hpp b/backend/cost_function/impl/se3_left_pgo_factor_impl.hpp index ff0e301..6d5bac6 100644 --- a/backend/cost_function/impl/se3_left_pgo_factor_impl.hpp +++ b/backend/cost_function/impl/se3_left_pgo_factor_impl.hpp @@ -1,7 +1,7 @@ #pragma once namespace backend { -bool LeftSE3RelativePoseError::Evaluate( double const* const* parameters, double* residuals, - double** jacobians ) const { +inline bool LeftSE3RelativePoseError::Evaluate( double const* const* parameters, double* residuals, + double** jacobians ) const { // [tx, ty, tz, qw, qx, qy, qz] Eigen::Map twc0( parameters[0] ); Eigen::Map qwc0( parameters[0] + 3 ); diff --git a/backend/example/common/read_g2o_format.hpp b/backend/example/common/read_g2o_format.hpp index b18238e..fa4ddc2 100644 --- a/backend/example/common/read_g2o_format.hpp +++ b/backend/example/common/read_g2o_format.hpp @@ -169,7 +169,8 @@ inline bool OutputPoses( const std::string& filename, auto pair = pose; Eigen::Vector3d p = pair.second.getPose().translation(); Eigen::Quaterniond q = pair.second.getPose().unit_quaternion(); - outfile << pair.first << " " << p.transpose() << " " << q.x() << " " << q.y() << " " << q.z() + outfile << pair.first << " " << p.transpose().x() << " " << p.transpose().y() << " " << p.transpose().z() + << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << '\n'; } return true; diff --git a/backend/example/common/type.hpp b/backend/example/common/type.hpp index 8013dc2..d6f080f 100644 --- a/backend/example/common/type.hpp +++ b/backend/example/common/type.hpp @@ -52,8 +52,7 @@ struct Pose3d EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - -std::istream& operator>>( std::istream& input, Pose3d& pose ) { +inline std::istream& operator>>( std::istream& input, Pose3d& pose ) { input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >> pose.q.y() >> pose.q.z() >> pose.q.w(); // Normalize the quaternion to account for precision loss due to @@ -87,8 +86,7 @@ struct Constraint3d EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - -std::istream& operator>>( std::istream& input, Constraint3d& constraint ) { +inline std::istream& operator>>( std::istream& input, Constraint3d& constraint ) { Pose3d& t_be = constraint.t_be; input >> constraint.id_begin >> constraint.id_end >> t_be; diff --git a/vio_hw/CMakeLists.txt b/vio_hw/CMakeLists.txt index c7fc9ca..5fe52f7 100644 --- a/vio_hw/CMakeLists.txt +++ b/vio_hw/CMakeLists.txt @@ -38,6 +38,7 @@ set(SOURCE_FILES src/tracker/optical_flow_impl.cpp src/mapping.cpp src/loop_closer.cpp + src/optimization.cpp ) add_library(vio_hw ${SOURCE_FILES}) diff --git a/vio_hw/internal/loop_closer.hpp b/vio_hw/internal/loop_closer.hpp index 834b123..028da5b 100644 --- a/vio_hw/internal/loop_closer.hpp +++ b/vio_hw/internal/loop_closer.hpp @@ -10,6 +10,7 @@ #include "geometry/epipolar/epipolar_constraint.hpp" #include "geometry/motion_ba/motion_ba.hpp" #include "vio_hw/internal/map_manager.hpp" +#include "vio_hw/internal/optimization.hpp" #include "vio_hw/internal/setting.hpp" namespace viohw { @@ -26,7 +27,7 @@ class LoopCloser ~LoopCloser() = default; // construction function - LoopCloser( SettingPtr param, MapManagerPtr map_manager ); + LoopCloser( SettingPtr param, MapManagerPtr map_manager, OptimizationPtr optimization ); // loop closer thread void run(); @@ -71,6 +72,7 @@ class LoopCloser FramePtr new_kf_; SettingPtr param_; MapManagerPtr map_manager_; + OptimizationPtr optimization_; bool is_new_kf_available_ = false; diff --git a/vio_hw/internal/optimization.hpp b/vio_hw/internal/optimization.hpp new file mode 100644 index 0000000..bb890be --- /dev/null +++ b/vio_hw/internal/optimization.hpp @@ -0,0 +1,42 @@ +#ifndef VIO_HELLO_WORLD_OPTIMIZATION_HPP +#define VIO_HELLO_WORLD_OPTIMIZATION_HPP + +#include + +#include + +#include "backend/cost_function/se3_left_pgo_factor.hpp" +#include "backend/parameter_block/se3_parameter_block.hpp" +#include "backend/parameterization/se3left_parametrization.hpp" +#include "vio_hw/internal/map_manager.hpp" +#include "vio_hw/internal/setting.hpp" +#include "tceres/problem.h" +#include "tceres/solver.h" +#include "backend/example/common/read_g2o_format.hpp" + +namespace viohw { + +class Optimization +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Optimization() = delete; + + ~Optimization() = default; + + Optimization( SettingPtr param, MapManagerPtr map_manager ); + + bool LocalPoseGraph( Frame &new_frame, int kf_loop_id, const Sophus::SE3d &new_Twc ); + +private: + SettingPtr param_; + MapManagerPtr map_manager_; +}; + +typedef std::shared_ptr OptimizationPtr; +typedef std::shared_ptr OptimizationConstPtr; + +} // namespace viohw + +#endif // VIO_HELLO_WORLD_OPTIMIZATION_HPP diff --git a/vio_hw/internal/world_manager.hpp b/vio_hw/internal/world_manager.hpp index 4595595..fdd1420 100644 --- a/vio_hw/internal/world_manager.hpp +++ b/vio_hw/internal/world_manager.hpp @@ -41,6 +41,7 @@ class WorldManager VisualFrontEndPtr visual_frontend_; MappingPtr mapping_; LoopCloserPtr loop_closer_; + OptimizationPtr optimization_; std::shared_ptr viz_; std::shared_ptr calib_model_left_, calib_model_right_; diff --git a/vio_hw/src/loop_closer.cpp b/vio_hw/src/loop_closer.cpp index 3e25d01..104c1be 100644 --- a/vio_hw/src/loop_closer.cpp +++ b/vio_hw/src/loop_closer.cpp @@ -2,8 +2,8 @@ namespace viohw { -LoopCloser::LoopCloser( SettingPtr param, MapManagerPtr map_manager ) - : param_( param ), map_manager_( map_manager ) { +LoopCloser::LoopCloser( SettingPtr param, MapManagerPtr map_manager, OptimizationPtr optimization ) + : param_( param ), map_manager_( map_manager ), optimization_( optimization ) { use_loop_ = param->loop_setting_.use_loop_closer_; loop_threshold_ = param->loop_setting_.loop_threshold_; @@ -115,6 +115,7 @@ void LoopCloser::ProcessLoopCandidate( int kf_loop_id ) { LOG( INFO ) << "loop kf pos: " << lc_kf->GetTwc().translation().transpose() << "; " << "new kf pos: " << new_kf_->GetTwc().translation().transpose() << "; " << "correct pos: " << Twc.translation().transpose(); + optimization_->LocalPoseGraph( *new_kf_, lc_kf->kfid_, Twc ); } } diff --git a/vio_hw/src/optimization.cpp b/vio_hw/src/optimization.cpp new file mode 100644 index 0000000..2379f11 --- /dev/null +++ b/vio_hw/src/optimization.cpp @@ -0,0 +1,79 @@ +#include "vio_hw/internal/optimization.hpp" + +namespace viohw { +viohw::Optimization::Optimization( viohw::SettingPtr param, viohw::MapManagerPtr map_manager ) + : map_manager_( map_manager ), param_( param ) {} + +bool Optimization::LocalPoseGraph( Frame& new_frame, int kf_loop_id, const Sophus::SE3d& new_Twc ) { + tceres::Problem problem; + + std::map map_id_poses_param_block; + std::map> map_kfs; + + Sophus::SE3d init_Twc = new_frame.GetTwc(); + auto loop_kf = map_manager_->GetKeyframe( kf_loop_id ); + CHECK( loop_kf != nullptr ) << "Loop KF empty"; + + // add loop kf pose to problem and set constant + Sophus::SE3d T_loop_wc = loop_kf->GetTwc(); + map_id_poses_param_block.emplace( kf_loop_id, + backend::PoseParametersBlock( kf_loop_id, T_loop_wc ) ); + auto* local_pose_parameterization = new backend::SE3LeftParameterization; + problem.AddParameterBlock( map_id_poses_param_block.at( kf_loop_id ).values(), + backend::PoseParametersBlock::ndim_, local_pose_parameterization ); + problem.SetParameterBlockConstant( map_id_poses_param_block.at( kf_loop_id ).values() ); + + Sophus::SE3d T_ci_w = loop_kf->GetTcw(); + int ci_kf_id = loop_kf->kfid_; + Sophus::SE3d T_loopkf_newkf = T_ci_w * new_Twc; + + // correct loopKF -> currKF pose + for ( int kfid = kf_loop_id + 1; kfid <= new_frame.kfid_; kfid++ ) { + auto kf = map_manager_->GetKeyframe( kfid ); + + if ( kf == nullptr ) { + if ( kfid == new_frame.kfid_ ) { + return false; + } else { + continue; + } + } + + map_kfs.emplace( kfid, kf ); + Sophus::SE3d T_w_cj = kf->GetTwc(); + + map_id_poses_param_block.emplace( kfid, backend::PoseParametersBlock( kfid, T_w_cj ) ); + auto* local_parameterization = new backend::SE3LeftParameterization; + problem.AddParameterBlock( map_id_poses_param_block.at( kfid ).values(), + backend::PoseParametersBlock::ndim_, local_parameterization ); + Sophus::SE3d T_ij = T_ci_w * T_w_cj; + tceres::CostFunction* cost_function = new backend::LeftSE3RelativePoseError( T_ij ); + problem.AddResidualBlock( cost_function, nullptr, + map_id_poses_param_block.at( ci_kf_id ).values(), + map_id_poses_param_block.at( kfid ).values() ); + T_ci_w = T_w_cj.inverse(); + ci_kf_id = kfid; + } + + tceres::CostFunction* cost_fun = new backend::LeftSE3RelativePoseError( T_loopkf_newkf ); + problem.AddResidualBlock( cost_fun, nullptr, map_id_poses_param_block.at( kf_loop_id ).values(), + map_id_poses_param_block.at( new_frame.kfid_ ).values() ); + + backend::examples::OutputPoses("./initial_poses.txt", map_id_poses_param_block); + + tceres::Solver::Options options; + options.linear_solver_type = tceres::SPARSE_NORMAL_CHOLESKY; + options.trust_region_strategy_type = tceres::LEVENBERG_MARQUARDT; + options.max_num_iterations = 10; + options.function_tolerance = 1.e-4; + + tceres::Solver::Summary summary; + tceres::Solve( options, &problem, &summary ); + LOG( INFO ) << summary.FullReport(); + + backend::examples::OutputPoses("./optimization_poses.txt", map_id_poses_param_block); + + return true; +} + +} // namespace viohw diff --git a/vio_hw/src/world_manager.cpp b/vio_hw/src/world_manager.cpp index 073cbdd..57ba2fb 100644 --- a/vio_hw/src/world_manager.cpp +++ b/vio_hw/src/world_manager.cpp @@ -37,7 +37,11 @@ WorldManager::WorldManager( std::shared_ptr& setting ) : params_( setti visual_frontend_.reset( new VisualFrontEnd( params_, current_frame_, map_manager_, tracker_, viz_ ) ); - loop_closer_.reset( new LoopCloser( params_, map_manager_ ) ); + // create optimization + optimization_.reset( new Optimization( params_, map_manager_ ) ); + + // create loop closer + loop_closer_.reset( new LoopCloser( params_, map_manager_, optimization_ ) ); // create mapping thread, and mapping will create sub thread for Estimator and LoopClosing mapping_.reset( new Mapping( params_, map_manager_, current_frame_, loop_closer_ ) ); @@ -59,7 +63,6 @@ void WorldManager::run() { VisualizationImage(); - if ( is_kf ) { Keyframe kf( current_frame_->kfid_, img_left, img_right, visual_frontend_->GetCurrentFramePyramid() );