From 91a01ec03d4a1a9854a041816bba26802b8f73da Mon Sep 17 00:00:00 2001 From: gfuse Date: Thu, 7 Oct 2021 19:36:49 +0200 Subject: [PATCH] add more restriction --- GPU_version/vio/include/sophus/so3.cpp | 2 +- GPU_version/vio/include/vio/atan_camera.h | 8 ++++++-- GPU_version/vio/include/vio/matcher.h | 4 ++-- GPU_version/vio/param/vo_fast.yaml | 8 ++++---- GPU_version/vio/src/cl_class.cpp | 2 +- GPU_version/vio/src/feature_alignment.cpp | 6 +++--- GPU_version/vio/src/frame.cpp | 1 - GPU_version/vio/src/frame_handler_mono.cpp | 8 ++++++-- GPU_version/vio/src/global_optimizer.cpp | 10 ++++------ GPU_version/vio/src/matcher.cpp | 4 ++-- GPU_version/vio/src/math_utils.cpp | 4 ++-- 11 files changed, 31 insertions(+), 26 deletions(-) diff --git a/GPU_version/vio/include/sophus/so3.cpp b/GPU_version/vio/include/sophus/so3.cpp index c3ab232..465d95b 100644 --- a/GPU_version/vio/include/sophus/so3.cpp +++ b/GPU_version/vio/include/sophus/so3.cpp @@ -161,7 +161,7 @@ Vector3d SO3 two_atan_nbyw_by_n = -M_PI/n; } } - two_atan_nbyw_by_n = 2*atan(n/w)/n; + two_atan_nbyw_by_n = 2*atan2(n,w)/n; } *theta = two_atan_nbyw_by_n*n; diff --git a/GPU_version/vio/include/vio/atan_camera.h b/GPU_version/vio/include/vio/atan_camera.h index 1186f26..96ff53a 100644 --- a/GPU_version/vio/include/vio/atan_camera.h +++ b/GPU_version/vio/include/vio/atan_camera.h @@ -43,8 +43,12 @@ private: { if(r < 0.001 || s_ == 0.0) return 1.0; - else - return (s_inv_* atan(r * tans_) / r); + else{ + float y=2*r*sin(0.5 * (float)s_); + float x=cos(0.5 * (float)s_); + return (atan2(y,x) / (r * (float)s_)); + //return (s_inv_* atan(r * tans_) / r); + } }; //! Inverse radial distortion: returns un-distorted radius from distorted. diff --git a/GPU_version/vio/include/vio/matcher.h b/GPU_version/vio/include/vio/matcher.h index 0823a6e..ef4df32 100644 --- a/GPU_version/vio/include/vio/matcher.h +++ b/GPU_version/vio/include/vio/matcher.h @@ -66,8 +66,8 @@ class Matcher public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - static const int halfpatch_size_ = 8; - static const int patch_size_ = 16; + static const int halfpatch_size_ = 4; + static const int patch_size_ = 8; typedef vk::patch_score::ZMSSD PatchScore; diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index 422fc79..53d4fd0 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -9,10 +9,10 @@ vio: structureoptim_max_pts: 50 kfselect_mindist: 0.01 poseoptim_thresh: 8.0 - init_min_inliers: 50 + init_min_inliers: 40 ACC_ekf: 0.2 #acc white noise in continuous in EKF - GYO_ekf: 0.05 #gyro white noise in continuous in EKF - VO_ekf_translation: 0.5 #VO noise in EKF - VO_ekf_orientation: 1.5 #VO noise in EKF + GYO_ekf: 0.1 #gyro white noise in continuous in EKF + VO_ekf_translation: 1.0 #VO noise in EKF + VO_ekf_orientation: 3.0 #VO noise in EKF CMD_ekf_translation: 0.1 #control vector noise in EKF CMD_ekf_orientation: 0.01 #control vector noise in EKF diff --git a/GPU_version/vio/src/cl_class.cpp b/GPU_version/vio/src/cl_class.cpp index 449611a..f8464b8 100644 --- a/GPU_version/vio/src/cl_class.cpp +++ b/GPU_version/vio/src/cl_class.cpp @@ -38,7 +38,7 @@ opencl::opencl(vk::AbstractCamera* cam):cam(cam) { sources.push_back({ compute_residual.src_str, compute_residual.size }); program=new cl::Program(*context, sources); double* camera=cam->params(); - std::string options="-DFAST_THRESH=40 -DPATCH_SIZE=2 -DPATCH_HALFSIZE=1 -DF_X="+ std::to_string(camera[0]) + + std::string options="-DFAST_THRESH=40 -DPATCH_SIZE=8 -DPATCH_HALFSIZE=4 -DF_X="+ std::to_string(camera[0]) + " -DF_Y="+std::to_string(camera[1])+ " -DC_X="+std::to_string(camera[2])+ " -DC_Y="+std::to_string(camera[3])+ diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp index 78fdb46..9032b76 100644 --- a/GPU_version/vio/src/feature_alignment.cpp +++ b/GPU_version/vio/src/feature_alignment.cpp @@ -163,9 +163,9 @@ bool align2D( return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate); #endif - const int halfpatch_size_ = 8; - const int patch_size_ = 16; - const int patch_area_ = 256; + const int halfpatch_size_ = 4; + const int patch_size_ = 8; + const int patch_area_ = 64; bool converged=false; // compute derivative of template and prepare inverse compositional diff --git a/GPU_version/vio/src/frame.cpp b/GPU_version/vio/src/frame.cpp index 3f58dfc..460a2c3 100644 --- a/GPU_version/vio/src/frame.cpp +++ b/GPU_version/vio/src/frame.cpp @@ -178,7 +178,6 @@ bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min) { double z = frame->w2f((*it)->point->pos_).z(); depth_vec.push_back(z); - } } if(depth_vec.empty()) diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index ef71ad7..85c6b01 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -179,14 +179,18 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() fprintf(log_,"[%s] Update EKF and 3D points the number of feature in the new frame: %d and number of obs: %d\n",vio::time_in_HH_MM_SS_MMM().c_str(), new_frame_->fts_.size(),new_frame_->nObs()); #endif + double depth_mean=0.0, depth_min=0.0; + frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min); + if(depth_mean>10.0)return RESULT_NO_KEYFRAME; + optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); // select keyframe + if(!needNewKf())//edited { return RESULT_NO_KEYFRAME; } - double depth_mean=0.0, depth_min=0.0; - frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min); + new_frame_->setKeyframe(); #if VIO_DEBUG fprintf(log_,"[%s] Choose frame as a key frame Scene Depth mean:%f ,depth min:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(), diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp index ba4851f..69ea6c1 100644 --- a/GPU_version/vio/src/global_optimizer.cpp +++ b/GPU_version/vio/src/global_optimizer.cpp @@ -164,9 +164,6 @@ namespace vio { if((*it_ftr)->point == NULL) continue; std::shared_ptr mp = (*it_ftr)->point; - if(mp->v_pt_ == NULL) - continue; // mp was updated before - mp->pos_ = mp->v_pt_->estimate(); mp->v_pt_ = NULL; } } @@ -192,7 +189,7 @@ namespace vio { { g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap(); v->setId(id); - v->setFixed(fixed); + v->setFixed(false); v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation())); return v; } @@ -204,7 +201,8 @@ namespace vio { { g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ(); v->setId(id); - v->setMarginalized(fixed); + v->setFixed(fixed); + v->setMarginalized(true); v->setEstimate(pos); return v; @@ -221,7 +219,7 @@ namespace vio { e->setVertex(0, dynamic_cast(v_point)); e->setVertex(1, dynamic_cast(v_frame)); e->setMeasurement(f_up); - e->information() = Eigen::Matrix2d::Identity(); + e->information() = weight*Eigen::Matrix2d::Identity(); g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; rk->setDelta(huber_width); e->setRobustKernel(rk); diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index c85f9b8..f352c12 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -41,7 +41,7 @@ void getWarpMatrixAffine( Matrix2d& A_cur_ref) { // Compute affine warp matrix A_ref_cur - const int halfpatch_size = 8; + const int halfpatch_size = 4; Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<= 0.75) + if(res[0] >= 0.85) success = true; else success = false; diff --git a/GPU_version/vio/src/math_utils.cpp b/GPU_version/vio/src/math_utils.cpp index 777daac..e109a86 100644 --- a/GPU_version/vio/src/math_utils.cpp +++ b/GPU_version/vio/src/math_utils.cpp @@ -36,8 +36,8 @@ triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, Vector3d b1 = t.cross(R_F2_p); double lamda0 = z.dot(b0)/pow(z.norm(), 2); double lamda1 = z.dot(b1)/pow(z.norm(), 2); - //if (lamda0 <= 0 || lamda1 <= 0)return Vector3d(0.0,0.0,0.0); - if (max(m0_hat.dot(R_F2_p), feature1.dot(f1_p)) > M_PI_2)return Vector3d(0.0,0.0,0.0); + if (lamda0 <= 0 || lamda1 <= 0)return Vector3d(0.0,0.0,0.0); + if (max(m0_hat.dot(R_F2_p), feature1.dot(f1_p)) > 0.70*M_PI_2)return Vector3d(0.0,0.0,0.0); if(R_F2_p.dot(f1_p) < 0.01)return Vector3d(0.0,0.0,0.0); Vector3d xm = lamda1 * f1_p; Vector3d xn = t + lamda0 * R_F2_p;