get an unstable currect result

GPU_version/vio/include/vio/config.h

@@ -33,23 +33,10 @@ class Config
 public:
   static Config& getInstance();
 
-  /// Base-name of the tracefiles.
-  static string& traceName() { return getInstance().trace_name; }
-
-  /// Directory where the tracefiles are saved.
-  static string& traceDir() { return getInstance().trace_dir; }
 
   /// Number of pyramid levels used for features.
   static size_t& nPyrLevels() { return getInstance().n_pyr_levels; }
 
-  /// Use the IMU to get relative rotations.
-  static bool& useImu() { return getInstance().use_imu; }
-
-  /// Number of keyframes in the core. The core-kfs are optimized through bundle adjustment.
-  static size_t& coreNKfs() { return getInstance().core_n_kfs; }
-
-  /// Initial scale of the map. Depends on the distance the camera is moved for the initialization.
-  static double& mapScale() { return getInstance().map_scale; }
 
   /// Feature grid size of a cell in [px].
   static size_t& gridSize() { return getInstance().grid_size; }
@@ -57,8 +44,6 @@ public:
   /// Initialization: Minimum required disparity between the first two frames.
   static double& initMinDisparity() { return getInstance().init_min_disparity; }
 
-  /// Initialization: Minimum number of tracked features.
-  static size_t& initMinTracked() { return getInstance().init_min_tracked; }
 
   /// Initialization: Minimum number of inliers after RANSAC.
   static size_t& initMinInliers() { return getInstance().init_min_inliers; }
@@ -69,14 +54,10 @@ public:
   /// Minimum level of the Lucas Kanade tracker.
   static size_t& kltMinLevel() { return getInstance().klt_min_level; }
 
-  /// Reprojection threshold [px].
-  static double& reprojThresh() { return getInstance().reproj_thresh; }
 
   /// Reprojection threshold after pose optimization.
   static double& poseOptimThresh() { return getInstance().poseoptim_thresh; }
 
-  /// Number of iterations in local bundle adjustment.
-  static size_t& poseOptimNumIter() { return getInstance().poseoptim_num_iter; }
 
   /// Maximum number of points to optimize at every iteration.
   static size_t& structureOptimMaxPts() { return getInstance().structureoptim_max_pts; }
@@ -85,7 +66,6 @@ public:
   static size_t& structureOptimNumIter() { return getInstance().structureoptim_num_iter; }
 
   /// Reprojection threshold after bundle adjustment.
-  static double& lobaThresh() { return getInstance().loba_thresh; }
 
   /// Threshold for the robust Huber kernel of the local bundle adjustment.
   static double& lobaRobustHuberWidth() { return getInstance().loba_robust_huber_width; }
@@ -93,31 +73,15 @@ public:
   /// Number of iterations in the local bundle adjustment.
   static size_t& lobaNumIter() { return getInstance().loba_num_iter; }
 
-  /// Minimum distance between two keyframes. Relative to the average height in the map.
-  static double& kfSelectMinDist() { return getInstance().kfselect_mindist; }
-
   /// Select only features with a minimum Harris corner score for triangulation.
   static double& triangMinCornerScore() { return getInstance().triang_min_corner_score; }
 
-  /// Subpixel refinement of reprojection and triangulation. Set to 0 if no subpix refinement required!
-  static size_t& subpixNIter() { return getInstance().subpix_n_iter; }
 
   /// 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.
   static size_t& maxNKfs() { return getInstance().max_n_kfs; }
 
-  /// How much (in milliseconds) is the camera delayed with respect to the imu.
-  static double& imgImuDelay() { return getInstance().img_imu_delay; }
-
-  /// Maximum number of features that should be tracked.
-  static size_t& maxFts() { return getInstance().max_fts; }
-
-  /// If the number of tracked features drops below this threshold. Tracking quality is bad.
-  static size_t& qualityMinFts() { return getInstance().quality_min_fts; }
-
-  /// 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; }

GPU_version/vio/param/vo_fast.yaml

@@ -8,7 +8,7 @@ vio:
   init_min_disparity: 10
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
-  poseoptim_thresh: 8.0
+  poseoptim_thresh: 1.75
   init_min_inliers: 40
   ACC_ekf: 0.2          #acc white noise in continuous in EKF
   GYO_ekf: 0.5         #gyro white noise in continuous in EKF

GPU_version/vio/src/frame_handler_mono.cpp

@@ -132,7 +132,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
     fprintf(log_,"[%s] Init: Selected Second frame. \t The number of features: %d depth mean:%f min:%f\n",
                        vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size(),depth_mean,depth_min);
 #endif
-  ba_glob_->new_key_frame();
+  //ba_glob_->new_key_frame();
   ROS_INFO("VIO initialized :)");
   ROS_INFO("Running ...");
   return RESULT_IS_KEYFRAME;
