debugging the optimization

GPU_version/vio/depth_filter_log.txt

@@ -1,9 +1,172 @@
-[17:03:24.779] init depth filter
-[17:03:46.024] DepthFilter: RESET.
-[17:03:48.535] add key frame
-[17:03:48.535] init seeds
-[17:03:48.535] update seed
-[17:03:48.535] init seeds
-[17:03:48.539] DepthFilter: Initialized 28 new seeds
-[17:03:48.540] DepthFilter: Initialized 63 new seeds
-[17:04:17.931] DepthFilter stop thread invoked.[17:04:17.931] DepthFilter interrupt and join thread 
+[11:52:21.714] init depth filter
+[11:52:30.966] DepthFilter: RESET.
+[11:52:32.781] add key frame
+[11:52:32.781] update seed
+[11:52:32.781] init seeds
+[11:52:32.786] DepthFilter: Initialized 80 new seeds
+[11:52:56.055] add new frame
+[11:52:56.055] update seed
+[11:52:56.055]  If point is visible? 0.027952, -0.014905, 0.048499
+[11:52:56.055]  If point is visible? 0.027753, -0.015271, 0.048499
+[11:52:56.055]  If point is visible? 0.027771, -0.015148, 0.048527
+[11:52:56.055]  If point is visible? 0.029140, -0.017475, 0.046917
+[11:52:56.055]  If point is visible? 0.028009, -0.017347, 0.047646
+[11:52:56.055]  If point is visible? 0.028030, -0.017226, 0.047678
+[11:52:56.055]  If point is visible? 0.028174, -0.017227, 0.047593
+[11:52:56.055]  If point is visible? 0.020293, -0.020609, 0.050188
+[11:52:56.055]  If point is visible? 0.020449, -0.020617, 0.050122
+[11:52:56.055]  If point is visible? 0.020400, -0.020853, 0.050044
+[11:52:56.055]  If point is visible? 0.020556, -0.020861, 0.049977
+[11:52:56.055]  If point is visible? 0.028621, -0.023543, 0.044525
+[11:52:56.055]  If point is visible? 0.019731, -0.024487, 0.048648
+[11:52:56.055]  If point is visible? 0.019759, -0.024372, 0.048694
+[11:52:56.055]  If point is visible? 0.026732, -0.030262, 0.041544
+[11:52:56.055]  If point is visible? 0.036145, -0.030595, 0.033388
+[11:52:56.055]  If point is visible? 0.035791, -0.031574, 0.032855
+[11:52:56.055]  If point is visible? 0.013190, -0.038644, 0.041103
+[11:52:56.055]  If point is visible? 0.021000, -0.039503, 0.036818
+[11:52:56.055]  If point is visible? 0.014550, -0.038122, 0.041130
+[11:52:56.055]  If point is visible? 0.022922, -0.039614, 0.035533
+[11:52:56.055]  If point is visible? 0.020696, -0.040387, 0.036024
+[11:52:56.055]  If point is visible? 0.020838, -0.040389, 0.035940
+[11:52:56.055]  If point is visible? 0.020980, -0.040391, 0.035856
+[11:52:56.055]  If point is visible? 0.031891, -0.007310, 0.047806
+[11:52:56.055]  If point is visible? 0.029369, -0.010330, 0.048851
+[11:52:56.055]  If point is visible? 0.029702, -0.012574, 0.048119
+[11:52:56.055]  If point is visible? 0.028635, -0.011822, 0.048948
+[11:52:56.055]  If point is visible? 0.028479, -0.015279, 0.048074
+[11:52:56.055]  If point is visible? 0.002201, -0.015321, 0.055823
+[11:52:56.055]  If point is visible? 0.027844, -0.017467, 0.047700
+[11:52:56.055]  If point is visible? 0.028134, -0.017469, 0.047528
+[11:52:56.055]  If point is visible? 0.002279, -0.014845, 0.055948
+[11:52:56.055]  If point is visible? 0.020332, -0.016985, 0.051512
+[11:52:56.055]  If point is visible? 0.017772, -0.020466, 0.051193
+[11:52:56.055]  If point is visible? 0.019668, -0.020577, 0.050450
+[11:52:56.055]  If point is visible? 0.016714, -0.020876, 0.051383
+[11:52:56.055]  If point is visible? 0.017035, -0.020898, 0.051269
+[11:52:56.055]  If point is visible? 0.019716, -0.020339, 0.050527
+[11:52:56.055]  If point is visible? 0.015707, -0.023889, 0.050380
+[11:52:56.055]  If point is visible? 0.016028, -0.023911, 0.050268
+[11:52:56.055]  If point is visible? 0.028686, -0.022018, 0.045256
+[11:52:56.055]  If point is visible? 0.019021, -0.024801, 0.048772
+[11:52:56.055]  If point is visible? 0.019079, -0.024570, 0.048865
+[11:52:56.055]  If point is visible? -0.017505, -0.022466, 0.050464
+[11:52:56.055]  If point is visible? -0.017814, -0.022169, 0.050487
+[11:52:56.055]  If point is visible? 0.045583, -0.028017, 0.022278
+[11:52:56.055]  If point is visible? 0.026594, -0.029818, 0.041951
+[11:52:56.055]  If point is visible? 0.026450, -0.030262, 0.041723
+[11:52:56.055]  If point is visible? 0.012977, -0.039745, 0.040110
+[11:52:56.055]  If point is visible? 0.013285, -0.039767, 0.039987
+[11:52:56.055]  If point is visible? 0.013593, -0.039789, 0.039862
+[11:52:56.055]  If point is visible? 0.013690, -0.039594, 0.040022
+[11:52:56.055]  If point is visible? -0.011674, 0.010977, 0.055690
+[11:52:56.055]  If point is visible? -0.014744, 0.014737, 0.054077
+[11:52:56.055]  If point is visible? -0.014718, 0.014253, 0.054213
+[11:52:56.055]  If point is visible? 0.026064, -0.011029, 0.050542
+[11:52:56.055]  If point is visible? 0.026660, -0.011042, 0.050228
+[11:52:56.055]  If point is visible? 0.027841, -0.011064, 0.049579
+[11:52:56.055]  If point is visible? 0.026116, -0.010529, 0.050622
+[11:52:56.055]  If point is visible? 0.026712, -0.010543, 0.050308
+[11:52:56.055]  If point is visible? 0.027892, -0.010564, 0.049659
+[11:52:56.055]  If point is visible? 0.029056, -0.010578, 0.048985
+[11:52:56.055]  If point is visible? 0.027940, -0.010064, 0.049736
+[11:52:56.055]  If point is visible? 0.021839, -0.010380, 0.052637
+[11:52:56.055]  If point is visible? 0.029292, -0.013562, 0.048101
+[11:52:56.055]  If point is visible? 0.020533, -0.010818, 0.053073
+[11:52:56.055]  If point is visible? 0.027934, -0.015028, 0.048471
+[11:52:56.055]  If point is visible? 0.028584, -0.014543, 0.048240
+[11:52:56.055]  If point is visible? 0.017772, -0.020466, 0.051193
+[11:52:56.055]  If point is visible? 0.019040, -0.020542, 0.050704
+[11:52:56.055]  If point is visible? 0.027966, -0.018435, 0.047262
+[11:52:56.055]  If point is visible? 0.015873, -0.023194, 0.050651
+[11:52:56.055]  If point is visible? 0.016515, -0.023239, 0.050425
+[11:52:56.055]  If point is visible? 0.017043, -0.023745, 0.050012
+[11:52:56.055]  If point is visible? 0.018310, -0.023823, 0.049525
+[11:52:56.055]  If point is visible? 0.017674, -0.020939, 0.051035
+[11:52:56.055]  If point is visible? 0.033505, -0.031977, 0.034813
+[11:52:56.055]  If point is visible? 0.033664, -0.031544, 0.035054
+[11:52:56.055]  If point is visible? 0.023273, -0.037710, 0.037325
+[11:52:56.560] add new frame
+[11:52:56.560] update seed
+[11:52:56.560]  If point is visible? 0.029261, -0.014902, 0.047707
+[11:52:56.560]  If point is visible? 0.029062, -0.015267, 0.047713
+[11:52:56.560]  If point is visible? 0.029081, -0.015145, 0.047741
+[11:52:56.560]  If point is visible? 0.030404, -0.017472, 0.046092
+[11:52:56.560]  If point is visible? 0.029295, -0.017344, 0.046854
+[11:52:56.560]  If point is visible? 0.029316, -0.017222, 0.046885
+[11:52:56.560]  If point is visible? 0.029458, -0.017224, 0.046795
+[11:52:56.560]  If point is visible? 0.021653, -0.020605, 0.049613
+[11:52:56.560]  If point is visible? 0.021807, -0.020613, 0.049542
+[11:52:56.560]  If point is visible? 0.021756, -0.020850, 0.049465
+[11:52:56.560]  If point is visible? 0.021910, -0.020858, 0.049394
+[11:52:56.560]  If point is visible? 0.029817, -0.023540, 0.043716
+[11:52:56.560]  If point is visible? 0.021047, -0.024484, 0.048089
+[11:52:56.560]  If point is visible? 0.021077, -0.024368, 0.048134
+[11:52:56.560]  If point is visible? 0.027845, -0.030258, 0.040790
+[11:52:56.560]  If point is visible? 0.037024, -0.030592, 0.032372
+[11:52:56.560]  If point is visible? 0.036656, -0.031570, 0.031848
+[11:52:56.560]  If point is visible? 0.014296, -0.038640, 0.040731
+[11:52:56.560]  If point is visible? 0.021982, -0.039499, 0.036228
+[11:52:56.560]  If point is visible? 0.015657, -0.038119, 0.040720
+[11:52:56.560]  If point is visible? 0.023867, -0.039610, 0.034888
+[11:52:56.560]  If point is visible? 0.021656, -0.040384, 0.035442
+[11:52:56.560]  If point is visible? 0.021796, -0.040386, 0.035355
+[11:52:56.560]  If point is visible? 0.021935, -0.040387, 0.035266
+[11:52:56.560]  If point is visible? 0.033179, -0.007307, 0.046904
+[11:52:56.560]  If point is visible? 0.030688, -0.010327, 0.048019
+[11:52:56.560]  If point is visible? 0.030999, -0.012571, 0.047278
+[11:52:56.560]  If point is visible? 0.029956, -0.011818, 0.048137
+[11:52:56.560]  If point is visible? 0.029776, -0.015275, 0.047268
+[11:52:56.560]  If point is visible? 0.003728, -0.015317, 0.055755
+[11:52:56.560]  If point is visible? 0.029131, -0.017463, 0.046912
+[11:52:56.560]  If point is visible? 0.029415, -0.017466, 0.046732
+[11:52:56.560]  If point is visible? 0.003808, -0.014842, 0.055878
+[11:52:56.560]  If point is visible? 0.021729, -0.016982, 0.050934
+[11:52:56.560]  If point is visible? 0.019161, -0.020463, 0.050688
+[11:52:56.560]  If point is visible? 0.021036, -0.020573, 0.049891
+[11:52:56.560]  If point is visible? 0.018109, -0.020873, 0.050908
+[11:52:56.560]  If point is visible? 0.018427, -0.020894, 0.050785
+[11:52:56.560]  If point is visible? 0.021086, -0.020336, 0.049967
+[11:52:56.560]  If point is visible? 0.017074, -0.023885, 0.049933
+[11:52:56.560]  If point is visible? 0.017392, -0.023908, 0.049812
+[11:52:56.560]  If point is visible? 0.029903, -0.022014, 0.044446
+[11:52:56.560]  If point is visible? 0.020342, -0.024797, 0.048232
+[11:52:56.560]  If point is visible? 0.020403, -0.024567, 0.048324
+[11:52:56.560]  If point is visible? -0.016122, -0.022463, 0.050955
+[11:52:56.560]  If point is visible? -0.016430, -0.022165, 0.050987
+[11:52:56.560]  If point is visible? 0.046145, -0.028014, 0.020999
+[11:52:56.560]  If point is visible? 0.027718, -0.029815, 0.041200
+[11:52:56.560]  If point is visible? 0.027568, -0.030258, 0.040977
+[11:52:56.560]  If point is visible? 0.014055, -0.039741, 0.039745
+[11:52:56.560]  If point is visible? 0.014360, -0.039764, 0.039613
+[11:52:56.560]  If point is visible? 0.014664, -0.039785, 0.039479
+[11:52:56.560]  If point is visible? 0.014765, -0.039591, 0.039637
+[11:52:56.560]  If point is visible? -0.010146, 0.010981, 0.056014
+[11:52:56.560]  If point is visible? -0.013260, 0.014740, 0.054489
+[11:52:56.560]  If point is visible? -0.013230, 0.014257, 0.054624
+[11:52:56.560]  If point is visible? 0.027432, -0.011025, 0.049803
+[11:52:56.560]  If point is visible? 0.028019, -0.011039, 0.049472
+[11:52:56.560]  If point is visible? 0.029180, -0.011060, 0.048790
+[11:52:56.560]  If point is visible? 0.027486, -0.010526, 0.049882
+[11:52:56.560]  If point is visible? 0.028072, -0.010540, 0.049551
+[11:52:56.560]  If point is visible? 0.029234, -0.010561, 0.048869
+[11:52:56.560]  If point is visible? 0.030379, -0.010574, 0.048162
+[11:52:56.560]  If point is visible? 0.029285, -0.010061, 0.048944
+[11:52:56.560]  If point is visible? 0.023268, -0.010376, 0.052017
+[11:52:56.560]  If point is visible? 0.030590, -0.013559, 0.047272
+[11:52:56.560]  If point is visible? 0.021974, -0.010814, 0.052489
+[11:52:56.560]  If point is visible? 0.029242, -0.015024, 0.047680
+[11:52:56.560]  If point is visible? 0.029886, -0.014539, 0.047431
+[11:52:56.560]  If point is visible? 0.019161, -0.020463, 0.050688
+[11:52:56.560]  If point is visible? 0.020415, -0.020539, 0.050164
+[11:52:56.560]  If point is visible? 0.029240, -0.018432, 0.046471
+[11:52:56.560]  If point is visible? 0.017248, -0.023190, 0.050200
+[11:52:56.560]  If point is visible? 0.017883, -0.023235, 0.049956
+[11:52:56.560]  If point is visible? 0.018399, -0.023741, 0.049528
+[11:52:56.560]  If point is visible? 0.019652, -0.023820, 0.049005
+[11:52:56.560]  If point is visible? 0.019059, -0.020936, 0.050533
+[11:52:56.560]  If point is visible? 0.034426, -0.031974, 0.033870
+[11:52:56.560]  If point is visible? 0.034591, -0.031540, 0.034107
+[11:52:56.560]  If point is visible? 0.024269, -0.037707, 0.036670
+[11:53:04.046] DepthFilter stop thread invoked.
+[11:53:04.046] DepthFilter interrupt and join thread 

