From 4cb5cbff09204a532c37368f1203422cb5f8f383 Mon Sep 17 00:00:00 2001 From: gfuse Date: Tue, 13 Jul 2021 12:05:55 +0200 Subject: [PATCH] solve the crash issue for feature delete --- GPU_version/vio/include/gpu_svo/map.h | 3 ++- GPU_version/vio/launch/Px30.launch | 2 +- GPU_version/vio/src/feature_alignment.cpp | 16 ++++++++-------- GPU_version/vio/src/frame_handler_mono.cpp | 7 +++---- GPU_version/vio/src/map.cpp | 9 +++++++++ GPU_version/vio/src/matcher.cpp | 22 +++++++++++----------- 6 files changed, 34 insertions(+), 25 deletions(-) diff --git a/GPU_version/vio/include/gpu_svo/map.h b/GPU_version/vio/include/gpu_svo/map.h index e554088..d4a02e0 100644 --- a/GPU_version/vio/include/gpu_svo/map.h +++ b/GPU_version/vio/include/gpu_svo/map.h @@ -66,7 +66,7 @@ public: void emptyTrash(); }; - +static bool syn_=false; /// Map object which saves all keyframes which are in a map. class Map : boost::noncopyable { @@ -75,6 +75,7 @@ public: list< Point* > trash_points_; //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also. MapPointCandidates point_candidates_; + Map(); ~Map(); diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index 581e4b1..efaecc2 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/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp index 00367df..918ab25 100644 --- a/GPU_version/vio/src/feature_alignment.cpp +++ b/GPU_version/vio/src/feature_alignment.cpp @@ -36,9 +36,9 @@ bool align1D( Vector2d& cur_px_estimate, double& h_inv) { - const int halfpatch_size_ = 4; - const int patch_size = 8; - const int patch_area = 64; + const int halfpatch_size_ = 1; + const int patch_size = 2; + const int patch_area = 4; bool converged=false; // compute derivative of template and prepare inverse compositional @@ -69,7 +69,7 @@ bool align1D( float v = cur_px_estimate.y(); // termination condition - const float min_update_squared = 0.5*0.5; + const float min_update_squared = 0.05; const int cur_step = cur_img.step.p[0]; float chi2 = 0; Vector2f update; update.setZero(); @@ -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_ = 4; - const int patch_size_ = 8; - const int patch_area_ = 64; + const int halfpatch_size_ = 1; + const int patch_size_ = 2; + const int patch_area_ = 4; bool converged=false; // compute derivative of template and prepare inverse compositional @@ -202,7 +202,7 @@ bool align2D( bool sign=true;// + // termination condition - const float min_update_squared = 0.5;//0.001 + const float min_update_squared = 0.05;//0.001 const int cur_step = cur_img.step.p[0]; // float chi2 = 0; Vector3f update; update.setZero(); diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index cc92553..caea65b 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -148,7 +148,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() pose_optimizer::optimizeGaussNewton( Config::poseOptimThresh(), Config::poseOptimNumIter(), false, new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); - if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>2.0 || fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){ + if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>2.0 || + fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI || + sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ new_frame_=last_frame_; return RESULT_NO_KEYFRAME; } @@ -160,9 +162,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() <<"\tnCell: "<T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; - if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ - return RESULT_NO_KEYFRAME; - } optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); // select keyframe diff --git a/GPU_version/vio/src/map.cpp b/GPU_version/vio/src/map.cpp index 3d0cace..9e4196e 100644 --- a/GPU_version/vio/src/map.cpp +++ b/GPU_version/vio/src/map.cpp @@ -40,6 +40,7 @@ void Map::reset() bool Map::safeDeleteFrame(FramePtr frame) { + if(syn_==true)return false; bool found = false; for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) { @@ -107,6 +108,7 @@ void Map::getCloseKeyframes( const FramePtr& frame, std::list< std::pair >& close_kfs) const { + syn_=true; for(auto kf : keyframes_) { // check if kf has overlaping field of view with frame, use therefore KeyPoints @@ -123,6 +125,7 @@ void Map::getCloseKeyframes( } } } + syn_=false; } FramePtr Map::getClosestKeyframe(const FramePtr& frame) const @@ -149,6 +152,7 @@ FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const { FramePtr furthest_kf; double maxdist = 0.0; + syn_=true; for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) { double dist = ((*it)->pos()-pos).norm(); @@ -157,18 +161,21 @@ FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const furthest_kf = *it; } } + syn_=false; return furthest_kf; } bool Map::getKeyframeById(const int id, FramePtr& frame) const { bool found = false; + syn_=true; for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it) if((*it)->id_ == id) { found = true; frame = *it; break; } + syn_=false; return found; } @@ -178,6 +185,7 @@ void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s) R3<T_f_w_.inverse().rotation_matrix(),s*R*(*it)->pos() + t)); @@ -192,6 +200,7 @@ void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s) (*ftr)->point->pos_ = s*R3*(*ftr)->point->pos_ + Vector3d(t(0),0.0,t(1)); } } + syn_=false; } void Map::emptyTrash() diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index 767d542..1b025c9 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -161,20 +161,20 @@ bool Matcher::findMatchDirect( Vector2d px_scaled(px_cur/(1<type == Feature::EDGELET) - //{ - // Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad); - // dir_cur.normalize(); - // success = feature_alignment::align1D( - // cur_frame.img_pyr_[search_level_], dir_cur.cast(), - // patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); - //} - //else - //{ + if(ref_ftr_->type == Feature::EDGELET) + { + Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad); + dir_cur.normalize(); + success = feature_alignment::align1D( + cur_frame.img_pyr_[search_level_], dir_cur.cast(), + patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + } + else + { success = feature_alignment::align2D( cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, options_.align_max_iter, px_scaled); - // } + } px_cur = px_scaled * (1<