calibrate imu

GPU_version/svo/include/gpu_svo/depth_filter.h

@@ -138,7 +138,6 @@ public:
 protected:
   feature_detection::DetectorPtr feature_detector_;
   callback_t seed_converged_cb_;
-  //std::list<Seed> seeds_;
   std::list<Seed, Eigen::aligned_allocator<Seed>> seeds_;
   boost::mutex seeds_mut_;
   bool seeds_updating_halt_;            //!< Set this value to true when seeds updating should be interrupted.
@@ -150,8 +149,8 @@ protected:
   bool new_keyframe_set_;               //!< Do we have a new keyframe to process?.
   double new_keyframe_min_depth_;       //!< Minimum depth in the new keyframe. Used for range in new seeds.
   double new_keyframe_mean_depth_;      //!< Maximum depth in the new keyframe. Used for range in new seeds.
-  //vk::PerformanceMonitor permon_;       //!< Separate performance monitor since the DepthFilter runs in a parallel thread.
   Matcher matcher_;
+  uint seeds_cont_=0;
 
   /// Initialize new seeds from a frame.
   void initializeSeeds(FramePtr frame);

GPU_version/svo/include/gpu_svo/imu_integration.h

@@ -24,13 +24,13 @@ class Imu_Integration{
 public:
     Imu_Integration(Sophus::SE3& SE_init);
     ~Imu_Integration();
-    bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
+    void reset();
     bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
     bool update(double* imu= nullptr);
     bool predict(boost::shared_ptr<svo::Frame>&,
-                 boost::shared_ptr<svo::Frame>& last_frame ,std::size_t&,const double reproj_thresh);
+                 boost::shared_ptr<svo::Frame>& last_frame ,const double match);
     bool init(boost::shared_ptr<svo::Frame>&);
-    Sophus::SE3 preintegrate_predict();
+    bool preintegrate_predict(svo::SE2_5& frame);
 private:
     FILE* logtime;
     std::shared_ptr<gtsam::NavState> statePtr;

GPU_version/svo/src/depth_filter.cpp

@@ -92,14 +92,17 @@ void DepthFilter::addFrame(FramePtr frame)
     seeds_updating_halt_ = false;
     frame_queue_cond_.notify_one();
   }
-  else
-    updateSeeds(frame);
+  else{
+      updateSeeds(frame);
+  }
+
 }
 
 void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_min)
 {
   new_keyframe_min_depth_ = depth_min;
   new_keyframe_mean_depth_ = depth_mean;
+  ++seeds_cont_;
   if(thread_ != NULL)
   {
     new_keyframe_ = frame;
@@ -108,7 +111,10 @@ void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_mi
     frame_queue_cond_.notify_one();
   }
   else
-    initializeSeeds(frame);
+    if(seeds_cont_>5){
+        initializeSeeds(frame);
+        seeds_cont_=0;
+    }
 }
 
 void DepthFilter::initializeSeeds(FramePtr frame)
@@ -178,7 +184,9 @@ void DepthFilter::updateSeedsLoop()
       if(new_keyframe_set_)
       {
         new_keyframe_set_ = false;
-        seeds_updating_halt_ = false;
+        if(seeds_cont_>5){
+            seeds_updating_halt_ = false;
+        }
         clearFrameQueue();
         frame = new_keyframe_;
       }
@@ -189,8 +197,12 @@ void DepthFilter::updateSeedsLoop()
       }
     }
     updateSeeds(frame);
-    if(frame->isKeyframe())
-      initializeSeeds(frame);
+
+    if(frame->isKeyframe() && seeds_cont_>5){
+        initializeSeeds(frame);
+        seeds_cont_=0;
+    }
+
   }
 }
 
@@ -233,6 +245,10 @@ void DepthFilter::updateSeeds(FramePtr frame)
     // we are using inverse depth coordinates
     float z_inv_min = it->mu + sqrt(it->sigma2);
     float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
+    if(z_inv_min <0){
+        ++it;
+        continue;
+    }
     double z;
     if(!matcher_.findEpipolarMatchDirect(
         *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z))
@@ -264,20 +280,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
       Vector3d xyz_world(it->ftr->frame->T_f_w_.T->inverse() * (it->ftr->f * (1.0/it->mu)));
       Point* point = new Point(xyz_world, it->ftr);
       it->ftr->point = point;
