From 661acff27f82140a1ccce4168f8f02d4407a7735 Mon Sep 17 00:00:00 2001 From: gfuse Date: Mon, 19 Apr 2021 19:49:17 +0200 Subject: [PATCH] The best possible situation 19.4.2021 --- svo/include/svo/imu_integration.h | 8 +- svo/include/svo/svoVisionFactor.h | 5 +- svo/src/frame_handler_mono.cpp | 35 ++------ svo/src/imu_integration.cpp | 142 ++++++++++++++++-------------- vio_svo/launch/Px30.launch | 6 +- vio_svo/param/vo_fast.yaml | 9 +- vio_svo/src/vo_node.cpp | 5 +- 7 files changed, 102 insertions(+), 108 deletions(-) diff --git a/svo/include/svo/imu_integration.h b/svo/include/svo/imu_integration.h index 8c6f95c..b6da3a7 100644 --- a/svo/include/svo/imu_integration.h +++ b/svo/include/svo/imu_integration.h @@ -11,13 +11,14 @@ #include #include #include +#include #include #include using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) using gtsam::symbol_shorthand::P; // Pose3 (x,y,z,r,p,y) -using gtsam::symbol_shorthand::F; // Feature +using gtsam::symbol_shorthand::C; // Camera pose //https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop // Wrapper class @@ -99,7 +100,7 @@ class Imu_Integration{ public: Imu_Integration(Sophus::SE3& SE_init); ~Imu_Integration(); - bool reset(gtsam::Values& result); + bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr& new_frame); bool update(double* imu= nullptr); bool predict(boost::shared_ptr&,std::size_t&,const double reproj_thresh); private: @@ -112,7 +113,8 @@ private: boost::shared_ptr ProjectNoisePtr; std::chrono::steady_clock::time_point t_1; std::uint32_t imu_factor_id=0; - std::uint64_t imu_stamp=0.0; + uint imu_n=0; + std::shared_ptr optimizerParamPtr; bool syn=false; }; #endif //SVO_IMU_INTEGRATION_H diff --git a/svo/include/svo/svoVisionFactor.h b/svo/include/svo/svoVisionFactor.h index b3f8ab3..cd00816 100644 --- a/svo/include/svo/svoVisionFactor.h +++ b/svo/include/svo/svoVisionFactor.h @@ -53,9 +53,8 @@ namespace svo { gtsam::Matrix26 J; gtsam::Point3 point(p*landmark); svo::Frame::jacobian_xyz2uv(point, J); - if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J / (1<T_f_w_ = last_frame_->T_f_w_; // sparse image align - SVO_START_TIMER("sparse_img_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_); - SVO_STOP_TIMER("sparse_img_align"); - SVO_LOG(img_align_n_tracked); - SVO_DEBUG_STREAM("Img Align:\t Tracked = " << img_align_n_tracked); + std::cout<<"Img Align:\t Tracked = "<T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps @@ -158,22 +150,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() } // pose optimization subject to change for adding IMU - SVO_START_TIMER("pose_optimizer"); size_t sfba_n_edges_final; if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh())) return RESULT_FAILURE; - /* - double sfba_thresh, sfba_error_init, sfba_error_final; - pose_optimizer::optimizeGaussNewton( - Config::poseOptimThresh(), Config::poseOptimNumIter(), false, - new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); - SVO_STOP_TIMER("pose_optimizer"); - SVO_LOG4(sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); - SVO_DEBUG_STREAM("PoseOptimizer:\t ErrInit = "<T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps diff --git a/svo/src/imu_integration.cpp b/svo/src/imu_integration.cpp index f6b9809..6d710fb 100644 --- a/svo/src/imu_integration.cpp +++ b/svo/src/imu_integration.cpp @@ -9,28 +9,25 @@ #include "svo/point.h" #include "svo/svoVisionFactor.h" #include +#include Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ - statePtr=std::make_shared(gtsam::Pose3(gtsam::Rot3(SE_init.rotation_matrix()),SE_init.translation()), - gtsam::Vector3(0.0,0.0,0.0)); - imu_biasPtr=std::make_shared(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8)); - valuesPtr=std::make_shared(); - valuesPtr->insert(P(0),statePtr->pose()); - valuesPtr->insert(V(0),statePtr->v()); - valuesPtr->insert(B(0),*imu_biasPtr); graphPtr=std::make_shared(); - graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.01, 0.01, 0.01, 0.01, 0.01, 0.01).finished())); - graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 0.01)); - graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-4)); + valuesPtr=std::make_shared(); parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame - parameterPtr->accelerometerCovariance=gtsam::I_3x3 * 1e-3; // acc white noise in continuous - parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * 1e-3; // gyro white noise in continuous - parameterPtr->integrationCovariance=gtsam::I_3x3 * 1e-8; // integration uncertainty continuous - parameterPtr->biasAccCovariance = gtsam::I_3x3 * 1e-8; // acc bias in continuous - parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * 1e-8; // gyro bias in continuous - parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * 1e-5; // error in the bias used for preintegration + 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 + 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); // TODO change the noise used for projection + ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 1e-8); // the noise used for projection + optimizerParamPtr=std::make_shared(); + optimizerParamPtr->relinearizeThreshold = 0.1; + optimizerParamPtr->relinearizeSkip = 1; + optimizerParamPtr->evaluateNonlinearError=true; assert(preintegratedPtr); } Imu_Integration::~Imu_Integration(){ @@ -38,48 +35,66 @@ Imu_Integration::~Imu_Integration(){ } bool Imu_Integration::update(double* imu){ if(syn)return false; - if(imu[6]>imu_stamp){ - auto in=std::chrono::steady_clock::now(); - if(std::chrono::duration(in-t_1).count()>20.0){ - t_1=in; - imu_stamp=imu[6]; - }else{ - preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration(in-t_1).count()); - t_1=in; - imu_stamp=imu[6]; - } - return true; + if(imu_factor_id<1)return false; + auto in=std::chrono::steady_clock::now(); + if(std::chrono::duration(in-t_1).count()>20.0){ + t_1=in; + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005); + }else{ + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration(in-t_1).count()); + t_1=in; + ++imu_n; } - return false; + return true; } -bool Imu_Integration::reset(gtsam::Values& result){ +bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr& new_frame){ + gtsam::Values result = optimizer.calculateEstimate(); // 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); - //graphPtr->resize(0); - //valuesPtr->erase(P(imu_factor_id-1)); - //valuesPtr->erase(V(imu_factor_id-1)); - //valuesPtr->erase(B(imu_factor_id-1)); - //t_1=std::chrono::steady_clock::now(); + ++imu_factor_id; + syn=false; + new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t()); + new_frame->Cov_=optimizer.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(),optimizer.marginalCovariance(P(imu_factor_id-1))); + graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),optimizer.marginalCovariance(V(imu_factor_id-1))); + graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,optimizer.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){ - // Adding IMU factor and and pre-integration. + usleep(5000); syn=true; - auto preint_imu_combined =dynamic_cast(*preintegratedPtr); - gtsam::NavState Estimate_state = preintegratedPtr->predict(*statePtr, *imu_biasPtr); - std::cout<<"--------------------------------------------------------------------------PreIntegration----------------------------------------------"<<'\n'; - //preintegratedPtr->print(); - ++imu_factor_id; - valuesPtr->insert(P(imu_factor_id), Estimate_state.pose()); - 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); - graphPtr->emplace_shared(imu_factor); - imu_factor.print(); + if(imu_factor_id<1){ + statePtr=std::make_shared(gtsam::Pose3(new_frame->T_f_w_.matrix()), + gtsam::Vector3(1e-12,1e-12,1e-12)); + valuesPtr->insert(P(0),statePtr->pose()); + valuesPtr->insert(V(0),statePtr->v()); + valuesPtr->insert(B(0),*imu_biasPtr); + graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9).finished())); + 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)); + }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); + graphPtr->emplace_shared(imu_factor); + imu_n=0; + } + num_obs=0; for(auto&& f:new_frame->fts_){ if(f->point==NULL)continue; @@ -87,32 +102,25 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size ++num_obs; } // Now optimize. - std::cout<<"--------------------------------------------------------------------------graph----------------------------------------------"<<'\n'; - //graphPtr->print(); - std::cout<<"--------------------------------------------------------------------------values----------------------------------------------"<<'\n'; - valuesPtr->print(); - gtsam::LevenbergMarquardtParams params; - params.setVerbosityLM("SUMMARY"); - gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr, params); - gtsam::Values result = optimizer.optimize(); - gtsam::Marginals marginals(*graphPtr, result); - std::cout<<"--------------------------------------------------------------------------marginals----------------------------------------------"<<'\n'; - //marginals.print(); - reset(result); - syn=false; - new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t()); - new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id)); - std::cout<Cov_<<'\n'; + 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.print(); + reset(optimizer,new_frame); + // Remove Measurements with too large reprojection error double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2(); for(auto&& f:new_frame->fts_) { if(f->point == NULL) continue; - Eigen::Vector2d e = vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_); - double sqrt_inv_cov = 1.0 / (1<level); - e *= sqrt_inv_cov; + 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/launch/Px30.launch b/vio_svo/launch/Px30.launch index 2afa3b2..06083d0 100644 --- a/vio_svo/launch/Px30.launch +++ b/vio_svo/launch/Px30.launch @@ -9,9 +9,6 @@ - - - @@ -19,4 +16,7 @@ + + + diff --git a/vio_svo/param/vo_fast.yaml b/vio_svo/param/vo_fast.yaml index 3df87dd..3fa20a3 100644 --- a/vio_svo/param/vo_fast.yaml +++ b/vio_svo/param/vo_fast.yaml @@ -1,3 +1,6 @@ -grid_size: 30 -max_n_kfs: 10 -loba_num_iter: 0 \ No newline at end of file +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 diff --git a/vio_svo/src/vo_node.cpp b/vio_svo/src/vo_node.cpp index ebe8c7a..2f98da0 100644 --- a/vio_svo/src/vo_node.cpp +++ b/vio_svo/src/vo_node.cpp @@ -108,14 +108,13 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) } } void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) { - double imu_in[7]; + double imu_in[6]; imu_in[0] = imu->linear_acceleration.x; imu_in[1] = imu->linear_acceleration.y; imu_in[2] = imu->linear_acceleration.z; imu_in[3] = imu->angular_velocity.x; imu_in[4] = imu->angular_velocity.y; imu_in[5] = imu->angular_velocity.z; - imu_in[6] = imu->header.stamp.toNSec(); vo_->imu_integPtr_->update(imu_in); } void VoNode::imu(){ @@ -123,7 +122,7 @@ void VoNode::imu(){ std::cout << "create vo_node" << std::endl; // subscribe to cam msgs image_transport::ImageTransport it(nh); - image_transport::Subscriber it_sub = it.subscribe(vk::getParam("vio/cam_topic", "camera/image_raw"), 5, &svo::VoNode::imgCb, this); + image_transport::Subscriber it_sub = it.subscribe(vk::getParam("vio/cam_topic", "camera/image_raw"), 1, &svo::VoNode::imgCb, this); // start processing callbacks while(ros::ok() && !quit_) {