The best possible situation 19.4.2021

svo/include/svo/imu_integration.h

@@ -11,13 +11,14 @@
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/ProjectionFactor.h>
+#include <gtsam/nonlinear/ISAM2.h>
 #include <sophus/se3.h>
 #include <svo/frame.h>
 
 using gtsam::symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
 using gtsam::symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
 using gtsam::symbol_shorthand::P;  // Pose3 (x,y,z,r,p,y)
-using gtsam::symbol_shorthand::F;  // Feature
+using gtsam::symbol_shorthand::C;  // Camera pose
 
 //https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop
 // Wrapper class
@@ -99,7 +100,7 @@ class Imu_Integration{
 public:
     Imu_Integration(Sophus::SE3& SE_init);
     ~Imu_Integration();
-    bool reset(gtsam::Values& result);
+    bool reset(gtsam::ISAM2& 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:
@@ -112,7 +113,8 @@ private:
     boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
     std::chrono::steady_clock::time_point t_1;
     std::uint32_t imu_factor_id=0;
-    std::uint64_t imu_stamp=0.0;
+    uint imu_n=0;
+    std::shared_ptr<gtsam::ISAM2Params> optimizerParamPtr;
     bool syn=false;
 };
 #endif //SVO_IMU_INTEGRATION_H

svo/include/svo/svoVisionFactor.h

@@ -53,9 +53,8 @@ namespace svo {
             gtsam::Matrix26 J;
             gtsam::Point3 point(p*landmark);
             svo::Frame::jacobian_xyz2uv(point, J);
-            if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J / (1<<level)).finished();
+            Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
-            Vector2d e = vk::project2d(feature) - vk::project2d(point);
+            if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J * 1.0 / (1<<level)).finished();
-            e *= 1.0 / (1<<level);
             return e;
         }
         // The second is a 'clone' function that allows the factor to be copied. Under most

svo/src/frame_handler_mono.cpp

@@ -66,9 +66,9 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
   overlap_kfs_.clear();
 
   // create new frame
-  SVO_START_TIMER("pyramid_creation");
+  //SVO_START_TIMER("pyramid_creation");
   new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
-  SVO_STOP_TIMER("pyramid_creation");
+  //SVO_STOP_TIMER("pyramid_creation");
 
   // process frame
   UpdateResult res = RESULT_FAILURE;
@@ -133,23 +133,15 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   new_frame_->T_f_w_ = last_frame_->T_f_w_;
 
   // sparse image align
-  SVO_START_TIMER("sparse_img_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_);
-  SVO_STOP_TIMER("sparse_img_align");
+  std::cout<<"Img Align:\t Tracked = "<<img_align_n_tracked<<'\n';
-  SVO_LOG(img_align_n_tracked);
-  SVO_DEBUG_STREAM("Img Align:\t Tracked = " << img_align_n_tracked);
 
   // map reprojection & feature alignment
-  SVO_START_TIMER("reproject");
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-  SVO_STOP_TIMER("reproject");
+  std::cout<<"Reprojection:\t nPoints = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<'\n';
-  const size_t repr_n_new_references = reprojector_.n_matches_;
+  if(reprojector_.n_matches_< Config::qualityMinFts())
-  const size_t repr_n_mps = reprojector_.n_trials_;
-  SVO_LOG2(repr_n_mps, repr_n_new_references);
-  SVO_DEBUG_STREAM("Reprojection:\t nPoints = "<<repr_n_mps<<"\t \t nMatches = "<<repr_n_new_references);
-  if(repr_n_new_references < Config::qualityMinFts())
   {
     SVO_WARN_STREAM_THROTTLE(1.0, "Not enough matched features.");
     new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
@@ -158,22 +150,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
 
   // pose optimization subject to change for adding IMU
-  SVO_START_TIMER("pose_optimizer");
   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';
-  double sfba_thresh, sfba_error_init, sfba_error_final;
+  //if(sfba_n_edges_final < 20)
-  pose_optimizer::optimizeGaussNewton(
+  //  return RESULT_FAILURE;
-      Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
-      new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
-  SVO_STOP_TIMER("pose_optimizer");
-  SVO_LOG4(sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
-  SVO_DEBUG_STREAM("PoseOptimizer:\t ErrInit = "<<sfba_error_init<<"px\t thresh = "<<sfba_thresh);
-  SVO_DEBUG_STREAM("PoseOptimizer:\t ErrFin. = "<<sfba_error_final<<"px\t nObsFin. = "<<sfba_n_edges_final);
-   */
-  if(sfba_n_edges_final < 20)
-    return RESULT_FAILURE;
 
   // structure optimization
   SVO_START_TIMER("point_optimizer");
@@ -183,6 +165,7 @@ 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

svo/src/imu_integration.cpp

@@ -9,28 +9,25 @@
 #include "svo/point.h"
 #include "svo/svoVisionFactor.h"
 #include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/geometry/Pose3.h>
 
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
-    statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(gtsam::Rot3(SE_init.rotation_matrix()),SE_init.translation()),
-                                               gtsam::Vector3(0.0,0.0,0.0));
-    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
-    valuesPtr=std::make_shared<gtsam::Values>();
-    valuesPtr->insert(P(0),statePtr->pose());
-    valuesPtr->insert(V(0),statePtr->v());
-    valuesPtr->insert(B(0),*imu_biasPtr);
     graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
-    graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.01, 0.01, 0.01, 0.01, 0.01, 0.01).finished()));
+    valuesPtr=std::make_shared<gtsam::Values>();
-    graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 0.01));
-    graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-4));
     parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