-      /* FIXME it is not threadsafe to add a feature to the frame here.
-      if(frame->isKeyframe())
-      {
-        Feature* ftr = new Feature(frame.get(), matcher_.px_cur_, matcher_.search_level_);
-        ftr->point = point;
-        point->addFrameRef(ftr);
-        frame->addFeature(ftr);
-        it->ftr->frame->addFeature(it->ftr);
-      }
-      else
-      */
-      {
-        seed_converged_cb_(point, it->sigma2); // put in candidate list
-      }
+      seed_converged_cb_(point, it->sigma2); // put in candidate list
       it = seeds_.erase(it);
     }
     else if(isnan(z_inv_min))

GPU_version/svo/src/feature_alignment.cpp

@@ -203,7 +203,7 @@ bool align2D(
   bool sign=true;// +
 
   // termination condition
-  const float min_update_squared = 0.05;
+  const float min_update_squared = 0.3;
   const int cur_step = cur_img.step.p[0];
 //  float chi2 = 0;
   Vector3f update; update.setZero();

GPU_version/svo/src/frame_handler_mono.cpp

@@ -136,14 +136,15 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
 
 FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 {
-  SE3 tem=imu_integPtr_->preintegrate_predict();
-  if((new_frame_->T_f_w_.T->translation()-tem.translation()).norm()>0.5){
-      new_frame_->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),atan(tem.rotation_matrix()(0,2)/tem.rotation_matrix()(0,0)));
+  bool IMU=false;
+  SE2_5 tem(0.0,0.0,0.0);
+  if(imu_integPtr_->preintegrate_predict(tem))if((last_frame_->T_f_w_.T->translation()-tem.T->translation()).norm()>0.5){
+      new_frame_->T_f_w_ = tem;
+      IMU=true;
   }else{
+      imu_integPtr_->reset();
       new_frame_->T_f_w_ = last_frame_->T_f_w_;
   }
-  new_frame_->T_f_w_.set();
-  //new_frame_->T_f_w_ = last_frame_->T_f_w_;
   // sparse image align
   SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
                            30, SparseImgAlign::LevenbergMarquardt, false, false);
@@ -151,30 +152,32 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   new_frame_->T_f_w_.set();
   // map reprojection & feature alignment
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-  size_t sfba_n_edges_final=0;
   new_frame_->T_f_w_.set();
-  double sfba_thresh, sfba_error_init, sfba_error_final;
-  if(!imu_integPtr_->predict(new_frame_,last_frame_,sfba_n_edges_final,reprojector_.n_matches_)){
-        return RESULT_FAILURE;
+  if(IMU)if(!imu_integPtr_->predict(new_frame_,last_frame_,reprojector_.n_matches_)){
+            return RESULT_FAILURE;
   }
+  size_t sfba_n_edges_final=0;
+  double sfba_thresh, sfba_error_init, sfba_error_final;
   new_frame_->T_f_w_.set();
   pose_optimizer::optimizeGaussNewton(
             Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
             new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
 
   new_frame_->T_f_w_.set();
-  std::cerr<<"Reprojection Map: "<<"\t nPoint:"<<overlap_kfs_.back().second
+
+  std::cout<<"Reprojection Map: "<<"\t nPoint:"<<overlap_kfs_.back().second
   <<"\t nCell = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_
   <<" \t Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
 
+  if((last_frame_->T_f_w_.T->translation()-new_frame_->T_f_w_.T->translation()).norm()>1.0){
+      new_frame_->T_f_w_ = last_frame_->T_f_w_;
+      return RESULT_NO_KEYFRAME;
+  }
   if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
       return RESULT_NO_KEYFRAME;
   }
 
-  // structure optimization
-//  SVO_START_TIMER("point_optimizer");
   optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
-  //SVO_STOP_TIMER("point_optimizer");
 
   new_frame_->T_f_w_.set();
   // select keyframe
@@ -188,8 +191,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 
   new_frame_->T_f_w_.set();
   frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
+  if(depth_mean > 50 || depth_mean < 0.0)
+  {
+      return RESULT_NO_KEYFRAME;
+  }
 
-  new_frame_->T_f_w_.set();
   if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited
   {
     depth_filter_->addFrame(new_frame_);
@@ -197,14 +203,13 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
   new_frame_->setKeyframe();
 
-    new_frame_->T_f_w_.set();
+  new_frame_->T_f_w_.set();
   // new keyframe selected
   for(auto&& it:new_frame_->fts_){
       if(it->point != NULL)it->point->addFrameRef(it);
   }
   map_.point_candidates_.addCandidatePointToFrame(new_frame_);
 
-    new_frame_->T_f_w_.set();
   // init new depth-filters
   depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
 
@@ -219,7 +224,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     new_frame_->T_f_w_.set();
   // add keyframe to map
   map_.addKeyframe(new_frame_);
-  std::cerr<<"Svo pipline was finished\n";
+  std::cout<<"Svo pipline was finished\n";
   return RESULT_IS_KEYFRAME;
 }
 
@@ -294,10 +299,10 @@ bool FrameHandlerMono::needNewKf(double scene_depth_mean)
   for(auto&& it:overlap_kfs_)
   {
     Vector3d relpos = new_frame_->w2f(it.first->pos());
-    if(fabs(relpos.x())/scene_depth_mean < Config::kfSelectMinDist() &&
-       fabs(relpos.y())/scene_depth_mean < Config::kfSelectMinDist() &&
-       fabs(relpos.z())/scene_depth_mean < Config::kfSelectMinDist())
-      return false;
+    if(fabs(relpos.norm())/scene_depth_mean < Config::kfSelectMinDist()){
+        return false;
+    }
+
   }
   return true;
 }

