ISAM2_SmartFactorStereo_IMU.cpp

• 一、imu結構體
• • 1.1 IMUHelper()構造函數
  • 1.2 預積分模型的參數設置
  • 1.3 坐標系
  • 1.4 其他
  • 1.5 成員變量
• 二、main函數
• • 2.1 構造雙目相機模型
  • 2.2 創建因子圖
  • • 2.2.1 SmartStereoProjectionPoseFactor
  • 2.3 初值初始化
  • 2.4讀取傳感器數據進行向因子圖中加入因子
  • 2.5 優化

利用stereo相機模式和imu的集成對iSAM2算法進行的測試

一、imu結構體

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()構造函數

gaussian構造一個高斯噪聲模型
huber核函數：系數$$\delta$$和加上高斯噪聲模型

velocityNoiseModel也是類似的操作

1.2 預積分模型的參數設置

p：
高斯白噪聲
bias的噪聲
預測的從速度進行位置積分的誤差 和 預積分的偏置協方差

1.3 坐標系

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

本來body系是和imu系重合的，但此時body系在左目相機上 設置一個T矩陣

1.4 其他

bias初值設置
先驗位姿初值設置

先前和傳播狀態設置
預積分初始化

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函數

2.1 構造雙目相機模型

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

這里的K包含了相機內參和基線長度(雙目)

2.2 創建因子圖

// 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讀取傳感器數據進行向因子圖中加入因子

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 優化

有數據進去時候進行優化

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;

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。