|
|
@@ -0,0 +1,225 @@
|
|
|
+//
|
|
|
+// Created by zx on 23-8-25.
|
|
|
+//
|
|
|
+
|
|
|
+#include "odometry.h"
|
|
|
+
|
|
|
+using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
|
|
|
+using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot)
|
|
|
+using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
|
|
|
+OdometryOpt::OdometryOpt(int length){
|
|
|
+ inited_=false;
|
|
|
+ key_=0;
|
|
|
+ length_=length;
|
|
|
+ isam_param_.factorization = gtsam::ISAM2Params::QR;
|
|
|
+ isam_param_.relinearizeThreshold=0.1;
|
|
|
+ isam_param_.relinearizeSkip=1;
|
|
|
+ isam_=new gtsam::ISAM2(isam_param_);
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+OdometryOpt::~OdometryOpt(){
|
|
|
+ if(isam_){
|
|
|
+ delete isam_;
|
|
|
+ isam_= nullptr;
|
|
|
+ }
|
|
|
+ if(imu_preint_){
|
|
|
+ delete imu_preint_;
|
|
|
+ imu_preint_= nullptr;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void OdometryOpt::init(gtsam::Pose3 lidar2imu,double imu_freq,double accN,double gryN,double integrationN,
|
|
|
+ double imuAccBias,double imuGryBias){
|
|
|
+ lidar2Imu_=lidar2imu;
|
|
|
+ imu_freq_=imu_freq;
|
|
|
+ imuAccN_=accN;
|
|
|
+ imuGryN_=gryN;
|
|
|
+ imuIntegrationN_=integrationN;
|
|
|
+ imuAccBiasN_=imuAccBias;
|
|
|
+ imuGyrBiasN_=imuGryBias;
|
|
|
+
|
|
|
+ boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(9.804);
|
|
|
+ p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccN_, 2); // acc white noise in continuous
|
|
|
+ p->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGryN_, 2); // gyro white noise in continuous
|
|
|
+ p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuIntegrationN_, 2); // error committed in integrating position from velocities
|
|
|
+
|
|
|
+ gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
|
|
|
+ imu_preint_=new gtsam::PreintegratedImuMeasurements(p,prior_imu_bias);
|
|
|
+ lidar2Imu_ = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(0,0,0));
|
|
|
+}
|
|
|
+
|
|
|
+bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
|
|
|
+{
|
|
|
+ Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
|
|
|
+ if (vel.norm() > 30)
|
|
|
+ {
|
|
|
+ printf("Large velocity, reset IMU-preintegration!\n");
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
|
|
|
+ Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
|
|
|
+ if (ba.norm() > 1.0 || bg.norm() > 1.0)
|
|
|
+ {
|
|
|
+ printf("Large bias, reset IMU-preintegration!\n");
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ return false;
|
|
|
+}
|
|
|
+
|
|
|
+gtsam::Pose3 OdometryOpt::update(gtsam::Pose3 lidarPose,gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise,
|
|
|
+ std::deque<ImuData>& imuQueue) {
|
|
|
+ static int N=0;
|
|
|
+ printf(" frame : %d , Key :%d >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n",N++,key_);
|
|
|
+ if (inited_ == false) { //第一帧
|
|
|
+ reset_opt();
|
|
|
+ // initial pose
|
|
|
+ prevPose_ = lidarPose.compose(lidar2Imu_);
|
|
|
+ gtsam::noiseModel::Diagonal::shared_ptr firstPoseNoise = gtsam::noiseModel::Diagonal::Sigmas(
|
|
|
+ (gtsam::Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
|
|
|
+ gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, firstPoseNoise);
|
|
|
+ graph_.add(priorPose);
|
|
|
+ // initial velocity
|
|
|
+ prevVel_ = gtsam::Vector3(0, 0, 0);
|
|
|
+ gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
|
|
|
+ gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
|
|
|
+ graph_.add(priorVel);
|
|
|
+ // initial bias
|
|
|
+ prevBias_ = gtsam::imuBias::ConstantBias(gtsam::Vector3(0,0,0),gtsam::Vector3(0,0,0));
|
|
|
+ gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1);
|
|
|
+ gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
|
|
|
+ graph_.add(priorBias);
|
|
|
+ // add values
|
|
|
+ values_.insert(X(0), prevPose_);
|
|
|
+ values_.insert(V(0), prevVel_);
|
|
|
+ values_.insert(B(0), prevBias_);
|
|
|
+
|
|
|
+ // optimize once
|
|
|
+ isam_->update(graph_, values_);
|
|
|
+ printf(" frame 0 int pose:%f %f %f,%f %f %f, V:%f %f %f, \nBias : %f %f %f,%f %f %f\n",
|
|
|
+ prevPose_.x(),prevPose_.y(),prevPose_.z(),
|
|
|
+ prevPose_.rotation().roll(), prevPose_.rotation().pitch(), prevPose_.rotation().yaw(),
|
|
|
+ prevVel_.x(),prevVel_.y(),prevVel_.z(),
|
|
|
+ prevBias_.vector()[0],prevBias_.vector()[1],prevBias_.vector()[2],
|
|
|
+ prevBias_.vector()[3],prevBias_.vector()[4],prevBias_.vector()[5]);
|
|
|
+ graph_.resize(0);
|
|
|
+ values_.clear();
|
|
|
+ key_ = 1;
|
|
|
+ inited_ = true;
|
|
|
+ imu_preint_->resetIntegrationAndSetBias(prevBias_);
|
|
|
+ return prevPose_;
|
|
|
+ }
|
|
|
+ if (key_ == length_) {
|
|
|
+ gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(
|
|
|
+ isam_->marginalCovariance(X(key_ - 1)));
|
|
|
+ gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(
|
|
|
+ isam_->marginalCovariance(V(key_ - 1)));
|
|
|
+ gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(
|
|
|
+ isam_->marginalCovariance(B(key_ - 1)));
|
|
|
+ reset_opt();
|
|
|
+ // initial pose
|
|
|
+ gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
|
|
|
+ gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
|
|
|
+ gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
|
|
|
+ graph_.add(priorPose);
|
|
|
+ graph_.add(priorVel);
|
|
|
+ graph_.add(priorBias);
|
|
|
+ // add values
|
|
|
+ values_.insert(X(0), prevPose_);
|
|
|
+ values_.insert(V(0), prevVel_);
|
|
|
+ values_.insert(B(0), prevBias_);
|
|
|
+
|
|
|
+ // optimize once
|
|
|
+ isam_->update(graph_, values_);
|
|
|
+ graph_.resize(0);
|
|
|
+ values_.clear();
|
|
|
+ key_ = 1;
|
|
|
+
|
|
|
+ }
|
|
|
+ int queue_size=imuQueue.size();
|
|
|
+ double lastTime = -1;
|
|
|
+ while (!imuQueue.empty()) {
|
|
|
+ ImuData imu = imuQueue.front();
|
|
|
+ double dt = (lastTime < 0) ? 1.0 / imu_freq_ : imu.time - lastTime;
|
|
|
+ imu_preint_->integrateMeasurement(imu.acc, imu.angular, dt);
|
|
|
+ lastTime = imu.time;
|
|
|
+ imuQueue.pop_front();
|
|
|
+ /*printf("imu_preint_->integrateMeasurement(gtsam::Vector3(%f,%f,%f),gtsam::Vector3(%f,%f,%f),%f);\n",
|
|
|
+ imu.acc[0],imu.acc[1],imu.acc[2], imu.angular[0],imu.angular[1],imu.angular[2],dt);*/
|
|
|
+ if(dt<1e-6){
|
|
|
+ printf("error >>>>>>>> dt === %f\n",dt);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // insert predicted values
|
|
|
+ gtsam::NavState prevState(prevPose_,prevVel_);
|
|
|
+ gtsam::NavState propState = imu_preint_->predict(prevState, prevBias_);
|
|
|
+ // add pose factor
|
|
|
+ gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu_);
|
|
|
+ gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key_), curPose, priorPoseNoise);
|
|
|
+ graph_.add(pose_factor);
|
|
|
+
|
|
|
+ gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 0.1); // m/s
|
|
|
+ gtsam::PriorFactor<gtsam::Vector3> velFactor(V(key_),propState.v(),priorVelNoise);
|
|
|
+ graph_.add(velFactor);
|
|
|
+
|
|
|
+ gtsam::Vector noiseModelBetweenBias = (gtsam::Vector(6)
|
|
|
+ << imuAccBiasN_, imuAccBiasN_, imuAccBiasN_, imuGyrBiasN_, imuGyrBiasN_, imuGyrBiasN_).finished();
|
|
|
+
|
|
|
+ graph_.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key_ - 1), B(key_), gtsam::imuBias::ConstantBias(),
|
|
|
+ gtsam::noiseModel::Diagonal::Sigmas(
|
|
|
+ imu_preint_->deltaTij() * noiseModelBetweenBias)));
|
|
|
+
|
|
|
+ // add imu factor to graph
|
|
|
+ const gtsam::PreintegratedImuMeasurements &preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements &>(*imu_preint_);
|
|
|
+ gtsam::ImuFactor imu_factor(X(key_ - 1), V(key_ - 1), X(key_), V(key_), B(key_ - 1), preint_imu);
|
|
|
+ graph_.add(imu_factor);
|
|
|
+
|
|
|
+ gtsam::Pose3 lidar_preint=propState.pose().compose(lidar2Imu_.inverse());
|
|
|
+
|
|
|
+ printf(" frame %d init pose:%f %f %f,%f %f %f, V:%f %f %f, \nBias : %f %f %f,%f %f %f\n",N,
|
|
|
+ propState.pose().x(),propState.pose().y(),propState.pose().z(),
|
|
|
+ propState.pose().rotation().roll(), propState.pose().rotation().pitch(), propState.pose().rotation().yaw(),
|
|
|
+ propState.v().x(),propState.v().y(),propState.v().z(),
|
|
|
+ prevBias_.vector()[0],prevBias_.vector()[1],prevBias_.vector()[2],
|
|
|
+ prevBias_.vector()[3],prevBias_.vector()[4],prevBias_.vector()[5]);
|
|
|
+ values_.insert(X(key_), propState.pose());
|
|
|
+ values_.insert(V(key_), propState.v());
|
|
|
+ values_.insert(B(key_), prevBias_);
|
|
|
+
|
|
|
+ // optimize
|
|
|
+ isam_->update(graph_, values_);
|
|
|
+ isam_->update();
|
|
|
+
|
|
|
+ // Overwrite the beginning of the preintegration for the next step.
|
|
|
+ gtsam::Values result = isam_->calculateEstimate();
|
|
|
+ prevPose_ = result.at<gtsam::Pose3>(X(key_));
|
|
|
+ prevVel_ = result.at<gtsam::Vector3>(V(key_));
|
|
|
+ prevState_ = gtsam::NavState(prevPose_, prevVel_);
|
|
|
+ prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key_));
|
|
|
+
|
|
|
+ /*printf(" vel:%f %f %f,Bias:%f %f %f,%f %f %f\n",prevVel_.x(),prevVel_.y(),prevVel_.z(),
|
|
|
+ prevBias_.vector()[0],prevBias_.vector()[1],prevBias_.vector()[2],
|
|
|
+ prevBias_.vector()[3],prevBias_.vector()[4],prevBias_.vector()[5]);*/
|
|
|
+ // Reset the optimization preintegration object.
|
|
|
+ imu_preint_->resetIntegrationAndSetBias(prevBias_);
|
|
|
+ ++key_;
|
|
|
+ graph_.resize(0);
|
|
|
+ values_.clear();
|
|
|
+ return prevPose_;
|
|
|
+}
|
|
|
+
|
|
|
+void OdometryOpt::reset_opt(){
|
|
|
+ if(isam_){
|
|
|
+ delete isam_;
|
|
|
+ }
|
|
|
+ isam_ = new gtsam::ISAM2(isam_param_);
|
|
|
+ gtsam::NonlinearFactorGraph newGraphFactors;
|
|
|
+ graph_ = newGraphFactors;
|
|
|
+ gtsam::Values NewGraphValues;
|
|
|
+ values_ = NewGraphValues;
|
|
|
+ key_=1;
|
|
|
+}
|