GPU_version/svo/src/imu_integration.cpp

@@ -15,7 +15,7 @@
 #include <vikit/params_helper.h>
 #include <gpu_svo/config.h>
 #include <gpu_svo/for_it.hpp>
-#define debug 0
+#define debug 1
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
     valuesPtr=std::make_shared<gtsam::Values>();
@@ -49,10 +49,12 @@ Imu_Integration::~Imu_Integration(){
 bool Imu_Integration::update(double* imu){
     if(syn)return false;
     if(imu_factor_id<1)return false;
+    if(abs(imu[0])<1.0)imu[0]=pow(imu[0],3);
+    if(abs(imu[1])<1.0)imu[0]=pow(imu[1],3);
     auto in=std::chrono::steady_clock::now();
     if(std::chrono::duration<double>(in-t_1).count()>20.0){
         t_1=in;
-        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),0.005);
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),0.0005);
     }else{
         preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),std::chrono::duration<double>(in-t_1).count());
         t_1=in;
@@ -61,126 +63,79 @@ bool Imu_Integration::update(double* imu){
     return true;
 
 }
-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);
-    ++imu_factor_id;
-    syn=false;
-    new_frame->T_f_w_=svo::SE2_5(statePtr->t().x(),statePtr->t().z(),atan(statePtr->R()(0,2)/statePtr->R()(0,0)));
-    new_frame->Cov_=optimizer.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(),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)));
-    for(auto&& f_n:_for(new_frame->fts_)){
-        if(f_n.item->point == NULL)continue;
-        if(!result.exists(L(f_n.index)))continue;
-        f_n.item->point->pos_=result.at<gtsam::Point3>(L(f_n.index));
-    //    valuesPtr->insert(L(f_n.index),f_n.item->point->pos_);
-//        graphPtr->emplace_shared<svo::VisionFactor>(f_n.item->f,f_n.item->level,ProjectNoisePtr,P(imu_factor_id),L(f_n.index));
-    }
-    return true;
-}
 bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
     gtsam::Values result = optimizer.optimize();
-    if(debug)optimizer.print();
-    // 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);
-    if(debug)fprintf(logtime, "%f,%f,%f,%f,%f,%f,%f,%f,\n",
-                     new_frame->T_f_w_.T->translation().x(),
-                     new_frame->T_f_w_.T->translation().y(),
-                     new_frame->T_f_w_.T->translation().z(),
-                     atan(new_frame->T_f_w_.T->rotation_matrix()(0,2)/new_frame->T_f_w_.T->rotation_matrix()(0,0)),
+    if(debug)fprintf(logtime, "%d,%f,%f,%f,%f,\n",
+                     imu_factor_id-1,
                      statePtr->pose().x(),
                      statePtr->pose().y(),
                      statePtr->pose().z(),
-                     atan(statePtr->R()(0,2)/statePtr->R()(0,0)));
-    if((statePtr->t()-new_frame->T_f_w_.T->translation()).norm()>2.0)return false;
-    new_frame->T_f_w_=svo::SE2_5(statePtr->t().x(),statePtr->t().z(),atan(statePtr->R()(0,2)/statePtr->R()(0,0)));
+                     statePtr->pose().rotation().pitch());
+    new_frame->T_f_w_=svo::SE2_5(statePtr->pose().x(),statePtr->pose().z(),statePtr->pose().rotation().pitch());
     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(P(imu_factor_id-1),gtsam::Pose3(new_frame->T_f_w_.T->matrix()));
     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(P(imu_factor_id-1),gtsam::Pose3(new_frame->T_f_w_.T->matrix()),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;
 }
