The best possible situation 20.4.2021

svo/include/svo/config.h

@@ -119,6 +119,23 @@ public:
   /// If within one frame, this amount of features are dropped. Tracking quality is bad.
   static int& qualityMaxFtsDrop() { return getInstance().quality_max_drop_fts; }
 
+  /// acc white noise in continuous.
+  static double& ACC_Noise() { return getInstance().ACC_noise; }
+
+  /// acc white noise in continuous.
+  static double& GYO_Noise() { return getInstance().GYO_noise; }
+  /// integration uncertainty continuous
+  static double& IUC() { return getInstance().iuc; }
+    // acc bias in continuous
+  static double& ABC() { return getInstance().abc; }
+    // gyro bias in continuous
+  static double& GBC() { return getInstance().gbc; }
+    // error in the bias used for preintegration
+  static double& EBP() { return getInstance().ebp; }
+    // the noise used for projection
+  static double& NRP() { return getInstance().nrp; }
+
+
 private:
   Config();
   Config(Config const&);
@@ -152,6 +169,7 @@ private:
   size_t max_fts;
   size_t quality_min_fts;
   int quality_max_drop_fts;
+  double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise;
 };
 
 } // namespace svo

svo/include/svo/imu_integration.h

@@ -101,6 +101,7 @@ public:
     Imu_Integration(Sophus::SE3& SE_init);
     ~Imu_Integration();
     bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
+    bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
     bool update(double* imu= nullptr);
     bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&,const double reproj_thresh);
 private:

svo/src/config.cpp

@@ -51,7 +51,14 @@ Config::Config() :
     img_imu_delay(vk::getParam<double>("svo/img_imu_delay", 0.0)),
     max_fts(vk::getParam<int>("svo/max_fts", 120)),
     quality_min_fts(vk::getParam<int>("svo/quality_min_fts", 50)),
-    quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40))
+    quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40)),
+    ACC_noise(vk::getParam<double>("svo/ACC_noise", 1e-4)),
+    GYO_noise(vk::getParam<double>("svo/GYO_noise", 1e-4)),
+    iuc(vk::getParam<double>("svo/IUC", 1e-1)),
+    abc(vk::getParam<double>("svo/ABC", 1e-1)),
+    gbc(vk::getParam<double>("svo/GBC", 1e-1)),
+    ebp(vk::getParam<double>("svo/EBP", 1e-2)),
+    nrp(vk::getParam<double>("svo/NRP", 1e-5))
 #else
     trace_name("svo"),
     trace_dir("/tmp"),

svo/src/frame_handler_mono.cpp

@@ -66,9 +66,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
   overlap_kfs_.clear();
 
   // create new frame
-  //SVO_START_TIMER("pyramid_creation");
   new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
-  //SVO_STOP_TIMER("pyramid_creation");
 
   // process frame
   UpdateResult res = RESULT_FAILURE;
@@ -135,12 +133,10 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   // sparse image align
   SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
                            30, SparseImgAlign::GaussNewton, false, false);
-  size_t img_align_n_tracked = img_align.run(last_frame_, new_frame_);
+  img_align.run(last_frame_, new_frame_);
-  std::cout<<"Img Align:\t Tracked = "<<img_align_n_tracked<<'\n';
-
   // map reprojection & feature alignment
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-  std::cout<<"Reprojection:\t nPoints = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<'\n';
+  std::cerr<<"Reprojection Map:\t nPoints = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<'\n';
   if(reprojector_.n_matches_< Config::qualityMinFts())
   {
     SVO_WARN_STREAM_THROTTLE(1.0, "Not enough matched features.");
@@ -153,9 +149,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   size_t sfba_n_edges_final;
   if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh()))
         return RESULT_FAILURE;
-  std::cout<<"reprojected point after opmization: "<<sfba_n_edges_final<<'\n';
+  std::cerr<<"Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
-  //if(sfba_n_edges_final < 20)
+  if(sfba_n_edges_final < Config::qualityMinFts())
-  //  return RESULT_FAILURE;
+    return RESULT_FAILURE;
 
   // structure optimization
   SVO_START_TIMER("point_optimizer");
@@ -165,7 +161,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   // select keyframe
   core_kfs_.insert(new_frame_);
   setTrackingQuality(sfba_n_edges_final);
-  std::cout<<"teracking quality: "<<tracking_quality_<<'\n';
   if(tracking_quality_ == TRACKING_INSUFFICIENT)
   {
     new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
@@ -179,7 +174,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     return RESULT_NO_KEYFRAME;
   }
   new_frame_->setKeyframe();
-  SVO_DEBUG_STREAM("New keyframe selected.");
 
   // new keyframe selected
   for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it)
@@ -218,7 +212,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 
   // add keyframe to map
   map_.addKeyframe(new_frame_);
-
+  std::cerr<<"Svo pipline was finished\n";
   return RESULT_IS_KEYFRAME;
 }
 

svo/src/imu_integration.cpp

