From 121cfe1095abce32b8248b85409c7f8917219dd5 Mon Sep 17 00:00:00 2001 From: gfuse Date: Fri, 16 Apr 2021 17:55:40 +0200 Subject: [PATCH] debuging based on px30 -> still in progress --- svo/include/svo/imu_integration.h | 4 +-- svo/src/imu_integration.cpp | 56 ++++++++++++++++++------------- vio_svo/launch/Px30.launch | 6 ++-- vio_svo/src/vo_node.cpp | 28 ++++++---------- 4 files changed, 47 insertions(+), 47 deletions(-) diff --git a/svo/include/svo/imu_integration.h b/svo/include/svo/imu_integration.h index 442565d..8c6f95c 100644 --- a/svo/include/svo/imu_integration.h +++ b/svo/include/svo/imu_integration.h @@ -100,7 +100,7 @@ public: Imu_Integration(Sophus::SE3& SE_init); ~Imu_Integration(); bool reset(gtsam::Values& result); - bool update(float* imu= nullptr); + bool update(double* imu= nullptr); bool predict(boost::shared_ptr&,std::size_t&,const double reproj_thresh); private: std::shared_ptr statePtr; @@ -112,7 +112,7 @@ private: boost::shared_ptr ProjectNoisePtr; std::chrono::steady_clock::time_point t_1; std::uint32_t imu_factor_id=0; - double imu_mean=0.0; + std::uint64_t imu_stamp=0.0; bool syn=false; }; #endif //SVO_IMU_INTEGRATION_H diff --git a/svo/src/imu_integration.cpp b/svo/src/imu_integration.cpp index 44e70ab..f6b9809 100644 --- a/svo/src/imu_integration.cpp +++ b/svo/src/imu_integration.cpp @@ -8,47 +8,45 @@ #include "svo/feature.h" #include "svo/point.h" #include "svo/svoVisionFactor.h" +#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(); + 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.5, 0.5, 0.5).finished())); - graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 0.1)); - graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-3)); + 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)); parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame - parameterPtr->accelerometerCovariance=gtsam::I_3x3 * pow(0.0003924, 2); // acc white noise in continuous - parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * pow(0.00020568902491,2); // gyro white noise in continuous + 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 * pow(0.004905, 2);; // acc bias in continuous - parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * pow(0.000001454441043, 2); // gyro bias in 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 preintegratedPtr = std::make_shared(parameterPtr,*imu_biasPtr); - ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // TODO change the noise used for projection + ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, 1e-8); // TODO change the noise used for projection assert(preintegratedPtr); } Imu_Integration::~Imu_Integration(){ } -bool Imu_Integration::update(float* imu){ +bool Imu_Integration::update(double* imu){ if(syn)return false; - double m=0.0; - for(int i=0;i<6;++i){ - m+=abs(imu[i]); - } - if((0.1666666667*m - imu_mean)>1e-6){ - imu_mean=0.1666666667*m; + 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; } @@ -60,11 +58,11 @@ bool Imu_Integration::reset(gtsam::Values& result){ 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(); + //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(); return true; } bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size_t& num_obs,const double reproj_thresh){ @@ -72,6 +70,8 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size 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()); @@ -79,28 +79,36 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size 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(); num_obs=0; for(auto&& f:new_frame->fts_){ if(f->point==NULL)continue; - graphPtr->add(svo::VisionFactor(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id))); + graphPtr->emplace_shared(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id)); ++num_obs; } // Now optimize. + std::cout<<"--------------------------------------------------------------------------graph----------------------------------------------"<<'\n'; //graphPtr->print(); - //valuesPtr->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'; 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(result.at(P(imu_factor_id)) * f->point->pos_); + 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; if(e.norm() > reproj_thresh_scaled) diff --git a/vio_svo/launch/Px30.launch b/vio_svo/launch/Px30.launch index 6ff3dab..2afa3b2 100644 --- a/vio_svo/launch/Px30.launch +++ b/vio_svo/launch/Px30.launch @@ -7,13 +7,13 @@ - + - + - + diff --git a/vio_svo/src/vo_node.cpp b/vio_svo/src/vo_node.cpp index 0333176..ebe8c7a 100644 --- a/vio_svo/src/vo_node.cpp +++ b/vio_svo/src/vo_node.cpp @@ -50,8 +50,7 @@ public: VoNode(); ~VoNode(); void imgCb(const sensor_msgs::ImageConstPtr& msg); - //void imuCb(const sensor_msgs::ImuPtr& imu); - void imuCb(const geometry_msgs::PoseStamped& imu); + void imuCb(const sensor_msgs::ImuPtr& imu); void imu(); }; @@ -108,22 +107,15 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) ROS_ERROR("cv_bridge exception: %s", e.what()); } } -void VoNode::imuCb(const geometry_msgs::PoseStamped &imu) { - float imu_in[6]; - /*imu_in[0] = imu->angular_velocity.x; - imu_in[1] = imu->angular_velocity.y; - imu_in[2] = imu->angular_velocity.z; - imu_in[3] = imu->linear_acceleration.x; - imu_in[4] = imu->linear_acceleration.y; - imu_in[5] = imu->linear_acceleration.z; - imu_in[6] = imu->header.stamp.sec; - */ - imu_in[0] = imu.pose.position.z; - imu_in[1] = imu.pose.position.x; - imu_in[2] = imu.pose.position.y+9.8; - imu_in[3] = imu.pose.orientation.z; - imu_in[4] = imu.pose.orientation.x; - imu_in[5] = imu.pose.orientation.y; +void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) { + double imu_in[7]; + 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(){