-    parameterPtr->accelerometerCovariance=gtsam::I_3x3 * 1e-3; // acc white noise in continuous
+    parameterPtr->accelerometerCovariance=gtsam::I_3x3 * 1e-4; // acc white noise in continuous
-    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * 1e-3; // gyro white noise in continuous
+    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * 1e-4; // gyro white noise in continuous
-    parameterPtr->integrationCovariance=gtsam::I_3x3 * 1e-8; // integration uncertainty continuous
+    parameterPtr->integrationCovariance=gtsam::I_3x3 * 1e-1; // integration uncertainty continuous
-    parameterPtr->biasAccCovariance = gtsam::I_3x3 * 1e-8;   // acc bias in continuous
+    parameterPtr->biasAccCovariance = gtsam::I_3x3 * 1e-1;   // acc bias in continuous
-    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * 1e-8;  // gyro bias in continuous
+    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * 1e-1;  // gyro bias in continuous
-    parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * 1e-5; // error in the bias used for preintegration
+    parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * 1e-2; // 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); // TODO change the noise used for projection
+    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 1e-8); // the noise used for projection
+    optimizerParamPtr=std::make_shared<gtsam::ISAM2Params>();
+    optimizerParamPtr->relinearizeThreshold = 0.1;
+    optimizerParamPtr->relinearizeSkip = 1;
+    optimizerParamPtr->evaluateNonlinearError=true;
     assert(preintegratedPtr);
 }
 Imu_Integration::~Imu_Integration(){
@@ -38,48 +35,66 @@ Imu_Integration::~Imu_Integration(){
 }
 bool Imu_Integration::update(double* imu){
     if(syn)return false;
-    if(imu[6]>imu_stamp){
+    if(imu_factor_id<1)return false;
-        auto in=std::chrono::steady_clock::now();
+    auto in=std::chrono::steady_clock::now();
-        if(std::chrono::duration<double>(in-t_1).count()>20.0){
+    if(std::chrono::duration<double>(in-t_1).count()>20.0){
-            t_1=in;
+        t_1=in;
-            imu_stamp=imu[6];
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005);
-        }else{
+    }else{
-            preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration<double>(in-t_1).count());
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration<double>(in-t_1).count());
-            t_1=in;
+        t_1=in;
-            imu_stamp=imu[6];
+        ++imu_n;
-        }
-        return true;
     }
-    return false;
+    return true;
 
 }
-bool Imu_Integration::reset(gtsam::Values& result){
+bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
+    gtsam::Values result = optimizer.calculateEstimate();
     // 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);
-    //graphPtr->resize(0);
+    ++imu_factor_id;
-    //valuesPtr->erase(P(imu_factor_id-1));
+    syn=false;
-    //valuesPtr->erase(V(imu_factor_id-1));
+    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
-    //valuesPtr->erase(B(imu_factor_id-1));
+    new_frame->Cov_=optimizer.marginalCovariance(P(imu_factor_id-1));
-    //t_1=std::chrono::steady_clock::now();
+    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(),optimizer.marginalCovariance(P(imu_factor_id-1)));
+    graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),optimizer.marginalCovariance(V(imu_factor_id-1)));
+    graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,optimizer.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){
-    // Adding IMU factor and and pre-integration.
+    usleep(5000);
     syn=true;
-    auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+    if(imu_factor_id<1){
-    gtsam::NavState Estimate_state = preintegratedPtr->predict(*statePtr, *imu_biasPtr);
+        statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(new_frame->T_f_w_.matrix()),
-    std::cout<<"--------------------------------------------------------------------------PreIntegration----------------------------------------------"<<'\n';
+                                                   gtsam::Vector3(1e-12,1e-12,1e-12));
-    //preintegratedPtr->print();
+        valuesPtr->insert(P(0),statePtr->pose());
-    ++imu_factor_id;
+        valuesPtr->insert(V(0),statePtr->v());
-    valuesPtr->insert(P(imu_factor_id), Estimate_state.pose());
+        valuesPtr->insert(B(0),*imu_biasPtr);
-    valuesPtr->insert(V(imu_factor_id), Estimate_state.v());
+        graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9).finished()));
-    valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);
+        graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
-    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),
+        graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2));
-                                 preint_imu_combined);
+    }else{
-    graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
+        if(imu_n<3)return false;
-    imu_factor.print();
+        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);
+        graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
+        imu_n=0;
+    }
+
     num_obs=0;
     for(auto&& f:new_frame->fts_){
        if(f->point==NULL)continue;
@@ -87,32 +102,25 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
         ++num_obs;
     }
     // Now optimize.
