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

debuging based on px30 -> still in progress

Browse Source
This commit is contained in:
gfuse
2021-04-16 17:55:40 +02:00
parent 382c41725b
commit 121cfe1095
4 changed files with 47 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/include/svo/imu_integration.h
+2 -2
View File
@@ -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<svo::Frame>&,std::size_t&,const double reproj_thresh);
private:
std::shared_ptr<gtsam::NavState> statePtr;
@@ -112,7 +112,7 @@ private:
boost::shared_ptr<gtsam::noiseModel::Isotropic> 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
svo/src/imu_integration.cpp
+32 -24
View File
@@ -8,47 +8,45 @@
#include "svo/feature.h"
#include "svo/point.h"
#include "svo/svoVisionFactor.h"
#include <gtsam/nonlinear/Marginals.h>
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
statePtr=std::make_shared<gtsam::NavState>(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::imuBias::ConstantBias>();
imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
valuesPtr=std::make_shared<gtsam::Values>();
valuesPtr->insert(P(0),statePtr->pose());
valuesPtr->insert(V(0),statePtr->v());
valuesPtr->insert(B(0),*imu_biasPtr);
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
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<gtsam::PreintegratedCombinedMeasurements>(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<double>(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<double>(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<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);
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<svo::Frame>& new_frame,std::size_t& num_obs,const double reproj_thresh){
@@ -72,6 +70,8 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
syn=true;
auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*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<svo::Frame>& 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<gtsam::CombinedImuFactor>(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<svo::VisionFactor>(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<<new_frame->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<gtsam::Pose3>(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<<f->level);
e *= sqrt_inv_cov;
if(e.norm() > reproj_thresh_scaled)
vio_svo/launch/Px30.launch
+4 -4
View File
@@ -7,13 +7,13 @@
<param name="imu_topic" value="/SensorNode/imu" type="str" />
<!-- Camera calibration file -->
<rosparam file="$(find vio_svo)/param/camera_pinhole.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find vio_svo)/param/px30.yaml" />
<!-- Default parameter settings: choose between vo_fast and vo_accurate -->
<rosparam file="$(find vio_svo)/param/vo_accurate.yaml" />
<!-- Initial camera orientation, make it point downwards -->
<param name="init_rx" value="0.00" />
<param name="init_rx" value="3.14" />
<param name="init_ry" value="0.00" />
<param name="init_rz" value="0.00" />
vio_svo/src/vo_node.cpp
+10 -18
View File
@@ -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(){