From 4987e55ec7fb5119c3202e1dc5d53cd92159995b Mon Sep 17 00:00:00 2001 From: gfuse Date: Tue, 20 Apr 2021 16:18:52 +0200 Subject: [PATCH] The best possible situation 20.4.2021 --- svo/include/svo/config.h | 18 +++++++++++ svo/include/svo/imu_integration.h | 1 + svo/src/config.cpp | 9 +++++- svo/src/frame_handler_mono.cpp | 18 ++++------- svo/src/imu_integration.cpp | 51 ++++++++++++++++++++++--------- vio_svo/param/vo_fast.yaml | 17 ++++++++--- 6 files changed, 81 insertions(+), 33 deletions(-) diff --git a/svo/include/svo/config.h b/svo/include/svo/config.h index d4f50df..e1c97b1 100644 --- a/svo/include/svo/config.h +++ b/svo/include/svo/config.h @@ -119,6 +119,23 @@ public: /// If within one frame, this amount of features are dropped. Tracking quality is bad. static int& qualityMaxFtsDrop() { return getInstance().quality_max_drop_fts; } + /// acc white noise in continuous. + static double& ACC_Noise() { return getInstance().ACC_noise; } + + /// acc white noise in continuous. + static double& GYO_Noise() { return getInstance().GYO_noise; } + /// integration uncertainty continuous + static double& IUC() { return getInstance().iuc; } + // acc bias in continuous + static double& ABC() { return getInstance().abc; } + // gyro bias in continuous + static double& GBC() { return getInstance().gbc; } + // error in the bias used for preintegration + static double& EBP() { return getInstance().ebp; } + // the noise used for projection + static double& NRP() { return getInstance().nrp; } + + private: Config(); Config(Config const&); @@ -152,6 +169,7 @@ private: size_t max_fts; size_t quality_min_fts; int quality_max_drop_fts; + double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise; }; } // namespace svo diff --git a/svo/include/svo/imu_integration.h b/svo/include/svo/imu_integration.h index b6da3a7..11b9055 100644 --- a/svo/include/svo/imu_integration.h +++ b/svo/include/svo/imu_integration.h @@ -101,6 +101,7 @@ public: Imu_Integration(Sophus::SE3& SE_init); ~Imu_Integration(); bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr& new_frame); + bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame); bool update(double* imu= nullptr); bool predict(boost::shared_ptr&,std::size_t&,const double reproj_thresh); private: diff --git a/svo/src/config.cpp b/svo/src/config.cpp index f6820f4..e8475fe 100644 --- a/svo/src/config.cpp +++ b/svo/src/config.cpp @@ -51,7 +51,14 @@ Config::Config() : img_imu_delay(vk::getParam("svo/img_imu_delay", 0.0)), max_fts(vk::getParam("svo/max_fts", 120)), quality_min_fts(vk::getParam("svo/quality_min_fts", 50)), - quality_max_drop_fts(vk::getParam("svo/quality_max_drop_fts", 40)) + quality_max_drop_fts(vk::getParam("svo/quality_max_drop_fts", 40)), + ACC_noise(vk::getParam("svo/ACC_noise", 1e-4)), + GYO_noise(vk::getParam("svo/GYO_noise", 1e-4)), + iuc(vk::getParam("svo/IUC", 1e-1)), + abc(vk::getParam("svo/ABC", 1e-1)), + gbc(vk::getParam("svo/GBC", 1e-1)), + ebp(vk::getParam("svo/EBP", 1e-2)), + nrp(vk::getParam("svo/NRP", 1e-5)) #else trace_name("svo"), trace_dir("/tmp"), diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index e5111cf..780d9c7 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/svo/src/frame_handler_mono.cpp @@ -66,9 +66,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp) overlap_kfs_.clear(); // create new frame - //SVO_START_TIMER("pyramid_creation"); new_frame_.reset(new Frame(cam_, img.clone(), timestamp)); - //SVO_STOP_TIMER("pyramid_creation"); // process frame UpdateResult res = RESULT_FAILURE; @@ -135,12 +133,10 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() // sparse image align SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), 30, SparseImgAlign::GaussNewton, false, false); - size_t img_align_n_tracked = img_align.run(last_frame_, new_frame_); - std::cout<<"Img Align:\t Tracked = "<predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh())) return RESULT_FAILURE; - std::cout<<"reprojected point after opmization: "<T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps @@ -179,7 +174,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() return RESULT_NO_KEYFRAME; } new_frame_->setKeyframe(); - SVO_DEBUG_STREAM("New keyframe selected."); // new keyframe selected for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it) @@ -218,7 +212,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() // add keyframe to map map_.addKeyframe(new_frame_); - + std::cerr<<"Svo pipline was finished\n"; return RESULT_IS_KEYFRAME; } diff --git a/svo/src/imu_integration.cpp b/svo/src/imu_integration.cpp index 6d710fb..2aed670 100644 --- a/svo/src/imu_integration.cpp +++ b/svo/src/imu_integration.cpp @@ -10,24 +10,26 @@ #include "svo/svoVisionFactor.h" #include #include +#include +#include Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ graphPtr=std::make_shared(); valuesPtr=std::make_shared(); parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame - parameterPtr->accelerometerCovariance=gtsam::I_3x3 * 1e-4; // acc white noise in continuous - parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * 1e-4; // gyro white noise in continuous - parameterPtr->integrationCovariance=gtsam::I_3x3 * 1e-1; // integration uncertainty continuous - parameterPtr->biasAccCovariance = gtsam::I_3x3 * 1e-1; // acc bias in continuous - parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * 1e-1; // gyro bias in continuous - parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * 1e-2; // error in the bias used for preintegration + parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous + parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous + parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous + parameterPtr->biasAccCovariance = gtsam::I_3x3 * svo::Config::ABC(); // acc bias in continuous + parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * svo::Config::GBC(); // gyro bias in continuous + parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * svo::Config::EBP(); // error in the bias used for preintegration imu_biasPtr=std::make_shared(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8)); preintegratedPtr = std::make_shared(parameterPtr,*imu_biasPtr); - ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 1e-8); // the noise used for projection + ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection optimizerParamPtr=std::make_shared(); optimizerParamPtr->relinearizeThreshold = 0.1; optimizerParamPtr->relinearizeSkip = 1; - optimizerParamPtr->evaluateNonlinearError=true; + //optimizerParamPtr->evaluateNonlinearError=true; assert(preintegratedPtr); } Imu_Integration::~Imu_Integration(){ @@ -68,6 +70,27 @@ bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptraddPrior(B(imu_factor_id-1),*imu_biasPtr,optimizer.marginalCovariance(B(imu_factor_id-1))); return true; } +bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr& new_frame){ + gtsam::Values result = optimizer.optimize(); + // Overwrite the beginning of the preintegration for the next step. + statePtr=std::make_shared(result.at(P(imu_factor_id)), result.at(V(imu_factor_id))); + imu_biasPtr=std::make_shared(result.at(B(imu_factor_id))); + preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr); + ++imu_factor_id; + syn=false; + gtsam::Marginals marginals(*graphPtr,result); + new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t()); + new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1)); + graphPtr->resize(0); + valuesPtr->clear(); + valuesPtr->insert(P(imu_factor_id-1),statePtr->pose()); + valuesPtr->insert(V(imu_factor_id-1),statePtr->v()); + valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr); + graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),marginals.marginalCovariance(P(imu_factor_id-1))); + graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1))); + graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1))); + return true; +} bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size_t& num_obs,const double reproj_thresh){ usleep(5000); syn=true; @@ -83,11 +106,8 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size }else{ if(imu_n<3)return false; auto preint_imu_combined =dynamic_cast(*preintegratedPtr); - //gtsam::NavState Estimate_state = preintegratedPtr->predict(*statePtr, *imu_biasPtr); valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix())); - //valuesPtr->insert(P(imu_factor_id), Estimate_state.pose()); valuesPtr->insert(V(imu_factor_id), statePtr->v()); - //valuesPtr->insert(V(imu_factor_id), Estimate_state.v()); valuesPtr->insert(B(imu_factor_id), *imu_biasPtr); gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id), preint_imu_combined); @@ -102,14 +122,16 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size ++num_obs; } // Now optimize. - gtsam::ISAM2 optimizer(*optimizerParamPtr); - optimizer.update(*graphPtr, *valuesPtr); + //gtsam::ISAM2 optimizer(*optimizerParamPtr); + //optimizer.update(*graphPtr, *valuesPtr); // Each call to iSAM2 update(*) performs one iteration of the iterative // nonlinear solver. If accuracy is desired at the expense of time, // update(*) can be called additional times to perform multiple optimizer // iterations every step. - optimizer.update(); + //optimizer.update(); // optimizer.print(); + gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr); + reset(optimizer,new_frame); // Remove Measurements with too large reprojection error double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2(); @@ -120,7 +142,6 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<level); if(e.norm() > reproj_thresh_scaled) { - std::cout<<"Error: "<point = NULL; --num_obs; diff --git a/vio_svo/param/vo_fast.yaml b/vio_svo/param/vo_fast.yaml index 3fa20a3..84e6549 100644 --- a/vio_svo/param/vo_fast.yaml +++ b/vio_svo/param/vo_fast.yaml @@ -1,6 +1,13 @@ svo: - grid_size: 30 - max_n_kfs: 10 - loba_num_iter: 0 - quality_min_fts: 20 - quality_max_drop_fts: 50 \ No newline at end of file + grid_size: 30 #Feature grid size of a cell in [px]. + max_n_kfs: 10 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3. + loba_num_iter: 0 #Number of iterations in the local bundle adjustment. + quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. + quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad. + ACC_noise: 0.0001 #acc white noise in continuous + GYO_noise: 0.0001 #gyro white noise in continuous + IUC: 0.1 #integration uncertainty continuous + ABC: 0.1 #acc bias in continuous + GBC: 0.1 #gyro bias in continuous + EBP: 0.01 #error in the bias used for preintegration + NRP: 0.0000001 #noise used for projection in preintegration