Merge remote-tracking branch 'origin/dev2' into dev2

# Conflicts:
#	GPU_version/vio/src/frame_handler_mono.cpp

GPU_version/vio/launch/Px30.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" /-->
+  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
 
     <!-- Camera topic to subscribe to -->
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />

GPU_version/vio/param/vo_fast.yaml

@@ -5,12 +5,12 @@ svo:
   quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
   quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
   init_min_tracked: 100
-  init_min_disparity: 30
+  init_min_disparity: 40
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
   poseoptim_thresh: 8.0
   init_min_inliers: 20
   ACC_noise: 0.5          #acc white noise in continuous in EKF
-  GYO_noise: 0.1          #gyro white noise in continuous in EKF
+  GYO_noise: 0.5          #gyro white noise in continuous in EKF
-  svo_ekf: 0.1              #Svo noise that will be devided by number of Reprojected points after opmization in EKF
+  svo_ekf: 0.05              #Svo noise that will be devided by number of Reprojected points after opmization in EKF
   Cmd_Cov: 0.1  #control vector noise in EKF

GPU_version/vio/src/depth_filter.cpp

@@ -269,7 +269,7 @@ namespace vio {
 #if VIO_DEBUG
             fprintf(log_,"[%s]  If point is visible? %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z());
 #endif
-            if(xyz_f.z() < 0.0)  {
+            if(xyz_f.z() > 0.0)  {
                 ++it; // behind the camera
                 continue;
             }

GPU_version/vio/src/feature_alignment.cpp

@@ -336,7 +336,7 @@ bool align2D_SSE2(
   float v = cur_px_estimate.y();
 
   // termination condition
-  const float min_update_squared = 0.1*0.1;
+  const float min_update_squared = 0.01*0.01;
   const int cur_step = cur_img.step.p[0];
   const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
   float chi2 = 0;
@@ -501,7 +501,7 @@ bool align2D_NEON (
   float v = cur_px_estimate.y();
 
   // termination condition
-  const float min_update_squared = 0.1*0.1;
+  const float min_update_squared = 0.01*0.01;
   const int cur_step = cur_img.step.p[0];
   Vector3f update;
   Vector3f Jres;

GPU_version/vio/src/frame_handler_mono.cpp

@@ -221,7 +221,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     }
     fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
 #endif*/
-  reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_);
+  reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
   int n_point=0;
   for(auto i:overlap_kfs_)n_point+=i.second;
 #if VIO_DEBUG

GPU_version/vio/src/pose_optimizer.cpp

@@ -40,7 +40,7 @@ void optimizeGaussNewton(
   // init
   double chi2(0.0);
   vector<double> chi2_vec_init, chi2_vec_final;
-  vk::robust_cost::TukeyWeightFunction weight_function;
+  vk::robust_cost::HuberWeightFunction weight_function;
   SE2_5 T_old(frame->T_f_w_);
   Matrix3d A;
   Vector3d b;
@@ -54,32 +54,32 @@ void optimizeGaussNewton(
     //Reprojection error
     Vector2d e = vk::project2d((*it)->f)
                - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
-
     e *= 1.0 / (1<<(*it)->level);
+    chi2_vec_init.push_back(e.squaredNorm()); // just for debug
     errors.push_back(e.norm());
   }
 
   if(errors.empty())
     return;
   vk::robust_cost::MADScaleEstimator scale_estimator;
-  estimated_scale = 0.5*scale_estimator.compute(errors);
+  estimated_scale = scale_estimator.compute(errors);
-/*#if VIO_DEBUG
+#if VIO_DEBUG
     fprintf(log,"[%s] Init estimate scale:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),estimated_scale);
-#endif*/
+#endif
 
   num_obs = errors.size();
   chi2_vec_init.reserve(num_obs);
   chi2_vec_final.reserve(num_obs);
   double scale = estimated_scale;
-  bool direction=true;
-  size_t n_direction=0;
   for(size_t iter=0; iter<n_iter; iter++)
   {
 
     b.setZero();
     A.setZero();
     double new_chi2(0.0);
-
+    // overwrite scale
+    if(iter == 5)
+        scale = 0.85/frame->cam_->errorMultiplier2();
     // compute residual
     for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
     {
@@ -89,20 +89,13 @@ void optimizeGaussNewton(
       Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
       frame->jacobian_xyz2uv_((*it)->f,(*it)->point->pos_,J);
       Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
-/*#if VIO_DEBUG
-        fprintf(log,"[%s] xyz_f:%f, %f, %f  Reprojection Error: %f, %f Feature: %f, %f, %f\n",
-                vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z(),e.x(),e.y(),
-                        (*it)->point->pos_.x(),(*it)->point->pos_.y(),(*it)->point->pos_.z());
-#endif*/
       double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
       e *= sqrt_inv_cov;
-      if(iter == 0)
-        chi2_vec_init.push_back(e.squaredNorm()); // just for debug
       J *= sqrt_inv_cov;
       double weight = weight_function.value(e.norm()/scale);
       A.noalias() += J.transpose()*J*weight;
       b.noalias() -= J.transpose()*e*weight;
-      new_chi2 += e.squaredNorm()*weight;
+      new_chi2 += e.squaredNorm();
     }
 
     // solve linear system
@@ -111,65 +104,54 @@ void optimizeGaussNewton(
     // check if error increased
     if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
     {
-/*#if VIO_DEBUG
+#if VIO_DEBUG
           fprintf(log,"[%s] it:%d \t FAILURE \t new_chi2: %f\n",
                   vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2);
-#endif*/
+#endif
       frame->T_f_w_ = T_old; // roll-back
       break;
     }
-    scale *=0.5;
+    dT *=new_chi2;
-    dT=0.1*dT;
     // update the model
     T_old = frame->T_f_w_;
     frame->T_f_w_=SE2_5(T_old.se2().translation().x()+dT.x(),T_old.se2().translation().y()+dT.y(),T_old.pitch()+dT.z());
 
     chi2 = new_chi2;
-/*#if VIO_DEBUG
+#if VIO_DEBUG
       fprintf(log,"[%s] it:%d \t Success \t new_chi2: %f \t dT = %f, %f, %f\n",
               vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2,dT.x(),dT.y(),dT.z());
-#endif*/
+#endif
 
     // stop when converged
     if(vk::norm_max(dT) <= EPS)
       break;
   }
 
-  // Set covariance as inverse information matrix. Optimistic estimator!
-  //const double pixel_variance=1.0;
-  //frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse();
-
-  // Remove Measurements with too large reprojection error
-  double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2();
   size_t n_deleted_refs = 0;
   for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
   {
     if((*it)->point == NULL)
       continue;
     Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
-    double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
+    e /= (1<<(*it)->level);
-    e *= sqrt_inv_cov;
     chi2_vec_final.push_back(e.squaredNorm());
-    if(e.norm() > reproj_thresh_scaled)
+    if(e.norm() > 0.01)
     {
       // we don't need to delete a reference in the point since it was not created yet
       (*it)->point = NULL;
       ++n_deleted_refs;
     }
   }
-  error_init=0.0;
+#if VIO_DEBUG
-  error_final=0.0;
+    error_init=0.0;
-  if(!chi2_vec_init.empty())
+    error_final=0.0;
-    error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2();
+    if(!chi2_vec_init.empty())for(auto&& i:chi2_vec_init)error_init+=i;
-  if(!chi2_vec_final.empty())
+        error_init /=chi2_vec_init.size();
-    error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2();
+    if(!chi2_vec_final.empty())for(auto&& i:chi2_vec_final)error_final+=i;
-
+        error_final /= chi2_vec_final.size();
-  estimated_scale *= frame->cam_->errorMultiplier2();
+    fprintf(log,"[%s] n deleted obs = %d \t scale =%f \t error init =%f \t error end=%f\n",
-  if(verbose)
+            vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,estimated_scale,error_init,error_final);
-    std::cout << "n deleted obs = " << n_deleted_refs
+#endif
-              << "\t scale = " << estimated_scale
-              << "\t error init = " << error_init
-              << "\t error end = " << error_final << std::endl;
   num_obs -= n_deleted_refs;
 }
 

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -65,7 +65,7 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
         featue_px[feature_counter_].y=(*it)->px(1);
         ++feature_counter_;
   }
-  if(!feature_counter_) // more than 10
+  if(feature_counter_<10) // more than 10
   {
       return 0;
   }