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

The best possible situation 20.4.2021

Browse Source
This commit is contained in:
gfuse
2021-04-20 16:18:52 +02:00
parent 661acff27f
commit 4987e55ec7
6 changed files with 81 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/include/svo/config.h
+18
View File
@@ -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
svo/include/svo/imu_integration.h
+1
View File
@@ -101,6 +101,7 @@ public:
Imu_Integration(Sophus::SE3& SE_init);
~Imu_Integration();
bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
bool update(double* imu= nullptr);
bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&,const double reproj_thresh);
private:
svo/src/config.cpp
+8 -1
View File
@@ -51,7 +51,14 @@ Config::Config() :
img_imu_delay(vk::getParam<double>("svo/img_imu_delay", 0.0)),
max_fts(vk::getParam<int>("svo/max_fts", 120)),
quality_min_fts(vk::getParam<int>("svo/quality_min_fts", 50)),
quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40))
quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40)),
ACC_noise(vk::getParam<double>("svo/ACC_noise", 1e-4)),
GYO_noise(vk::getParam<double>("svo/GYO_noise", 1e-4)),
iuc(vk::getParam<double>("svo/IUC", 1e-1)),
abc(vk::getParam<double>("svo/ABC", 1e-1)),
gbc(vk::getParam<double>("svo/GBC", 1e-1)),
ebp(vk::getParam<double>("svo/EBP", 1e-2)),
nrp(vk::getParam<double>("svo/NRP", 1e-5))
#else
trace_name("svo"),
trace_dir("/tmp"),
svo/src/frame_handler_mono.cpp
+6 -12
View File
@@ -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 = "<<img_align_n_tracked<<'\n';
img_align.run(last_frame_, new_frame_);
// map reprojection & feature alignment
reprojector_.reprojectMap(new_frame_, overlap_kfs_);
std::cout<<"Reprojection:\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_<<'\n';
if(reprojector_.n_matches_< Config::qualityMinFts())
{
SVO_WARN_STREAM_THROTTLE(1.0, "Not enough matched features.");
@@ -153,9 +149,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
size_t sfba_n_edges_final;
if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh()))
return RESULT_FAILURE;
std::cout<<"reprojected point after opmization: "<<sfba_n_edges_final<<'\n';
//if(sfba_n_edges_final < 20)
// return RESULT_FAILURE;
std::cerr<<"Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
if(sfba_n_edges_final < Config::qualityMinFts())
return RESULT_FAILURE;
// structure optimization
SVO_START_TIMER("point_optimizer");
@@ -165,7 +161,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// select keyframe
core_kfs_.insert(new_frame_);
setTrackingQuality(sfba_n_edges_final);
std::cout<<"teracking quality: "<<tracking_quality_<<'\n';
if(tracking_quality_ == TRACKING_INSUFFICIENT)
{
new_frame_->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;
}
svo/src/imu_integration.cpp
+36 -15
View File
@@ -10,24 +10,26 @@
#include "svo/svoVisionFactor.h"
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose3.h>
#include <vikit/params_helper.h>
#include <svo/config.h>
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
valuesPtr=std::make_shared<gtsam::Values>();
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::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(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<gtsam::ISAM2Params>();
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_ptr<svo::Frame
graphPtr->addPrior(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<svo::Frame>& new_frame){
gtsam::Values result = optimizer.optimize();
// Overwrite the beginning of the preintegration for the next step.
statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(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<svo::Frame>& 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<svo::Frame>& new_frame,std::size
}else{
if(imu_n<3)return false;
auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*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<svo::Frame>& 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<svo::Frame>& new_frame,std::size
Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<<f->level);
if(e.norm() > reproj_thresh_scaled)
{
std::cout<<"Error: "<<e.norm()<<'\n';
// we don't need to delete a reference in the point since it was not created yet
f->point = NULL;
--num_obs;
vio_svo/param/vo_fast.yaml
+12 -5
View File
@@ -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
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