Fixed some bugs and got better results

GPU_version/vio/include/vio/sparse_img_align_gpu.h

@@ -52,7 +52,8 @@ public:
 
   size_t run(
       FramePtr ref_frame,
-      FramePtr cur_frame);
+      FramePtr cur_frame,
+      FILE* log);
 
    // FILE* data= nullptr;
 

GPU_version/vio/kernel/compute-residual.cl

@@ -51,6 +51,37 @@ void jacobian_xyz2uv(float3 xyz_in_f, float* J)
     J[5] = -(xyz_in_f.x) * (xyz_in_f.y) / (xyz_in_f.z * xyz_in_f.z);   // -x*y/z^2
 }
 
+void jacobian_xyz2uv_(float3 xyz_in_f, float3 cur_p, float* J)
+{
+    double x_n = xyz_in_f.x;
+    double y_n = xyz_in_f.y;
+    double z_n = xyz_in_f.z;
+
+    double r = sqrt(pow(xyz_in_f.x/xyz_in_f.z, 2) + pow(xyz_in_f.y/xyz_in_f.z, 2));
+
+    double x_c = cur_p.x;
+    double z_c = cur_p.y;
+    double theta = cur_p.z;
+
+    double alpha = (F_X*(theta/r))-(F_X*((x_n*x_n)/(r*r))*theta)+((1+3*S*theta*theta)/((r*r)+1))*((F_X*x_n*x_n)/(r*r));
+    double beta  =                -(F_X*((x_n*y_n)/(r*r))*theta)+((1+3*S*theta*theta)/((r*r)+1))*((F_X*x_n*y_n)/(r*r));
+    double gamma =                -(F_Y*((x_n*y_n)/(r*r))*theta)+((1+3*S*theta*theta)/((r*r)+1))*((F_Y*x_n*y_n)/(r*r));
+    double lamda = (F_Y*(theta/r))-(F_Y*((y_n*y_n)/(r*r))*theta)+((1+3*S*theta*theta)/((r*r)+1))*((F_Y*y_n*y_n)/(r*r));
+
+    double Xf_Xc = x_n - x_c;
+    double Zf_Zc = z_n - z_c;
+    double n1 = -1*sin(theta)*Xf_Xc + cos(theta)*Zf_Zc;
+    double n2 = -1*cos(theta)*Xf_Xc - sin(theta)*Zf_Zc;
+
+    J[0] = ((-1*cos(theta)/z_n)*alpha)-((sin(theta)/z_n)*beta)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*n1);
+    J[1] = 0;
+    J[2] = ((sin(theta)/z_n)*alpha)-((cos(theta)/z_n)*beta)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*n2);
+
+    J[3] = ((-1*cos(theta)/z_n)*gamma)-((sin(theta)/z_n)*lamda)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n1);
+    J[4] = 0;
+    J[5] = ((sin(theta)/z_n)*gamma)-((cos(theta)/z_n)*lamda)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n2);
+}
+
 void compute_hessain(float3 j, __global float* H, float w)
 {
     H[0] += j.x * j.x * w;
@@ -90,7 +121,8 @@ float2 uv_ref_i = floor(uv_ref);
 if(uv_ref_i.x - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.y - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.x + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref).x || uv_ref_i.y + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref).y)return;
 // evaluate projection jacobian
 float frame_jac[6]={0.0}; // 2X3
-jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (float3)(ref_pose[0].x, 0.0, ref_pose[0].y), 2.0)), &frame_jac);
+//jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (float3)(ref_pose[0].x, 0.0, ref_pose[0].y), 2.0)), &frame_jac);
+jacobian_xyz2uv_(ref_feature[f], cur_pose[0], &frame_jac);
 
 // compute bilateral interpolation weights for reference image
 float2 subpix=  uv_ref - uv_ref_i;

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

@@ -176,7 +176,51 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   new_frame_->Cov_ = init_f.first;
   // sparse image align
   SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
-  if(!img_align.run(last_frame_, new_frame_))new_frame_->T_f_w_=init_f.second;
+/*#if VIO_DEBUG
+    fprintf(log_,"[%s] Before alignment, distance between two frames x:%f ,z=%f,angle between two frames:%f last_frame feature size : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+            new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
+            new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
+            fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()), last_frame_->fts_.size());
+    int nullcount = 0;
+    int nullpointcount = 0;
+    for(auto f = last_frame_->fts_.begin(); f!= last_frame_->fts_.end(); f++)
+    {
+        if(*f == nullptr) {
+            nullcount++;
+            continue;
+        }
+        if((*f)->point == nullptr) {
+            nullpointcount++;
+            continue;
+        }
+        fprintf(log_,"[%s] last frame : %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+                (*f)->point->pos_.x(), (*f)->point->pos_.y(), (*f)->point->pos_.z());
+    }
+    fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
+#endif*/
+  if(!img_align.run(last_frame_, new_frame_, log_))new_frame_->T_f_w_=init_f.second;
+/*#if VIO_DEBUG
+    fprintf(log_,"[%s] After alignment, distance between two frames x:%f ,z=%f,angle between two frames:%f last_frame feature size : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+            new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
+            new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
+            fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()), last_frame_->fts_.size());
+    nullcount = 0;
+    nullpointcount = 0;
+    for(auto f = last_frame_->fts_.begin(); f!= last_frame_->fts_.end(); f++)
+    {
+        if(*f == nullptr) {
+            nullcount++;
+            continue;
+        }
+        if((*f)->point == nullptr) {
+            nullpointcount++;
+            continue;
+        }
+        fprintf(log_,"[%s] last frame : %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+                (*f)->point->pos_.x(), (*f)->point->pos_.y(), (*f)->point->pos_.z());
+    }
+    fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
+#endif*/
   reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
   int n_point=0;
   for(auto i:overlap_kfs_)n_point+=i.second;

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -32,15 +32,23 @@ SparseImgAlignGpu::SparseImgAlignGpu(
   eps_ = 1e-10;
 }
 
-size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
+size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
 {
   reset();
-  if(ref_frame->fts_.empty())
+  /*if(ref_frame->fts_.empty()) // more than 10
   {
     return 0;
-  }
+  }*/
   cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()};
+/*#if VIO_DEBUG
+    fprintf(log,"[%s] ref pose : , x:%f, z=%f, pitch:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+            ref_pos[0].x, ref_pos[0].y, ref_pos[0].z);
+#endif*/
   cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()};
+/*#if VIO_DEBUG
+    fprintf(log,"[%s] cur pose : , x:%f, z=%f, pitch:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+            cur_pos[0].x, cur_pos[0].y, cur_pos[0].z);
+#endif*/
   residual_->reload_buf(1,2,cur_pos);
   residual_->reload_buf(1,3,ref_pos);
   cl_float3 features[ref_frame->fts_.size()];
@@ -57,6 +65,10 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
         featue_px[feature_counter_].y=(*it)->px(1);
         ++feature_counter_;
   }
+  if(!feature_counter_) // more than 10
+  {
+      return 0;
+  }
   residual_->reload_buf(1,4,features);
   residual_->reload_buf(1,5,featue_px);
   SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it