说明：
Example of application of ISAM2 for GPS-aided navigation on the KITTI

一、预先定义

1.1 symbol

using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)

1.2 kitti参数

外参数、噪声

struct KittiCalibration {double body_ptx;double body_pty;double body_ptz;double body_prx;double body_pry;double body_prz;double accelerometer_sigma;double gyroscope_sigma;double integration_sigma;double accelerometer_bias_sigma;double gyroscope_bias_sigma;double average_delta_t;
};

1.3 imu和gps测量

struct ImuMeasurement {double time;double dt;Vector3 accelerometer;Vector3 gyroscope;  // omega
};struct GpsMeasurement {double time;Vector3 position;  // x,y,z
};

1.4 加载数据集

void loadKittiData(KittiCalibration& kitti_calibration,vector<ImuMeasurement>& imu_measurements,vector<GpsMeasurement>& gps_measurements) {}

二、main函数

2.1 读入数据

  KittiCalibration kitti_calibration;vector<ImuMeasurement> imu_measurements;vector<GpsMeasurement> gps_measurements;loadKittiData(kitti_calibration, imu_measurements, gps_measurements);

body系 imu系的位姿变换

  Vector6 BodyP =(Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty,kitti_calibration.body_ptz, kitti_calibration.body_prx,kitti_calibration.body_pry, kitti_calibration.body_prz).finished();auto body_T_imu = Pose3::Expmap(BodyP);if (!body_T_imu.equals(Pose3(), 1e-5)) {printf("Currently only support IMUinBody is identity, i.e. IMU and body frame ""are the same");exit(-1);}

2.2 初始变量设置

  // Configure different variables// double t_offset = gps_measurements[0].time;size_t first_gps_pose = 1;size_t gps_skip = 10;  // Skip this many GPS measurements each timedouble g = 9.8;auto w_coriolis = Vector3::Zero();  // zero vector

$coriolisforce$

2.3 噪声和imu预积分参数设置

  // Configure noise modelsauto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07)).finished());// Set initial conditions for the estimated trajectory// initial pose is the reference frame (navigation frame)auto current_pose_global =Pose3(Rot3(), gps_measurements[first_gps_pose].position);// the vehicle is stationary at the beginning at position 0,0,0Vector3 current_velocity_global = Vector3::Zero();auto current_bias = imuBias::ConstantBias();  // init with zero biasauto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished());auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05)).finished());// Set IMU preintegration parametersMatrix33 measured_acc_cov =I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);Matrix33 measured_omega_cov =I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);// error committed in integrating position from velocitiesMatrix33 integration_error_cov =I_3x3 * pow(kitti_calibration.integration_sigma, 2);auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);imu_params->accelerometerCovariance =measured_acc_cov;  // acc white noise in continuousimu_params->integrationCovariance =integration_error_cov;  // integration uncertainty continuousimu_params->gyroscopeCovariance =measured_omega_cov;  // gyro white noise in continuousimu_params->omegaCoriolis = w_coriolis;

2.4 iSAM求解器、因子图、Values定义设置

  // Set ISAM2 parameters and create ISAM2 solver objectISAM2Params isam_params;isam_params.factorization = ISAM2Params::CHOLESKY;isam_params.relinearizeSkip = 10;ISAM2 isam(isam_params);// Create the factor graph and values object that will store new factors and// values to add to the incremental graphNonlinearFactorGraph new_factors;Values new_values;  // values storing the initial estimates of new nodes in// the factor graph

2.5 main loop

