From 094c9f428294cf967c702d96399d2bade43161a7 Mon Sep 17 00:00:00 2001 From: gfuse Date: Tue, 12 Oct 2021 20:41:15 +0200 Subject: [PATCH] add landmark type --- GPU_version/vio/launch/Px30.launch | 2 +- GPU_version/vio/param/vo_fast.yaml | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 4 ++-- GPU_version/vio/src/global_optimizer.cpp | 1 + GPU_version/vio/src/initialization.cpp | 1 + GPU_version/vio/src/point.cpp | 6 ++--- GPU_version/vio/src/pose_optimizer.cpp | 27 ++++++++++++++++++---- GPU_version/vio/src/reprojector.cpp | 3 ++- 8 files changed, 33 insertions(+), 13 deletions(-) diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index 7c4cbdf..d12f08d 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/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index d28e8fe..da4c0af 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -13,6 +13,6 @@ vio: ACC_ekf: 0.2 #acc white noise in continuous in EKF GYO_ekf: 0.5 #gyro white noise in continuous in EKF VO_ekf_translation: 1.0 #VO noise in EKF - VO_ekf_orientation: 0.1 #VO noise in EKF + VO_ekf_orientation: 0.5 #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/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 7143cd9..6064c89 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -181,7 +181,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() #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; + if(depth_mean>20.0)return RESULT_NO_KEYFRAME; optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); // select keyframe @@ -215,7 +215,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() // add keyframe to map map_.addKeyframe(new_frame_); map_.checkKeyFrames(); - //ba_glob_->new_key_frame(); + ba_glob_->new_key_frame(); return RESULT_IS_KEYFRAME; } diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp index c327201..70d0e64 100644 --- a/GPU_version/vio/src/global_optimizer.cpp +++ b/GPU_version/vio/src/global_optimizer.cpp @@ -110,6 +110,7 @@ namespace vio { for(auto&& it_ftr:(*it_kf)->fts_) { if(it_ftr->point==NULL)continue; + if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue; // for each keyframe add edges to all observed mapoints std::shared_ptr mp = it_ftr->point; if(mp->pos_.hasNaN())continue; diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp index bfc0b8d..1f4619a 100644 --- a/GPU_version/vio/src/initialization.cpp +++ b/GPU_version/vio/src/initialization.cpp @@ -102,6 +102,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) frame_cur->c2f(px_cur_.at(f.index).x,px_cur_.at(f.index).y), f.item->score,f.item->level,f.item->descriptor); frame_cur->addFeature(ftr_cur); new_point->addFrameRef(ftr_cur); + new_point->type_=vio::Point::TYPE_GOOD; std::shared_ptr ftr_ref=std::make_shared(frame_ref_, new_point, f.item->px, f.item->f, f.item->score,f.item->level,f.item->descriptor); frame_ref_->addFeature(ftr_ref); diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index 85d57b9..cfe9c97 100644 --- a/GPU_version/vio/src/point.cpp +++ b/GPU_version/vio/src/point.cpp @@ -166,11 +166,11 @@ void Point::optimize(const size_t n_iter) for(auto it=obs_.begin(); it!=obs_.end(); ++it) { Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * pos_)); e /= (1<<(*it)->level); - if (e.norm() > 3.0/ (*it)->frame->cam_->errorMultiplier2()) + if (e.norm() > 4.0/ (*it)->frame->cam_->errorMultiplier2()) n_failed_reproj_++; } - - + if(n_failed_reproj_<0.75*obs_.size()) + type_=TYPE_GOOD; } } // namespace vio diff --git a/GPU_version/vio/src/pose_optimizer.cpp b/GPU_version/vio/src/pose_optimizer.cpp index 8ea3fa5..01a0cfa 100644 --- a/GPU_version/vio/src/pose_optimizer.cpp +++ b/GPU_version/vio/src/pose_optimizer.cpp @@ -48,11 +48,20 @@ void optimizeGaussNewton( std::vector errors; for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/) { - if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) { + if((*it)->point == NULL) { map.safeDeletePoint((*it)->point); it = frame->fts_.erase(it); continue; } + if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.){ + map.safeDeletePoint((*it)->point); + it = frame->fts_.erase(it); + continue; + } + if((*it)->point->type_==vio::Point::TYPE_UNKNOWN){ + it++; + continue; + } //Reprojection error Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_)); @@ -86,7 +95,8 @@ void optimizeGaussNewton( // compute residual for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) { - if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) + if((*it)->point == NULL)continue; + if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0. || (*it)->point->type_==vio::Point::TYPE_UNKNOWN) continue; Matrix23d J; Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_)); @@ -125,12 +135,17 @@ void optimizeGaussNewton( size_t n_deleted_refs = 0; for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/) { - if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) { + if((*it)->point == NULL) { map.safeDeletePoint((*it)->point); it = frame->fts_.erase(it); ++n_deleted_refs; continue; } + if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.){ + map.safeDeletePoint((*it)->point); + it = frame->fts_.erase(it); + continue; + } Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_)); e /= (1<<(*it)->level); chi2_vec_final.push_back(e.norm()); @@ -139,9 +154,11 @@ void optimizeGaussNewton( map.safeDeletePoint((*it)->point); it = frame->fts_.erase(it); ++n_deleted_refs; - } - else + }else{ + (*it)->point->type_=vio::Point::TYPE_CANDIDATE; it++; + } + } //point_mut_.unlock(); num_obs -= n_deleted_refs; diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 6ff43f3..1ee7885 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -152,16 +152,17 @@ namespace vio { new_point->addFrameRef(frame->fts_.back()); added_keypoints.push_back(match.trainIdx); frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_; + frame->fts_.back()->point->type_=vio::Point::TYPE_UNKNOWN; grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px)); overlap_kfs.back().second++; ++points_count; }else{ if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px))continue; - (*it_ref)->point->last_frame_overlap_id_ = frame->id_; frame->addFeature(std::make_shared(frame, (*it_ref)->point, px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor)); + (*it_ref)->point->type_=vio::Point::TYPE_CANDIDATE; added_keypoints.push_back(match.trainIdx); grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px)); overlap_kfs.back().second++;