success loop closer

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<const Eigen::Vector3d> twc0( parameters[0] );
   Eigen::Map<const Eigen::Quaterniond> qwc0( parameters[0] + 3 );

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;

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;
 

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})

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;
 

vio_hw/internal/optimization.hpp

@@ -0,0 +1,42 @@
+#ifndef VIO_HELLO_WORLD_OPTIMIZATION_HPP
+#define VIO_HELLO_WORLD_OPTIMIZATION_HPP
+
+#include <glog/logging.h>
+
+#include <Eigen/Core>
+
+#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<Optimization> OptimizationPtr;
+typedef std::shared_ptr<const Optimization> OptimizationConstPtr;
+
+}  // namespace viohw
+
+#endif  // VIO_HELLO_WORLD_OPTIMIZATION_HPP

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<VisualizationBase> viz_;
   std::shared_ptr<CameraCalibration> calib_model_left_, calib_model_right_;

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 );
   }
 }
 

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<int, backend::PoseParametersBlock> map_id_poses_param_block;
+  std::map<int, std::shared_ptr<Frame>> 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

vio_hw/src/world_manager.cpp

@@ -37,7 +37,11 @@ WorldManager::WorldManager( std::shared_ptr<Setting>& 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() );