debug depth mines

GPU_version/vio/depth_filter_log.txt

@@ -1,28 +0,0 @@
-[19:53:47.528] New key frame 
-[19:53:47.528] BA loop run 
-[19:53:47.529] init error: 16.678827 
-[19:53:47.530] end error: 0.000156 
-[19:53:47.626] New key frame 
-[19:53:47.626] BA loop run 
-[19:53:47.626] init error: 5.578004 
-[19:53:47.629] end error: 0.135229 
-[19:53:47.760] New key frame 
-[19:53:47.760] BA loop run 
-[19:53:47.760] init error: 11.645444 
-[19:53:47.761] end error: 0.000002 
-[19:53:47.877] New key frame 
-[19:53:47.877] BA loop run 
-[19:53:47.877] init error: 23.621901 
-[19:53:47.878] end error: 0.000000 
-[19:53:47.967] New key frame 
-[19:53:47.967] BA loop run 
-[19:53:47.968] init error: 2.238685 
-[19:53:47.968] end error: 0.000000 
-[19:53:48.433] New key frame 
-[19:53:48.433] BA loop run 
-[19:53:48.434] init error: 3.916923 
-[19:53:48.434] end error: 0.000339 
-[19:53:48.549] New key frame 
-[19:53:48.549] BA loop run 
-[19:53:48.549] init error: 28.908982 
-[19:53:48.550] end error: 0.000015 

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/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;
@@ -179,7 +179,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     fprintf(log_,"[%s] Update EKF and 3D points the number of feature in the new frame: %d and number of obs: %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
             new_frame_->fts_.size(),new_frame_->nObs());
 #endif
-  //posEdit(new_frame_);
   double depth_mean=0.0, depth_min=0.0;
     new_frame_->getSceneDepth(map_, depth_mean, depth_min);
 #if VIO_DEBUG
@@ -220,7 +219,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   // add keyframe to map
   map_.addKeyframe(new_frame_);
   map_.checkKeyFrames();
-  ba_glob_->new_key_frame();
+  //ba_glob_->new_key_frame();
   return RESULT_IS_KEYFRAME;
 }
 

GPU_version/vio/src/global_optimizer.cpp

@@ -191,7 +191,7 @@ namespace vio {
                 }
             }
 
-            // Remove Measurements with too large reprojection error
+            /*// Remove Measurements with too large reprojection error
             for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
                 it!=incorrect_edges.end(); ++it)
                 map_.removePtFrameRef(it->first, it->second);
@@ -202,7 +202,7 @@ namespace vio {
                 {
                     map_.removePtFrameRef(it->frame, it->feature);
                 }
-            }
+            }*/
             ba_mux_.unlock();
         }
     }

GPU_version/vio/src/point.cpp

@@ -136,9 +136,9 @@ void Point::optimize(const size_t n_iter)
     for(auto it=obs_.begin(); it!=obs_.end(); ++it)
     {
       Matrix23d J;
-      const Vector3d p_in_f((*it)->frame->se3()*pos_);
+      const Vector3d p_in_f((*it)->frame->w2f(pos_));
-      //jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
+      jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
-      jacobian_xyz2uv(p_in_f,(*it)->frame->se3().rotation_matrix(),J);
+      //jacobian_xyz2uv(p_in_f,(*it)->frame->se3().rotation_matrix(),J);
       const Vector2d e=vk::project2d((*it)->f) - vk::project2d(p_in_f)/(1<<(*it)->level);
       new_chi2 += e.norm();
       A.noalias() += J.transpose() * J;
@@ -167,9 +167,9 @@ 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_));
+            Vector2d e = vk::project2d((*it)->f) - vk::project2d((*it)->frame->w2f(pos_));
             e /= (1<<(*it)->level);
-            if (e.squaredNorm() > 2.0*vio::Config::poseOptimThresh()/ (*it)->frame->cam_->errorMultiplier2())
+            if (e.squaredNorm() > 8.0*vio::Config::poseOptimThresh()/ (*it)->frame->cam_->errorMultiplier2())
                 n_failed_reproj_++;
   }
   if(n_failed_reproj_<0.50*obs_.size())

GPU_version/vio/src/pose_optimizer.cpp

@@ -129,7 +129,6 @@ void optimizeGaussNewton(
     if(vk::norm_max(dT) <= EPS)
       break;
   }
-  //point_mut_.lock();
   num_obs=0;
   for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   {
@@ -143,7 +142,7 @@ void optimizeGaussNewton(
           it = frame->fts_.erase(it);
           continue;
     }
-    Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
+    Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->w2f((*it)->point->pos_));
     e /= (1<<(*it)->level);
     chi2_vec_final.push_back(e.norm());
     if(e.norm() >  vio::Config::poseOptimThresh() / frame->cam_->errorMultiplier2())
@@ -157,7 +156,6 @@ void optimizeGaussNewton(
     }
 
   }
-  //point_mut_.unlock();
 #if VIO_DEBUG
     error_init=0.0;
     error_final=0.0;