From d35296c6550a0ba81db133529325689b2f7900a2 Mon Sep 17 00:00:00 2001 From: gfuse Date: Thu, 12 Aug 2021 10:14:33 +0200 Subject: [PATCH] debug the reading kernel buffer --- GPU_version/vio/kernel/compute-residual.cl | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 4 +++- GPU_version/vio/src/sparse_img_align_gpu.cpp | 2 +- GPU_version/vio/src/vo_node.cpp | 2 +- 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index baf62f5..c5b899f 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -74,7 +74,7 @@ __kernel void compute_residual( 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)[0] || uv_ref_i.y + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[1])return; // evaluate projection jacobian double frame_jac[6]={0.0}; // 2X3 - jacobian_xyz2uv(sqrt(pow(ref_feature[f] - ref_pose[0], 2.0)), &frame_jac); + jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (double3)(ref_pose[0].x,0.0,ref_pose[0].y), 2.0)), &frame_jac); // compute bilateral interpolation weights for reference image double2 subpix= uv_ref - uv_ref_i; diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 84cb7b3..e8a91cf 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -164,10 +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: "<T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; + (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( diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index 431e041..418fcdd 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -78,7 +78,7 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame) optimize(T_cur); } cl_double3 pos[1]={0}; - residual_->read(0,2,pos); + residual_->read(1,2,pos); cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z); return 1; } diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 2060ea0..489d7f2 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -92,7 +92,7 @@ private: ros::Time imu_time_; boost::thread* imu_the_= nullptr; #if VIO_DEBUG - svo::Visualizer visualizer_; + vio::Visualizer visualizer_; #endif };