From e9d5ff2033e1b0e0b361e38338ffc2cbcdb48c75 Mon Sep 17 00:00:00 2001 From: gfuse Date: Fri, 20 Aug 2021 15:50:29 +0200 Subject: [PATCH] add relocalization --- .../vio/include/vio/frame_handler_mono.h | 1 - GPU_version/vio/src/frame_handler_mono.cpp | 45 +++++++------------ GPU_version/vio/src/initialization.cpp | 6 ++- GPU_version/vio/src/vo_node.cpp | 4 +- 4 files changed, 23 insertions(+), 33 deletions(-) diff --git a/GPU_version/vio/include/vio/frame_handler_mono.h b/GPU_version/vio/include/vio/frame_handler_mono.h index 96b839f..6185fc5 100644 --- a/GPU_version/vio/include/vio/frame_handler_mono.h +++ b/GPU_version/vio/include/vio/frame_handler_mono.h @@ -94,7 +94,6 @@ protected: /// Try relocalizing the frame at relative position to provided keyframe. virtual UpdateResult relocalizeFrame( - const SE3& T_cur_ref, FramePtr ref_keyframe); /// Reset the frame handler. Implement in derived class. diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 2eebc25..430f051 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -104,26 +104,29 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const res = processSecondFrame(); else if(stage_ == STAGE_FIRST_FRAME) res = processFirstFrame(); - else if(stage_ == STAGE_RELOCALIZING) - res = relocalizeFrame(SE3(Matrix3d::Identity(), Vector3d::Zero()), - map_.getClosestKeyframe(last_frame_)); // set last frame if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)last_frame_ = new_frame_; new_frame_.reset(); // finish processing finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); + if(stage_ == STAGE_RELOCALIZING){ + depthFilter()->stopThread(); + start(); + } } FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() { - new_frame_->T_f_w_ = SE2_5(0.0,0.0,0.0); + auto init_f= ukfPtr_.get_location(); + new_frame_->T_f_w_=init_f.second; + new_frame_->Cov_ = init_f.first; if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE) return RESULT_NO_KEYFRAME; new_frame_->setKeyframe(); + map_.reset(); map_.addKeyframe(new_frame_); stage_ = STAGE_SECOND_FRAME; - ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); SVO_INFO_STREAM("Init: Selected first frame."); return RESULT_IS_KEYFRAME; } @@ -137,8 +140,13 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); new_frame_->T_f_w_ = result.second; new_frame_->Cov_ = result.first; - if(res == initialization::NO_KEYFRAME) + if(res == initialization::NO_KEYFRAME){ return RESULT_NO_KEYFRAME; + }else if(res == initialization::FAILURE){ + stage_ = STAGE_FIRST_FRAME; + return RESULT_FAILURE; + } + new_frame_->setKeyframe(); double depth_mean, depth_min; frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); @@ -232,30 +240,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() } FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame( - const SE3& T_cur_ref, FramePtr ref_keyframe) { - if(ref_keyframe == nullptr) - { - return RESULT_FAILURE; - } - SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), - 30, SparseImgAlign::GaussNewton, false, false); - size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_); - usleep(2000); - if(img_align_n_tracked > Config::qualityMinFts()) - { - SE2_5 T_f_w_last = last_frame_->T_f_w_; - last_frame_ = ref_keyframe; - FrameHandlerMono::UpdateResult res = processFrame(); - if(res != RESULT_FAILURE) - { - stage_ = STAGE_DEFAULT_FRAME; - } - else - new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose - return res; - } return RESULT_FAILURE; } @@ -265,6 +251,7 @@ bool FrameHandlerMono::relocalizeFrameAtPose( const cv::Mat& img, const double timestamp) { + /* FramePtr ref_keyframe; if(!map_.getKeyframeById(keyframe_id, ref_keyframe)) return false; @@ -273,7 +260,7 @@ bool FrameHandlerMono::relocalizeFrameAtPose( if(res != RESULT_FAILURE) { last_frame_ = new_frame_; return true; - } + }*/ return false; } diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp index 4ec1434..a881478 100644 --- a/GPU_version/vio/src/initialization.cpp +++ b/GPU_version/vio/src/initialization.cpp @@ -56,11 +56,15 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(), inliers_, xyz_in_cur_, T_cur_from_ref_); - if(inliers_.size() < Config::initMinInliers()) + if(inliers_.size() < Config::initMinInliers() && disparity < 80) { SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "< 80){ + SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "<stage()!=vio::FrameHandlerBase::STAGE_DEFAULT_FRAME) vo_->start(); @@ -136,7 +136,7 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg) if(stddev.val[0] * stddev.val[0]< 50.0)return; vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp); } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + ROS_ERROR("vo exception: %s", e.what()); } } void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {