public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

The best possible situation 21.4.2021

Browse Source
This commit is contained in:
gfuse
2021-04-21 11:16:56 +02:00
parent 33c4e7e8e2
commit 85470c37b6
2 changed files with 9 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/src/frame_handler_mono.cpp
+8 -9
View File
@@ -136,10 +136,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
img_align.run(last_frame_, new_frame_); img_align.run(last_frame_, new_frame_);
// map reprojection & feature alignment // map reprojection & feature alignment
reprojector_.reprojectMap(new_frame_, overlap_kfs_); reprojector_.reprojectMap(new_frame_, overlap_kfs_);
std::cerr<<"Reprojection Map:\t nPoints = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<'\n'; std::cerr<<"Reprojection Map:\t nPoints = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<" \t Quality min: "<<Config::qualityMinFts()<<'\n';
if(reprojector_.n_matches_< Config::qualityMinFts()) if(reprojector_.n_matches_< Config::qualityMinFts())
{ {
SVO_WARN_STREAM_THROTTLE(1.0, "Not enough matched features.");
new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
tracking_quality_ = TRACKING_INSUFFICIENT; tracking_quality_ = TRACKING_INSUFFICIENT;
return RESULT_FAILURE; return RESULT_FAILURE;
@@ -148,15 +147,17 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// pose optimization subject to change for adding IMU // pose optimization subject to change for adding IMU
size_t sfba_n_edges_final; size_t sfba_n_edges_final;
if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh())) if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh()))
return RESULT_FAILURE; return RESULT_FAILURE;
std::cerr<<"Reprojected points after opmization: "<<sfba_n_edges_final<<'\n'; std::cerr<<"Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
if(sfba_n_edges_final < Config::qualityMinFts()) if(sfba_n_edges_final < Config::qualityMinFts())
return RESULT_FAILURE; return RESULT_FAILURE;
// structure optimization // structure optimization
SVO_START_TIMER("point_optimizer"); //SVO_START_TIMER("point_optimizer");
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
SVO_STOP_TIMER("point_optimizer"); //SVO_STOP_TIMER("point_optimizer");
// select keyframe // select keyframe
core_kfs_.insert(new_frame_); core_kfs_.insert(new_frame_);
@@ -220,16 +221,15 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
const SE3& T_cur_ref, const SE3& T_cur_ref,
FramePtr ref_keyframe) FramePtr ref_keyframe)
{ {
SVO_WARN_STREAM_THROTTLE(1.0, "Relocalizing frame");
if(ref_keyframe == nullptr) if(ref_keyframe == nullptr)
{ {
SVO_INFO_STREAM("No reference keyframe.");
return RESULT_FAILURE; return RESULT_FAILURE;
} }
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::GaussNewton, false, false); 30, SparseImgAlign::GaussNewton, false, false);
size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_); size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_);
if(img_align_n_tracked > 30) usleep(2000);
if(img_align_n_tracked > Config::qualityMinFts())
{ {
SE3 T_f_w_last = last_frame_->T_f_w_; SE3 T_f_w_last = last_frame_->T_f_w_;
last_frame_ = ref_keyframe; last_frame_ = ref_keyframe;
@@ -237,7 +237,6 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
if(res != RESULT_FAILURE) if(res != RESULT_FAILURE)
{ {
stage_ = STAGE_DEFAULT_FRAME; stage_ = STAGE_DEFAULT_FRAME;
SVO_INFO_STREAM("Relocalization successful.");
} }
else else
new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose
svo/src/imu_integration.cpp
+1 -1
View File
@@ -104,7 +104,7 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4)); graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2)); graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2));
}else{ }else{
if(imu_n<3)return false; if(imu_n<1)return false;
auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr); auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix())); valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix()));
valuesPtr->insert(V(imu_factor_id), statePtr->v()); valuesPtr->insert(V(imu_factor_id), statePtr->v());