@@ -158,7 +158,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   size_t sfba_n_edges_final=0;
   double sfba_thresh, sfba_error_init, sfba_error_final;
   pose_optimizer::optimizeGaussNewton(
-            Config::poseOptimThresh(),
+            10,
             new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,map_,log_);
 #if VIO_DEBUG
     fprintf(log_,"[%s] After pose optimization, distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),

GPU_version/vio/src/global_optimizer.cpp

@@ -126,7 +126,7 @@ namespace vio {
 
                     // Due to merging of mappoints it is possible that references in kfs suddenly
                     // have a very large reprojection error which may result in distorted results.
-                 /*   Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
+/*                    Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
                     if(error.squaredNorm() > pow(2,Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()))
                         incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
                     else
@@ -140,10 +140,13 @@ namespace vio {
                         edge.feature=it_ftr;
                         edges.push_back(edge);
                         optimizer.addEdge(e);
-                    /*}*/
+                   // }
                 }
             }
-            if(optimizer.vertices().size()<1)continue;
+            if(optimizer.vertices().empty()){
+                ba_mux_.unlock();
+                continue;
+            }
             // Optimization
             optimizer.initializeOptimization();
             optimizer.computeActiveErrors();
@@ -161,7 +164,10 @@ namespace vio {
             fprintf(log_,"[%s] init error: %f \n",
                     vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
 #endif
-            if(optimizer.optimize(vio::Config::lobaNumIter())<1)continue;
+            if(optimizer.optimize(vio::Config::lobaNumIter())<1){
+                ba_mux_.unlock();
+                continue;
+            }
 #if VIO_DEBUG
             fprintf(log_,"[%s] end error: %f \n",
                     vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());

GPU_version/vio/src/point.cpp

@@ -19,6 +19,7 @@
 #include <vio/point.h>
 #include <vio/frame.h>
 #include <vio/feature.h>
+#include <vio/config.h>
  
 namespace vio {
 
@@ -166,10 +167,10 @@ void Point::optimize(const size_t n_iter)
   for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
             Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * pos_));
             e /= (1<<(*it)->level);
-            if (e.norm() > 4.0/ (*it)->frame->cam_->errorMultiplier2())
+            if (e.norm() > vio::Config::poseOptimThresh()/ (*it)->frame->cam_->errorMultiplier2())
                 n_failed_reproj_++;
   }
-  if(n_failed_reproj_<0.75*obs_.size())
+  if(n_failed_reproj_<0.70*obs_.size())
       type_=TYPE_GOOD;
 }
 

GPU_version/vio/src/pose_optimizer.cpp

@@ -22,6 +22,7 @@
 #include <vio/robust_cost.h>
 #include <vio/math_utils.h>
 #include <sophus/se2.h>
+#include <vio/config.h>
 
 namespace vio {
 namespace pose_optimizer {
@@ -81,7 +82,6 @@ void optimizeGaussNewton(
   vk::robust_cost::MADScaleEstimator scale_estimator;
   estimated_scale = scale_estimator.compute(errors);
 
-  num_obs = errors.size();
   double scale = estimated_scale;
   for(size_t iter=0; iter<n_iter; iter++)
   {
@@ -132,13 +132,12 @@ void optimizeGaussNewton(
       break;
   }
   //point_mut_.lock();
-  size_t n_deleted_refs = 0;
+  num_obs=0;
   for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   {
     if((*it)->point == NULL) {
         map.safeDeletePoint((*it)->point);
         it = frame->fts_.erase(it);
-        ++n_deleted_refs;
         continue;
     }
     if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.){
@@ -149,19 +148,18 @@ void optimizeGaussNewton(
     Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
     e /= (1<<(*it)->level);
     chi2_vec_final.push_back(e.norm());
-    if(e.norm() >  1.0 / frame->cam_->errorMultiplier2())
+    if(e.norm() >  0.5*vio::Config::poseOptimThresh() / frame->cam_->errorMultiplier2())
     {
       map.safeDeletePoint((*it)->point);
       it = frame->fts_.erase(it);
-      ++n_deleted_refs;
     }else{
         (*it)->point->type_=vio::Point::TYPE_CANDIDATE;
+        ++num_obs;
         it++;
     }
 
   }
   //point_mut_.unlock();
-  num_obs -= n_deleted_refs;
 #if VIO_DEBUG
     error_init=0.0;
     error_final=0.0;
@@ -169,8 +167,8 @@ void optimizeGaussNewton(
         error_init /=chi2_vec_init.size();
     if(!chi2_vec_final.empty())for(auto&& i:chi2_vec_final)error_final+=i;
         error_final /= chi2_vec_final.size();
-    fprintf(log,"[%s] n deleted obs = %d \t n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =%d \t error init =%f \t error end=%f\n",
+    fprintf(log,"[%s]  n obs with reprojection error less than 1.0 / frame->cam_->errorMultiplier2() =%d \t error init =%f \t error end=%f\n",
-            vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,num_obs,error_init,error_final);
+            vio::time_in_HH_MM_SS_MMM().c_str(),num_obs,error_init,error_final);
 #endif
 }
 

GPU_version/vio/src/vo_node.cpp

@@ -147,9 +147,6 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
     imu_in[0] = 0.2*imu_[0]+0.8*imu->linear_acceleration.x;
     imu_in[1] = 0.2*imu_[1]+0.8*imu->linear_acceleration.y;
     imu_in[2] = 0.2*imu_[2]+0.8*imu->angular_velocity.z;
-    if(fabs(imu_in[2])>0.4)return;
-    if(fabs(imu_in[1])>1.0)return;
-    if(fabs(imu_in[0])>1.0)return;
     memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
     vo_->UpdateIMU(imu_in,imu->header.stamp);
     imu_time_=imu->header.stamp;