GPU_version/vio/include/vio/pose_optimizer.h

@@ -42,7 +42,8 @@ void optimizeGaussNewton(
     double& estimated_scale,
     double& error_init,
     double& error_final,
-    size_t& num_obs);
+    size_t& num_obs,
+    FILE* log);
 
 } // namespace pose_optimizer
 } // namespace vio

GPU_version/vio/src/depth_filter.cpp

@@ -136,8 +136,9 @@ namespace vio {
             new_keyframe_set_ = true;
             seeds_updating_halt_ = true;
             frame_queue_cond_.notify_one();
+        }else{
+            initializeSeeds(frame);
         }
-        initializeSeeds(frame);
     }
 
     void DepthFilter::initializeSeeds(FramePtr frame)
@@ -265,6 +266,9 @@ namespace vio {
                 * AngleAxisd(0.0, Vector3d::UnitZ());
             SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
             const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
+#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)  {
                 ++it; // behind the camera
                 continue;

GPU_version/vio/src/frame_handler_base.cpp

@@ -127,9 +127,9 @@ void FrameHandlerBase::optimizeStructure(
   for(auto&& it:frame->fts_){
       if(it->point!=NULL)pts.push_back(it->point);
   }
-  max_n_pts = min(max_n_pts, pts.size());
-  nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);
-  for(deque<Point*>::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it)
+  //max_n_pts = min(max_n_pts, pts.size());
+  //nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);
+  for(deque<Point*>::iterator it=pts.begin(); it!=pts.begin(); ++it)
   {
     (*it)->optimize(max_iter);
     (*it)->last_structure_optim_ = frame->id_;

GPU_version/vio/src/frame_handler_mono.cpp

@@ -197,10 +197,18 @@ 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(), Config::poseOptimNumIter(), false,
-            new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
-  if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
+            Config::poseOptimThresh(), 70, false,
+            new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);
+#if VIO_DEBUG
+    fprintf(log_,"[%s] After pose optimization, distance between two frames:%f ,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+            (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm(),
+            last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch());
+#endif
+    if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
        fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI_4){
+        auto init_f= ukfPtr_.get_location();
+        new_frame_->T_f_w_=init_f.second;
+        new_frame_->Cov_ = init_f.first;
         return RESULT_FAILURE;
     }
   auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),

