diff --git a/GPU_version/vio/include/vio/cl_class.h b/GPU_version/vio/include/vio/cl_class.h index 9ff34e5..2b30b3b 100644 --- a/GPU_version/vio/include/vio/cl_class.h +++ b/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) - queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf); + for(auto&& i:_buffers)if(i.second==id){ + 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"<(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"<::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 diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index d02fd2b..6d11d83 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/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; diff --git a/GPU_version/vio/kernel/fast-gray.cl b/GPU_version/vio/kernel/fast-gray.cl index ce6a927..7622206 100644 --- a/GPU_version/vio/kernel/fast-gray.cl +++ b/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 && x16){ + if(x>5 && x5){ int2 const xy = (int2)(x, y); // Read the candidate pixel. int const p00 = read_imageui(image, sampler, xy).x; diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index 1ecb630..b4e3359 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/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 diff --git a/GPU_version/vio/src/feature_detection.cpp b/GPU_version/vio/src/feature_detection.cpp index d3eb264..d19b427 100644 --- a/GPU_version/vio/src/feature_detection.cpp +++ b/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}; diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index eddcafc..f59dd72 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -66,8 +66,6 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrixwrite_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, false); - if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;*/ - if(imageAlign_->run(last_frame_, new_frame_)==0)return RESULT_FAILURE; + //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, true); + SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::GaussNewton, false,gpu_fast_); + if(img_align.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; diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index b2eaf4f..617c690 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/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& ref_img = ref_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_); 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());