debug files

GPU_version/vio/include/vio/nlls_solver_impl.hpp

@@ -37,7 +37,7 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
     if(!solve())
     {
       // matrix was singular and could not be computed
-      std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
+      if(verbose_)std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
       stop_ = true;
     }
     // check if error increased since last optimization

GPU_version/vio/kernel/compute-residual.cl

@@ -8,9 +8,9 @@ float2 world2cam(float3 feature)
 {
     float r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2));
     float factor = 1.0;
-    if(S != 0 || r > 0.001)
-        factor = (atan(r * 2.0 * tan(0.5 * S)) / (r * S));
-    return (float2)(C_X + F_X * factor * feature.x/feature.z, C_Y + F_Y * factor * feature.y/feature.z);
+    if((float)S != 0 || r > 0.001)
+        factor = (atan(r * 2.0 * tan(0.5 * (float)S)) / (r * (float)S));
+    return (float2)((float)C_X + (float)F_X * factor * feature.x/feature.z, (float)C_Y + (float)F_Y * factor * feature.y/feature.z);
 }
 
 float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
@@ -142,7 +142,7 @@ __kernel void compute_residual(
 chi_ += pow(res,2) * weight;
 float3 j_row0 = (float3)((dx * frame_jac[0]), (dx * frame_jac[1]), (dx * frame_jac[2]));
 float3 j_row1 = (float3)((dy * frame_jac[3]), (dy * frame_jac[4]), (dy * frame_jac[5]));
-            float3 J = (j_row0 + j_row1) * (F_X / scale);
+float3 J = (j_row0 + j_row1) * ((float)F_X / scale);
 compute_hessain(J, &Hessian[f*9], weight);
 Jacobian[f] -= J * res * weight;
 
@@ -151,4 +151,3 @@ __kernel void compute_residual(
 errors[f] = e / pow(PATCH_SIZE,2.0);
 chi[f]=chi_;
 }
-

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,7 +5,7 @@ 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: 60
+  init_min_disparity: 40
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
   poseoptim_thresh: 4.0

GPU_version/vio/src/frame_handler_mono.cpp

@@ -148,7 +148,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
   map_.addKeyframe(new_frame_);
   stage_ = STAGE_DEFAULT_FRAME;
   klt_homography_init_->reset();
-  SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
+  SVO_INFO_STREAM("Init: triangulated initial map. Vio is starting ...");
   return RESULT_IS_KEYFRAME;
 }
 
@@ -164,12 +164,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
   //assert (false);
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-
+/*
   std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
              <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
              <<"\t distance between two frames: "<<
              (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
-
+*/
   size_t sfba_n_edges_final=0;
   double sfba_thresh, sfba_error_init, sfba_error_final;
   pose_optimizer::optimizeGaussNewton(

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -128,8 +128,10 @@ int SparseImgAlignGpu::solve()
         J[0].y -= J[i].y;
         J[0].z -= J[i].z;
     }
-    double Hd[9]={};
-    double Jd[3]={};
+    double Hd[9]={(double)H[0],(double)H[1],(double)H[2],
+                  (double)H[3],(double)H[4],(double)H[5],
+                  (double)H[6],(double)H[7],(double)H[8]};
+    double Jd[3]={(double)J[0].x,(double)J[0].y,(double)J[0].z};
     x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
     if((bool) std::isnan((double) x_[0]))
         return 0;
@@ -140,7 +142,7 @@ void SparseImgAlignGpu::update()
 /// TODO the update situation may have a smarter solution
     cl_float3 pos[1]={0.0,0.0,0.0};
     residual_->read(1,2,pos);
-    Sophus::SE2 update =  Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-0.5*x_);
+    Sophus::SE2 update =  Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-1.0*x_);
     pos[0].x=(float)update.translation()(0);
     pos[0].y=(float)update.translation()(1);
     pos[0].z=(float)atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());