From 604ae2629789e5113155c0483c3e992c4ee926ff Mon Sep 17 00:00:00 2001 From: VahdPourAmin Date: Sun, 12 Sep 2021 21:48:50 +0200 Subject: [PATCH] Fixed some bugs and got better results --- .../vio/include/vio/sparse_img_align_gpu.h | 3 +- GPU_version/vio/kernel/compute-residual.cl | 54 +++++++++++++++---- GPU_version/vio/launch/Px30.launch | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 46 +++++++++++++++- GPU_version/vio/src/sparse_img_align_gpu.cpp | 22 ++++++-- 5 files changed, 108 insertions(+), 19 deletions(-) diff --git a/GPU_version/vio/include/vio/sparse_img_align_gpu.h b/GPU_version/vio/include/vio/sparse_img_align_gpu.h index 27d7b53..57a7bdd 100644 --- a/GPU_version/vio/include/vio/sparse_img_align_gpu.h +++ b/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; diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index c452e1f..037246e 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -43,12 +43,43 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature) void jacobian_xyz2uv(float3 xyz_in_f, float* J) { - J[0] = -(1. / xyz_in_f.z); // -1/z + J[0] = -(1. / xyz_in_f.z); // -1/z J[1] = (xyz_in_f.x) / (xyz_in_f.z * xyz_in_f.z); // x/z^2 J[2] = -(1.0 + pow((xyz_in_f.x),2) / (xyz_in_f.z * xyz_in_f.z)); // -(1.0 + x^2/z^2) - J[3] = 1e-19; // 0 + J[3] = 1e-19; // 0 J[4] = (xyz_in_f.y) / (xyz_in_f.z * xyz_in_f.z); // y/z^2 - J[5] = -(xyz_in_f.x) * (xyz_in_f.y) / (xyz_in_f.z * xyz_in_f.z); // -x*y/z^2 + 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) @@ -68,15 +99,15 @@ __kernel void compute_residual( __read_only image2d_t image_cur, // current frame __read_only image2d_t image_ref, // reference frame __global float3 * cur_pose,//[reference frame pose{x,z,pitch}] -__global float3 * ref_pose,//[current frame pose{x,z,pitch}] -__global float3 * ref_feature, // feature on the reference frame, when we applied the distance calculation: xyz_ref((*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm()); -__global float2 * featue_px, -int level, + __global float3 * ref_pose,//[current frame pose{x,z,pitch}] + __global float3 * ref_feature, // feature on the reference frame, when we applied the distance calculation: xyz_ref((*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm()); + __global float2 * featue_px, + int level, __global float * errors, - __global float * Hessian, + __global float * Hessian, __global float3 * Jacobian, - __global float * chi, -float scale_ + __global float * chi, + float scale_ ) { float scale = pow(2.0, -level); @@ -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; diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index d12f08d..51ef079 100644 --- a/GPU_version/vio/launch/Px30.launch +++ b/GPU_version/vio/launch/Px30.launch @@ -1,6 +1,6 @@ - + diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 710fe0d..56c102e 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/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; diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index 1ce97e2..ff2eb8f 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/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 @@ -67,8 +79,8 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame) residual_->reload_buf(1,0,cur_img); residual_->reload_buf(1,1,ref_img); residual_->write_buf(1,6,level_); - mu_ = 1.0; - optimize(T_cur); + mu_ = 1.0; + optimize(T_cur); } cl_float3 pos[1]={0}; residual_->read(1,2,1,pos);