debug crashing

GPU_version/vio/include/vio/cl_class.h

@@ -57,8 +57,14 @@ public:
     int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
         try{
             if(id>_buffers.size())return CL_MAP_FAILURE;
-            for(auto i:_buffers)if(i.second==id)
+            for(auto&& i:_buffers)if(i.second==id){
-                queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf);
+                    cl_int err1=queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf);
+                    cl_int err2=_kernel->setArg(id,i.first.first);
+                    if(err1!=0 || err2!=0){
+                        std::cerr<<"Error C:Kernel, F:write, L3:setArg\t"<<id<<"\t"<<err1<<"\t"<<err2<<'\n';
+                        return CL_MAP_FAILURE;
+                    }
+            }
             return CL_SUCCESS;
         }catch (std::exception& err){
             std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
@@ -67,15 +73,19 @@ public:
     };
     int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){
         try{
-            for(auto i:_images)if(i.second==id){
+            for(auto&& i:_images)if(i.second==id){
                     i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
                                         cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
                                         buf.size().width,
                                         buf.size().height,
                                         0,
                                         reinterpret_cast<uchar*>(buf.data));
+                    cl_int err=_kernel->setArg(id,i.first);
+                    if(err!=0){
+                        std::cerr<<"Error C:Kernel, F:write image, L:setArg ID:\t"<<id<<"\t"<<err<<'\n';
+                        return err;
+                    }
             }
-
             return CL_SUCCESS;
         }catch (std::exception& err){
             std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';

GPU_version/vio/include/vio/frame_handler_mono.h

@@ -78,7 +78,6 @@ protected:
   DepthFilter* depth_filter_;                   //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
   opencl* gpu_fast_;
   ros::Time time_;
-  SparseImgAlignGpu* imageAlign_;
 
 
   /// Initialize the visual odometry algorithm.

GPU_version/vio/include/vio/nlls_solver_impl.hpp

@@ -60,7 +60,6 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
       std::cout << "It. " << iter_
                 << "\t Success"
                 << "\t new_chi2 = " << new_chi2
-                << "\t n_meas = " << n_meas_
                 << "\t x_norm = " << vk::norm_max(x_)<<'\n';
     }
     // stop when converged, i.e. update step too small

GPU_version/vio/kernel/compute-residual.cl

@@ -94,7 +94,6 @@ __kernel void compute_residual(
     double v_ref = featue_px[f].y * scale;
     int u_ref_i = floor(u_ref);
     int v_ref_i = floor(v_ref);
-
     if(u_ref_i - (PATCH_HALFSIZE + 1) < 0 || v_ref_i - (PATCH_HALFSIZE + 1) < 0 ||
     u_ref_i + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[0] || v_ref_i + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[1])
     return;

GPU_version/vio/kernel/fast-gray.cl

@@ -17,7 +17,7 @@ __kernel void fast_gray(
     // Use global work item as 2D image coordinates.
     int  const x   = get_global_id(0);
     int  const y   = get_global_id(1);
-    if(x>16 && x<get_image_width(image)-16 && y<get_image_height(image)-16 && y>16){
+    if(x>5 && x<get_image_width(image)-5 && y<get_image_height(image)-5 && y>5){
         int2 const xy  = (int2)(x, y);
         // Read the candidate pixel.
         int  const p00 = read_imageui(image, sampler, xy).x;

GPU_version/vio/param/vo_fast.yaml

@@ -3,7 +3,7 @@ svo:
   max_n_kfs: 20            #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed.  Minimum number of keyframes is 3.
   loba_num_iter: 0         #Number of iterations in the local bundle adjustment.
   quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
-  quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. 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
   structureoptim_max_pts: 50

GPU_version/vio/src/feature_detection.cpp

@@ -79,7 +79,7 @@ void FastDetector::detect(
     gpu_fast_->reload_buf(0,0,img);
     int icorner[1]={0};
     gpu_fast_->reload_buf(0,2,icorner);
-    gpu_fast_->run(0,img_pyr[L].cols,img_pyr[L].rows);
+    gpu_fast_->run(0,img.cols,img.rows);
     cl_int2 fast_corners[350000];
     gpu_fast_->read(0,1,fast_corners);
     int count[1]={0};

GPU_version/vio/src/frame_handler_mono.cpp

@@ -66,8 +66,6 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
     gpu_fast_->write_buf(1,10,3,J);
     double chi2[1]={0.0};
     gpu_fast_->write_buf(1,11,1,chi2);
-    imageAlign_=new SparseImgAlignGpu(Config::kltMaxLevel(), Config::kltMinLevel(),
-                                      30, SparseImgAlign::GaussNewton, true,gpu_fast_);
     klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
     initialize();
 }
@@ -164,11 +162,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   new_frame_->T_f_w_=init_f.second;
   new_frame_->Cov_ = init_f.first;
   // sparse image align
-  /*
+  //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, true);
-  SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
+  SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::GaussNewton, false,gpu_fast_);
-                           30, SparseImgAlign::LevenbergMarquardt, false, false);
+  if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
-  if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;*/
-  if(imageAlign_->run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
   size_t sfba_n_edges_final=0;
   double sfba_thresh, sfba_error_init, sfba_error_final;

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -49,6 +49,8 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
   cl_double2 featue_px[ref_frame->fts_.size()];
   feature_counter_ = 0; // is used to compute the index of the cached jacobian
   for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();++it){
+      if(*it == nullptr)continue;
+      if((*it)->point == nullptr)continue;
         Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm();
         features[feature_counter_].x=xyz_ref(0);
         features[feature_counter_].y=xyz_ref(1);
@@ -62,17 +64,19 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
   SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it
   for(level_=max_level_; level_>=min_level_; --level_)
   {
-      cv::Mat& cur_img = cur_frame->img_pyr_.at(level_);
+      cv::Mat cur_img = cur_frame->img_pyr_.at(level_);
-      cv::Mat& ref_img = ref_frame->img_pyr_.at(level_);
+      cv::Mat ref_img = ref_frame->img_pyr_.at(level_);
       residual_->reload_buf(1,0,cur_img);
       residual_->reload_buf(1,1,ref_img);
       residual_->reload_buf(1,6,&level_);
     mu_ = 1.0;
     optimize(T_cur);
   }
-  cur_frame->T_f_w_ = SE2_5(T_cur);
+  double pos[3]={0};
+  residual_->read(1,2,pos);
+  cur_frame->T_f_w_ = SE2_5(pos[0],pos[1],pos[2]);
 
-  return n_meas_/patch_area_;
+  return 1;
 }
 
 double SparseImgAlignGpu::computeResiduals(
@@ -111,7 +115,7 @@ void SparseImgAlignGpu::update()
 /// TODO the update situation may have a smarter solution
   double pos[3]={0};
   residual_->read(1,2,pos);
-  SE2 update =  SE2(pos[2],Eigen::Vector2d(pos[0],pos[1])) * SE2::exp(-0.25*x_);
+  SE2 update =  SE2(pos[2],Eigen::Vector2d(pos[0],pos[1])) * SE2::exp(-0.5*x_);
   pos[0]=update.translation()(0);
   pos[1]=update.translation()(1);
   pos[2]=atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());