public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

debug crashing

Browse Source
This commit is contained in:
gfuse
2021-08-05 14:54:44 +02:00
parent 47e2993ed8
commit e20e2c76e1
9 changed files with 29 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/cl_class.h
+14 -4
View File
@@ -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"<<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
-1
View File
@@ -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
-1
View File
@@ -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
-1
View File
@@ -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
+1 -1
View File
@@ -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
+1 -1
View File
@@ -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
+1 -1
View File
@@ -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
+3 -7
View File
@@ -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, 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;
GPU_version/vio/src/sparse_img_align_gpu.cpp
+9 -5
View File
@@ -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());