-    std::cout<<"--------------------------------------------------------------------------graph----------------------------------------------"<<'\n';
+    gtsam::ISAM2 optimizer(*optimizerParamPtr);
-    //graphPtr->print();
+    optimizer.update(*graphPtr, *valuesPtr);
-    std::cout<<"--------------------------------------------------------------------------values----------------------------------------------"<<'\n';
+    // Each call to iSAM2 update(*) performs one iteration of the iterative
-    valuesPtr->print();
+    // nonlinear solver. If accuracy is desired at the expense of time,
-    gtsam::LevenbergMarquardtParams params;
+    // update(*) can be called additional times to perform multiple optimizer
-    params.setVerbosityLM("SUMMARY");
+    // iterations every step.
-    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr, params);
+    optimizer.update();
-    gtsam::Values result = optimizer.optimize();
+   // optimizer.print();
-    gtsam::Marginals marginals(*graphPtr, result);
+    reset(optimizer,new_frame);
-    std::cout<<"--------------------------------------------------------------------------marginals----------------------------------------------"<<'\n';
+    // Remove Measurements with too large reprojection error
-    //marginals.print();
-    reset(result);
-    syn=false;
-    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
-    new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id));
-    std::cout<<new_frame->Cov_<<'\n';
     double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2();
     for(auto&& f:new_frame->fts_)
     {
         if(f->point == NULL)
             continue;
-        Eigen::Vector2d e = vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_);
+        Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<<f->level);
-        double sqrt_inv_cov = 1.0 / (1<<f->level);
-        e *= sqrt_inv_cov;
         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/launch/Px30.launch

@@ -9,9 +9,6 @@
     <!-- Camera calibration file -->
     <rosparam file="$(find vio_svo)/param/px30.yaml" />
 
-    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
-    <rosparam file="$(find vio_svo)/param/vo_accurate.yaml" />
-    
     <!-- Initial camera orientation, make it point downwards -->
     <param name="init_rx" value="3.14" />
     <param name="init_ry" value="0.00" />
@@ -19,4 +16,7 @@
 
   </node>
 
+  <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+  <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
+
 </launch>

vio_svo/param/vo_fast.yaml

@@ -1,3 +1,6 @@
-grid_size: 30
+svo:
-max_n_kfs: 10
+  grid_size: 30
-loba_num_iter: 0
+  max_n_kfs: 10
+  loba_num_iter: 0
+  quality_min_fts: 20
+  quality_max_drop_fts: 50

vio_svo/src/vo_node.cpp

@@ -108,14 +108,13 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
       }
 }
 void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
-    double imu_in[7];
+    double imu_in[6];
     imu_in[0] = imu->linear_acceleration.x;
     imu_in[1] = imu->linear_acceleration.y;
     imu_in[2] = imu->linear_acceleration.z;
     imu_in[3] = imu->angular_velocity.x;
     imu_in[4] = imu->angular_velocity.y;
     imu_in[5] = imu->angular_velocity.z;
-    imu_in[6] = imu->header.stamp.toNSec();
     vo_->imu_integPtr_->update(imu_in);
 }
 void VoNode::imu(){
@@ -123,7 +122,7 @@ void VoNode::imu(){
     std::cout << "create vo_node" << std::endl;
     // subscribe to cam msgs
     image_transport::ImageTransport it(nh);
-    image_transport::Subscriber it_sub = it.subscribe(vk::getParam<std::string>("vio/cam_topic", "camera/image_raw"), 5, &svo::VoNode::imgCb, this);
+    image_transport::Subscriber it_sub = it.subscribe(vk::getParam<std::string>("vio/cam_topic", "camera/image_raw"), 1, &svo::VoNode::imgCb, this);
     // start processing callbacks
     while(ros::ok() && !quit_)
     {