@@ -10,24 +10,26 @@
 #include "svo/svoVisionFactor.h"
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/Pose3.h>
+#include <vikit/params_helper.h>
+#include <svo/config.h>
 
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
     valuesPtr=std::make_shared<gtsam::Values>();
     parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
-    parameterPtr->accelerometerCovariance=gtsam::I_3x3 * 1e-4; // acc white noise in continuous
+    parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
-    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * 1e-4; // gyro white noise in continuous
+    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
-    parameterPtr->integrationCovariance=gtsam::I_3x3 * 1e-1; // integration uncertainty continuous
+    parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
-    parameterPtr->biasAccCovariance = gtsam::I_3x3 * 1e-1;   // acc bias in continuous
+    parameterPtr->biasAccCovariance = gtsam::I_3x3 * svo::Config::ABC();   // acc bias in continuous
-    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * 1e-1;  // gyro bias in continuous
+    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * svo::Config::GBC();  // gyro bias in continuous
-    parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * 1e-2; // error in the bias used for preintegration
+    parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * svo::Config::EBP();  // error in the bias used for preintegration
     imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
     preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(parameterPtr,*imu_biasPtr);
-    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 1e-8); // the noise used for projection
+    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection
     optimizerParamPtr=std::make_shared<gtsam::ISAM2Params>();
     optimizerParamPtr->relinearizeThreshold = 0.1;
     optimizerParamPtr->relinearizeSkip = 1;
-    optimizerParamPtr->evaluateNonlinearError=true;
+    //optimizerParamPtr->evaluateNonlinearError=true;
     assert(preintegratedPtr);
 }
 Imu_Integration::~Imu_Integration(){
@@ -68,6 +70,27 @@ bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame
     graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,optimizer.marginalCovariance(B(imu_factor_id-1)));
     return true;
 }
+bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
+    gtsam::Values result = optimizer.optimize();
+    // Overwrite the beginning of the preintegration for the next step.
+    statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
+    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(B(imu_factor_id)));
+    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
+    ++imu_factor_id;
+    syn=false;
+    gtsam::Marginals marginals(*graphPtr,result);
+    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
+    new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1));
+    graphPtr->resize(0);
+    valuesPtr->clear();
+    valuesPtr->insert(P(imu_factor_id-1),statePtr->pose());
+    valuesPtr->insert(V(imu_factor_id-1),statePtr->v());
+    valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr);
+    graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),marginals.marginalCovariance(P(imu_factor_id-1)));
+    graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1)));
+    graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1)));
+    return true;
+}
 bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size_t& num_obs,const double reproj_thresh){
     usleep(5000);
     syn=true;
@@ -83,11 +106,8 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
     }else{
         if(imu_n<3)return false;
         auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
-        //gtsam::NavState Estimate_state = preintegratedPtr->predict(*statePtr, *imu_biasPtr);
         valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix()));
-        //valuesPtr->insert(P(imu_factor_id), Estimate_state.pose());
         valuesPtr->insert(V(imu_factor_id), statePtr->v());
-        //valuesPtr->insert(V(imu_factor_id), Estimate_state.v());
         valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);
         gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
                                             preint_imu_combined);
@@ -102,14 +122,16 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
         ++num_obs;
     }
     // Now optimize.
-    gtsam::ISAM2 optimizer(*optimizerParamPtr);
+    //gtsam::ISAM2 optimizer(*optimizerParamPtr);
-    optimizer.update(*graphPtr, *valuesPtr);
+    //optimizer.update(*graphPtr, *valuesPtr);
     // Each call to iSAM2 update(*) performs one iteration of the iterative
     // nonlinear solver. If accuracy is desired at the expense of time,
     // update(*) can be called additional times to perform multiple optimizer
     // iterations every step.
-    optimizer.update();
+    //optimizer.update();
    // optimizer.print();
+    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr);
+
     reset(optimizer,new_frame);
     // Remove Measurements with too large reprojection error
     double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2();
@@ -120,7 +142,6 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
         Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<<f->level);
         if(e.norm() > reproj_thresh_scaled)
         {
-            std::cout<<"Error: "<<e.norm()<<'\n';
             // we don't need to delete a reference in the point since it was not created yet
             f->point = NULL;
             --num_obs;

vio_svo/param/vo_fast.yaml

@@ -1,6 +1,13 @@
 svo:
-  grid_size: 30
+  grid_size: 30            #Feature grid size of a cell in [px].
-  max_n_kfs: 10
+  max_n_kfs: 10            #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed.  Minimum number of keyframes is 3.
-  loba_num_iter: 0
+  loba_num_iter: 0         #Number of iterations in the local bundle adjustment.
-  quality_min_fts: 20
+  quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
-  quality_max_drop_fts: 50
+  quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad.
+  ACC_noise: 0.0001          #acc white noise in continuous
+  GYO_noise: 0.0001          #gyro white noise in continuous
+  IUC: 0.1                #integration uncertainty continuous
+  ABC: 0.1                #acc bias in continuous
+  GBC: 0.1                #gyro bias in continuous
+  EBP: 0.01                #error in the bias used for preintegration
+  NRP: 0.0000001                #noise used for projection in preintegration