2.5.1 添加因子

  size_t j = 0;size_t included_imu_measurement_count = 0;for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {// At each non=IMU measurement we initialize a new node in the graphauto current_pose_key = X(i);auto current_vel_key = V(i);auto current_bias_key = B(i);double t = gps_measurements[i].time;if (i == first_gps_pose) {// Create initial estimate and prior on initial pose, velocity, and biases// 添加关于位姿、速度、bias的初始估计和prior factornew_values.insert(current_pose_key, current_pose_global);new_values.insert(current_vel_key, current_velocity_global);new_values.insert(current_bias_key, current_bias);new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x);new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b);} else {double t_previous = gps_measurements[i - 1].time;// Summarize IMU data between the previous GPS measurement and now// 两个GPS帧之间进行IMU预积分current_summarized_measurement =std::make_shared<PreintegratedImuMeasurements>(imu_params,current_bias);while (j < imu_measurements.size() && imu_measurements[j].time <= t) {if (imu_measurements[j].time >= t_previous) {current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer, imu_measurements[j].gyroscope,imu_measurements[j].dt);included_imu_measurement_count++;}j++;}// Create IMU factor// 创建IMU Factorauto previous_pose_key = X(i - 1);auto previous_vel_key = V(i - 1);auto previous_bias_key = B(i - 1);new_factors.emplace_shared<ImuFactor>(previous_pose_key, previous_vel_key, current_pose_key,current_vel_key, previous_bias_key, *current_summarized_measurement);// Bias evolution as given in the IMU metadata// 从元数据中获取bias的噪声模型auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(sqrt(included_imu_measurement_count) *kitti_calibration.accelerometer_bias_sigma),Vector3::Constant(sqrt(included_imu_measurement_count) *kitti_calibration.gyroscope_bias_sigma)).finished());// 利用bias添加BetweenFactornew_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(previous_bias_key, current_bias_key, imuBias::ConstantBias(),sigma_between_b);// Create GPS factor// 构建并向因子图中添加GPS factorauto gps_pose =Pose3(current_pose_global.rotation(), gps_measurements[i].position);if ((i % gps_skip) == 0) {//到达添加GPS因子的周期new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, gps_pose, noise_model_gps);new_values.insert(current_pose_key, gps_pose);printf("############ POSE INCLUDED AT TIME %.6lf ############\n",t);cout << gps_pose.translation();printf("\n\n");} else {new_values.insert(current_pose_key, current_pose_global);//否则只进行变量的添加}// Add initial values for velocity and bias based on the previous estimates// 添加速度和bias变量new_values.insert(current_vel_key, current_velocity_global);new_values.insert(current_bias_key, current_bias);// Update solver// =======================================================================// We accumulate 2*GPSskip GPS measurements before updating the solver a first so that the heading becomes observable.// 在第一次解算之前累积一定时间以便于可观测if (i > (first_gps_pose + 2 * gps_skip)) {printf("############ NEW FACTORS AT TIME %.6lf ############\n",t);new_factors.print();isam.update(new_factors, new_values);//更新解算// Reset the newFactors and newValues list// 重置graphs和Valuesnew_factors.resize(0);new_values.clear();// Extract the result/current estimatesValues result = isam.calculateEstimate();//从isam2求解器中读取值current_pose_global = result.at<Pose3>(current_pose_key);current_velocity_global = result.at<Vector3>(current_vel_key);current_bias = result.at<imuBias::ConstantBias>(current_bias_key);printf("\n############ POSE AT TIME %lf ############\n", t);current_pose_global.print();printf("\n\n");}}}

2.5.2 保存文件

  // Save results to fileprintf("\nWriting results to file...\n");FILE* fp_out = fopen(output_filename.c_str(), "w+");fprintf(fp_out,"#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");Values result = isam.calculateEstimate();for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {auto pose_key = X(i);auto vel_key = V(i);auto bias_key = B(i);auto pose = result.at<Pose3>(pose_key);auto velocity = result.at<Vector3>(vel_key);auto bias = result.at<imuBias::ConstantBias>(bias_key);auto pose_quat = pose.rotation().toQuaternion();auto gps = gps_measurements[i].position;cout << "State at #" << i << endl;cout << "Pose:" << endl << pose << endl;cout << "Velocity:" << endl << velocity << endl;cout << "Bias:" << endl << bias << endl;fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",gps_measurements[i].time, pose.x(), pose.y(), pose.z(),pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0),gps(1), gps(2));}fclose(fp_out);

三、mention

        isam.update(new_factors, new_values);//更新解算// Reset the newFactors and newValues list// 重置graphs和Valuesnew_factors.resize(0);new_values.clear();// Extract the result/current estimatesValues result = isam.calculateEstimate();//从isam2求解器中读取值

3.1 update

graphs和Values都是const &传递
所以iSAM求解器和

        new_factors.resize(0);new_values.clear();

上述代码不会影响iSAM求解器中的值
isam优化器的重置见lio-sam有例子

    /*** 重置ISAM2优化器*/void resetOptimization(){gtsam::ISAM2Params optParameters;optParameters.relinearizeThreshold = 0.1;optParameters.relinearizeSkip = 1;optimizer = gtsam::ISAM2(optParameters);gtsam::NonlinearFactorGraph newGraphFactors;graphFactors = newGraphFactors;gtsam::Values NewGraphValues;graphValues = NewGraphValues;}

相当于重新定义

3.1.1 gtsam中iSAM求解器的update成员函数

  /*** Add new factors, updating the solution and relinearizing as needed.** Optionally, this function remove existing factors from the system to enable* behaviors such as swapping existing factors with new ones.** Add new measurements, and optionally new variables, to the current system.* This runs a full step of the ISAM2 algorithm, relinearizing and updating* the solution as needed, according to the wildfire and relinearize* thresholds.** @param newFactors The new factors to be added to the system* @param newTheta Initialization points for new variables to be added to the* system. You must include here all new variables occuring in newFactors* (which were not already in the system).  There must not be any variables* here that do not occur in newFactors, and additionally, variables that were* already in the system must not be included here.* @param removeFactorIndices Indices of factors to remove from system* @param force_relinearize Relinearize any variables whose delta magnitude is* sufficiently large (Params::relinearizeThreshold), regardless of the* relinearization interval (Params::relinearizeSkip).* @param constrainedKeys is an optional map of keys to group labels, such* that a variable can be constrained to a particular grouping in the* BayesTree* @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will* hold at a constant linearization point, regardless of the size of the* linear delta* @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will* re-eliminate, regardless of the size of the linear delta. This allows the* provided keys to be reordered.* @return An ISAM2Result struct containing information about the update*/virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),const Values& newTheta = Values(),const FactorIndices& removeFactorIndices = FactorIndices(),const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,const boost::optional<FastList<Key> >& noRelinKeys = boost::none,const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,bool force_relinearize = false);/*** Add new factors, updating the solution and relinearizing as needed.** Alternative signature of update() (see its documentation above), with all* additional parameters in one structure. This form makes easier to keep* future API/ABI compatibility if parameters change.** @param newFactors The new factors to be added to the system* @param newTheta Initialization points for new variables to be added to the* system. You must include here all new variables occuring in newFactors* (which were not already in the system).  There must not be any variables* here that do not occur in newFactors, and additionally, variables that were* already in the system must not be included here.* @param updateParams Additional parameters to control relinearization,* constrained keys, etc.* @return An ISAM2Result struct containing information about the update* @note No default parameters to avoid ambiguous call errors.*/virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,const Values& newTheta,const ISAM2UpdateParams& updateParams);