ISAM2_SmartFactorStereo_IMU.cpp

• 一、imu結(jié)構(gòu)體
• • 1.1 IMUHelper()構(gòu)造函數(shù)
  • 1.2 預(yù)積分模型的參數(shù)設(shè)置
  • 1.3 坐標(biāo)系
  • 1.4 其他
  • 1.5 成員變量
• 二、main函數(shù)
• • 2.1 構(gòu)造雙目相機(jī)模型
  • 2.2 創(chuàng)建因子圖
  • • 2.2.1 SmartStereoProjectionPoseFactor
  • 2.3 初值初始化
  • 2.4讀取傳感器數(shù)據(jù)進(jìn)行向因子圖中加入因子
  • 2.5 優(yōu)化

利用stereo相機(jī)模式和imu的集成對(duì)iSAM2算法進(jìn)行的測(cè)試

一、imu結(jié)構(gòu)體

struct IMUHelper {IMUHelper() {{auto gaussian = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3)).finished());auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), gaussian);biasNoiseModel = huber;}{auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01);auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), gaussian);velocityNoiseModel = huber;}// expect IMU to be rotated in image space co-ordsauto p = boost::make_shared<PreintegratedCombinedMeasurements::Params>(Vector3(0.0, 9.8, 0.0));p->accelerometerCovariance =I_3x3 * pow(0.0565, 2.0); // acc white noise in continuousp->integrationCovariance =I_3x3 * 1e-9; // integration uncertainty continuousp->gyroscopeCovariance =I_3x3 * pow(4.0e-5, 2.0); // gyro white noise in continuousp->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuousp->biasOmegaCovariance =I_3x3 * pow(0.001, 2.0); // gyro bias in continuousp->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;// body to IMU rotationRot3 iRb(0.036129, -0.998727, 0.035207,0.045417, -0.033553, -0.998404,0.998315, 0.037670, 0.044147);// body to IMU translation (meters)Point3 iTb(0.03, -0.025, -0.06);// body in this example is the left camerap->body_P_sensor = Pose3(iRb, iTb);Rot3 prior_rotation = Rot3(I_3x3);Pose3 prior_pose(prior_rotation, Point3(0, 0, 0));Vector3 acc_bias(0.0, -0.0942015, 0.0); // in camera frameVector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias);prevState = NavState(prior_pose, Vector3(0, 0, 0));propState = prevState;prevBias = priorImuBias;preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias);}imuBias::ConstantBias priorImuBias; // assume zero initial biasnoiseModel::Robust::shared_ptr velocityNoiseModel;noiseModel::Robust::shared_ptr biasNoiseModel;NavState prevState;NavState propState;imuBias::ConstantBias prevBias;PreintegratedCombinedMeasurements* preintegrated; };

1.1 IMUHelper()構(gòu)造函數(shù)

gaussian構(gòu)造一個(gè)高斯噪聲模型
huber核函數(shù)：系數(shù)$$\delta$$和加上高斯噪聲模型

velocityNoiseModel也是類似的操作

1.2 預(yù)積分模型的參數(shù)設(shè)置

p：
高斯白噪聲
bias的噪聲
預(yù)測(cè)的從速度進(jìn)行位置積分的誤差 和 預(yù)積分的偏置協(xié)方差

1.3 坐標(biāo)系

// body in this example is the left camerap->body_P_sensor = Pose3(iRb, iTb);

本來(lái)body系是和imu系重合的，但此時(shí)body系在左目相機(jī)上 設(shè)置一個(gè)T矩陣

1.4 其他

bias初值設(shè)置
先驗(yàn)位姿初值設(shè)置

先前和傳播狀態(tài)設(shè)置
預(yù)積分初始化

1.5 成員變量

imuBias::ConstantBias priorImuBias; // assume zero initial biasnoiseModel::Robust::shared_ptr velocityNoiseModel;noiseModel::Robust::shared_ptr biasNoiseModel;NavState prevState;NavState propState;imuBias::ConstantBias prevBias;PreintegratedCombinedMeasurements* preintegrated;

二、main函數(shù)

2.1 構(gòu)造雙目相機(jī)模型

Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));

這里的K包含了相機(jī)內(nèi)參和基線長(zhǎng)度(雙目)

2.2 創(chuàng)建因子圖

// Create a factor graphstd::map<size_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;NonlinearFactorGraph graph;Values initialEstimate;IMUHelper imu;

2.2.1 SmartStereoProjectionPoseFactor

This factor assumes that camera calibration is fixed, but each camera has its own calibration. The factor only constrains poses (variable dimension is 6). This factor requires that values contains the involved poses (Pose3).
只約束位姿
一元因子

2.3 初值初始化

// Pose prior - at identityauto priorPoseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());graph.addPrior(X(1), Pose3::identity(), priorPoseNoise);initialEstimate.insert(X(0), Pose3::identity());// Bias priorgraph.addPrior(B(1), imu.priorImuBias,imu.biasNoiseModel);initialEstimate.insert(B(0), imu.priorImuBias);// Velocity prior - assume stationarygraph.addPrior(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel);initialEstimate.insert(V(0), Vector3(0, 0, 0));

2.4讀取傳感器數(shù)據(jù)進(jìn)行向因子圖中加入因子

if (type == 'i') { // Process IMU measurementdouble ax, ay, az;double gx, gy, gz;double dt = 1 / 800.0; // IMU at ~800Hzss >> ax;ss >> ay;ss >> az;ss >> gx;ss >> gy;ss >> gz;Vector3 acc(ax, ay, az);Vector3 gyr(gx, gy, gz);imu.preintegrated->integrateMeasurement(acc, gyr, dt);} else if (type == 's') { // Process stereo measurementint landmark;double xl, xr, y;ss >> landmark;ss >> xl;ss >> xr;ss >> y;if (smartFactors.count(landmark) == 0) {auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(new SmartStereoProjectionPoseFactor(gaussian, params));graph.push_back(smartFactors[landmark]);}smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);} else {throw runtime_error("unexpected data type: " + string(1, type));}

2.5 優(yōu)化

有數(shù)據(jù)進(jìn)去時(shí)候進(jìn)行優(yōu)化

if (frame != lastFrame || in.eof()) {cout << "Running iSAM for frame: " << lastFrame << "\n";initialEstimate.insert(X(lastFrame), Pose3::identity());initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));initialEstimate.insert(B(lastFrame), imu.prevBias);CombinedImuFactor imuFactor(X(lastFrame - 1), V(lastFrame - 1),X(lastFrame), V(lastFrame), B(lastFrame - 1),B(lastFrame), *imu.preintegrated);graph.add(imuFactor);isam.update(graph, initialEstimate);Values currentEstimate = isam.calculateEstimate();imu.propState = imu.preintegrated->predict(imu.prevState, imu.prevBias);imu.prevState = NavState(currentEstimate.at<Pose3>(X(lastFrame)),currentEstimate.at<Vector3>(V(lastFrame)));imu.prevBias = currentEstimate.at<imuBias::ConstantBias>(B(lastFrame));imu.preintegrated->resetIntegrationAndSetBias(imu.prevBias);graph.resize(0);initialEstimate.clear();if (in.eof()) {break;}}/*......*//*......*/lastFrame = frame;

總結(jié)

以上是生活随笔為你收集整理的002_2 gtsam/unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。