-Sophus::SE3 Imu_Integration::preintegrate_predict() {
+bool Imu_Integration::preintegrate_predict(svo::SE2_5& frame) {
     if(imu_n<1){
         syn= false;
-        usleep(10000);
-        syn=true;
+        return false;
     }else{
         syn=true;
     }
-    auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
-    Integ_predict=preint_imu_combined.predict(*statePtr,*imu_biasPtr);
-    return Sophus::SE3(Integ_predict.R(),Integ_predict.t());
+    Integ_predict=preintegratedPtr->predict(*statePtr,*imu_biasPtr);
+    frame =svo::SE2_5(Integ_predict.pose().x(),Integ_predict.pose().z(),Integ_predict.pose().rotation().pitch());
+    return true;
+}
+void Imu_Integration::reset() {
+    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
+    imu_n=0;
+    syn=false;
 }
 bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,
                               boost::shared_ptr<svo::Frame>& last_frame,
-                              std::size_t& num_obs,const double cell_match){
-    if(debug)std::cout<<"----------------------------IMU infuse---------------------------------------\n";
-    //if(debug)preintegratedPtr->print();
-    auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+                              const double cell_match){
     valuesPtr->insert(P(imu_factor_id), Integ_predict.pose());
     valuesPtr->insert(V(imu_factor_id), Integ_predict.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);
+                                        *preintegratedPtr);
     graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
     imu_n=0;
     graphPtr->emplace_shared<svo::VisionFactor>(gtsam::Pose3(new_frame->T_f_w_.T->matrix()),
                                                 gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(3) << 1.0, 1.0, 1.0).finished()/(cell_match+1)),
                                                 P(imu_factor_id));
     gtsam::LevenbergMarquardtParams params0;
-    //params0.verbosity=gtsam::LevenbergMarquardtParams::LINEAR;
     params0.verbosityLM = gtsam::LevenbergMarquardtParams::SILENT;
     params0.setlambdaFactor(10.0);
     params0.setlambdaInitial(100);
     params0.setlambdaUpperBound(1e10);
     gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr,params0);
     reset(optimizer,new_frame);
-
-    if(debug)std::cout<<"------------------------------------------------------------------------------\n";
     return true;
 }
 bool Imu_Integration::init(boost::shared_ptr<svo::Frame>& new_frame){
     statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(new_frame->T_f_w_.T->matrix()),
                                                gtsam::Vector3(1e-9,1e-9,1e-9));
-    /*
-    statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0,0.0,0.0),gtsam::Point3(0.0,0.0,0.0)),
-                                               gtsam::Vector3(1e-9,1e-9,1e-9));
-                                               */
     valuesPtr->insert(P(0),statePtr->pose());
     valuesPtr->insert(V(0),statePtr->v());
     valuesPtr->insert(B(0),*imu_biasPtr);
     graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-5, 1e-15, 1e-5, 1e-15, 1e-8, 1e-15).finished()));
     graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
     graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2));
-    /*
-    for(auto&& f_n:_for(new_frame->fts_)){
-        if(f_n.item->point == NULL)continue;
-        valuesPtr->insert(L(f_n.index),f_n.item->point->pos_);
-        graphPtr->emplace_shared<svo::VisionFactor>(f_n.item->f,f_n.item->level,ProjectNoisePtr,P(imu_factor_id),L(f_n.index));
-    }
-    graphPtr->addPrior(L(0),new_frame->fts_.front()->point->pos_,gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
-     */
     ++imu_factor_id;
     return true;
 }

GPU_version/svo/src/matcher.cpp

@@ -184,6 +184,7 @@ bool Matcher::findEpipolarMatchDirect(
     const double d_max,
     double& depth)
 {
+  if(isnan(d_min) || isnan(d_max))return false;
   SE3 T_cur_ref = (*cur_frame.T_f_w_.T) * ref_frame.T_f_w_.T->inverse();
   int zmssd_best = PatchScore::threshold();
   Vector2d uv_best;
@@ -217,6 +218,7 @@ bool Matcher::findEpipolarMatchDirect(
   Vector2d px_B(cur_frame.cam_->world2cam(B));
   epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
 
+
   // Warp reference patch at ref_level
   warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
                    ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);

GPU_version/svo/src/sparse_img_align.cpp

@@ -54,11 +54,11 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
   cur_frame_ = cur_frame;
   ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
   jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);