GPU_version/vio/src/pose_optimizer.cpp

@@ -34,7 +34,8 @@ void optimizeGaussNewton(
     double& estimated_scale,
     double& error_init,
     double& error_final,
-    size_t& num_obs)
+    size_t& num_obs,
+    FILE* log)
 {
   // init
   double chi2(0.0);
@@ -57,10 +58,14 @@ void optimizeGaussNewton(
     e *= 1.0 / (1<<(*it)->level);
     errors.push_back(e.norm());
   }
+
   if(errors.empty())
     return;
   vk::robust_cost::MADScaleEstimator scale_estimator;
   estimated_scale = 0.5*scale_estimator.compute(errors);
+#if VIO_DEBUG
+    fprintf(log,"[%s] Init estimate scale:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),estimated_scale);
+#endif
 
   num_obs = errors.size();
   chi2_vec_init.reserve(num_obs);
@@ -70,9 +75,6 @@ void optimizeGaussNewton(
   size_t n_direction=0;
   for(size_t iter=0; iter<n_iter; iter++)
   {
-    // overwrite scale
-    if(iter == 5)
-      scale = 0.85/frame->cam_->errorMultiplier2();
 
     b.setZero();
     A.setZero();
@@ -87,6 +89,11 @@ void optimizeGaussNewton(
       Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
       Frame::jacobian_xyz2uv(xyz_f, 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)
@@ -97,39 +104,31 @@ void optimizeGaussNewton(
       b.noalias() -= J.transpose()*e*weight;
       new_chi2 += e.squaredNorm()*weight;
     }
+
     // solve linear system
-    const Vector3d dT(A.ldlt().solve(b));
+    Vector3d dT(A.ldlt().solve(b));
 
     // check if error increased
-    if((iter > 0 && new_chi2 > chi2 && n_direction>3) || (bool) std::isnan((double)dT[0]))
+    if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
     {
-      if(verbose)
-        std::cout << "it " << iter
-                  << "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl;
+#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
       frame->T_f_w_ = T_old; // roll-back
       break;
-    }else if((iter > 0 && new_chi2 > chi2 && n_direction<3) ){
-        ++n_direction;
-        scale *=0.5;
-        if(direction){
-            direction=false;
-        }else{
-            direction=true;
-        }
     }
-
+    scale *=0.5;
+    dT=0.1*dT;
     // update the model
     T_old = frame->T_f_w_;
-    if(direction){
-        frame->T_f_w_=SE2_5(SE2::exp(dT)*(T_old.se2()));
-    }else{
-        frame->T_f_w_=SE2_5(SE2::exp(-1.0*dT)*(T_old.se2()));
-    }
+    frame->T_f_w_=SE2_5(T_old.se2().translation().y()+dT.x(),T_old.se2().translation().y()+dT.y(),T_old.pitch()+dT.z());
+
     chi2 = new_chi2;
-    if(verbose)
-      std::cout << "it " << iter
-                << "\t Success \t new_chi2 = " << new_chi2
-                << "\t norm(dT) = " << vk::norm_max(dT) << std::endl;
+#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
 
     // stop when converged
     if(vk::norm_max(dT) <= EPS)