-  //visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
+  visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
   SE3 T_cur_from_ref(*cur_frame_->T_f_w_.T * ref_frame_->T_f_w_.T->inverse());
   for(level_=max_level_; level_>=min_level_; --level_)
   {
-    visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
+    //visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
     mu_ = 0.1;
     jacobian_cache_.setZero();
     have_ref_patch_cache_ = false;

GPU_version/vio/include/vio_gpu/visualizer.h

@@ -44,7 +44,7 @@ public:
   size_t trace_id_;
   ros::Publisher pub_frames_;
   ros::Publisher pub_points_;
-  ros::Publisher pub_pose_;
+  ros::Publisher pub_pose_with_cov_;
   ros::Publisher pub_info_;
   tf::TransformBroadcaster br_;
   SE3 T_world_from_vision_;

GPU_version/vio/param/vo_fast.yaml

@@ -1,13 +1,13 @@
 svo:
   grid_size: 30            #Feature grid size of a cell in [px].
-  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.
+  max_n_kfs: 20            #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         #Number of iterations in the local bundle adjustment.
   quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
   quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad.
   init_min_tracked: 100
-  init_min_disparity: 25
+  init_min_disparity: 20
   structureoptim_max_pts: 50
-  kfselect_mindist: 0.12
+  kfselect_mindist: 0.05
   poseoptim_thresh: 4.0
   ACC_noise: 0.0001          #acc white noise in continuous
   GYO_noise: 0.0001          #gyro white noise in continuous
@@ -17,6 +17,6 @@ svo:
   EBP: 0.1                #error in the bias used for preintegration
   NRP: 0.001                #noise used for projection in preintegration
   gravity: -9.65
-  Acceleration_offset_x: 0.0 #-0.05
-  Acceleration_offset_y: 0.0 #-0.04
-  Gyroscope_offset_z: 0.0 #0.001
+  Acceleration_offset_x: 0.0
+  Acceleration_offset_y: 0.0
+  Gyroscope_offset_z: 0.0

GPU_version/vio/src/visualizer.cpp

@@ -21,6 +21,7 @@
 #include <gpu_svo/feature.h>
 #include <vio_gpu/Info.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <gpu_svo/timer.h>
 #include <vio_gpu/output_helper.h>
 #include <deque>
@@ -35,8 +36,8 @@ Visualizer() :
 {
   // Init ROS Marker Publishers
   //pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
- // pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
-  pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);
+  pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
+  pub_pose_with_cov_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_cov",10);
 }
 
 void Visualizer::publishMinimal(
@@ -50,28 +51,31 @@ void Visualizer::publishMinimal(
   header_msg.frame_id = "/world";
   header_msg.seq = trace_id_;
   header_msg.stamp = ros::Time(timestamp);
-  if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
+  if(pub_pose_with_cov_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
   {
-    Quaterniond q;
-    Vector3d p;
+    //Quaterniond q;
     Matrix<double,6,6> Cov;
     // publish cam in world frame (Estimated odometry in the worls frame)
     SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.T->inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera
     //q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
-    //p = T_world_from_cam.translation();
     Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
-    geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
-    msg_pose->header = header_msg;
-    msg_pose->pose.pose.position.x = p[0];
-    msg_pose->pose.pose.position.y = p[1];
-    msg_pose->pose.pose.position.z = p[2];
-    msg_pose->pose.pose.orientation.x = q.x()/q.norm();
-    msg_pose->pose.pose.orientation.y = q.y()/q.norm();
-    msg_pose->pose.pose.orientation.z = q.z()/q.norm();
-    msg_pose->pose.pose.orientation.w = q.w()/q.norm();
+    geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
+      msg_pose_with_cov->header = header_msg;
+      msg_pose_with_cov->pose.pose.position.x = T_world_from_cam.translation().x();
+      msg_pose_with_cov->pose.pose.position.y = T_world_from_cam.translation().y();
+      msg_pose_with_cov->pose.pose.position.z = T_world_from_cam.translation().z();
+      msg_pose_with_cov->pose.pose.orientation.x = frame->T_f_w_.T->unit_quaternion().x()/frame->T_f_w_.T->unit_quaternion().norm();
+      msg_pose_with_cov->pose.pose.orientation.y = frame->T_f_w_.T->unit_quaternion().y()/frame->T_f_w_.T->unit_quaternion().norm();
+      msg_pose_with_cov->pose.pose.orientation.z = frame->T_f_w_.T->unit_quaternion().z()/frame->T_f_w_.T->unit_quaternion().norm();
+      msg_pose_with_cov->pose.pose.orientation.w = frame->T_f_w_.T->unit_quaternion().w()/frame->T_f_w_.T->unit_quaternion().norm();
     for(size_t i=0; i<36; ++i)
-      msg_pose->pose.covariance[i] = Cov(i%6, i/6);
-    pub_pose_.publish(msg_pose);
+        msg_pose_with_cov->pose.covariance[i] = Cov(i%6, i/6);
+    pub_pose_with_cov_.publish(msg_pose_with_cov);
+  }
+  if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
+        vk::output_helper::publishPointMarker(
+                pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
+                ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
   }
 }