多传感器融合SLAM --- 10.LIO-SAM前端代码分析 imuPreintegration.cpp
发布时间
阅读量：
阅读量
目录
0.流程图
1.main函数
2 IMU预积分类
2.1 代码详细注释
2.2 分块讲解--构造函数讲解
2.2.1 以IMU信息怎么来的为例介绍ROS通信机制
2.2.2 构造函数讲解
2.2.3 收到IMU信息的回调函数
2.2.4 收到IMU信息的回调函数
2.2.5 优化函数置位 resetOptimization
3 TF类
3.1 流程图
3.2 代码详细注释
0.流程图
1.main函数
> 1. int main(int argc, char** argv)
>
> 2. {
>
> 3. ros::init(argc, argv, "roboat_loam");
>
> 4.
>
> 5. IMUPreintegration ImuP;
>
> 6.
>
> 7. TransformFusion TF;
>
> 8.
>
> 9. ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
>
> 10.
>
> 11. // 分配4个线程执行这个节点
>
> 12. ros::MultiThreadedSpinner spinner(4);
>
> 13.
>
> 14. // 程序阻塞，后台等待回调函数的执行
>
> 15. spinner.spin();
>
> 16.
>
> 17. return 0;
>
> 18. }
>
>
>
>
> 代码解读此程序的入口函数名为main，其接收命令行参数argc和argv两个变量。
该节点初始化函数调用ros::init执行初始化操作，请注意该函数将使用命令行参数中的数据。
特别地，请注意该节点将设置节点名称为'roboat_loam'。
接下来我们将创建一个名为ImuP的对象实例化自定义类IMUPreintegration。
随后再创建一个名为TF的对象实例化另一个自定义类TransformFusion。
为了方便后续调试与查看信息我们向ROS日志系统发送了一条消息"IMU Preintegration Started"并使用绿色显示以便于识别。
接下来我们将创建一个名为spinner的对象实例化MultiThreadedSpinner类并指定其内部线程数目为4个。
最后启动这些线程以执行相应的回调函数请求用户耐心等待这些操作完成后再返回主程序状态码0表示程序正常退出。
2 IMU预积分类
2.1 代码详细注释
> 1. class IMUPreintegration : public ParamServer
>
> 2. {
>
> 3. public:
>
> 4.
>
> 5. std::mutex mtx;
>
> 6.
>
> 7. ros::Subscriber subImu;
>
> 8. ros::Subscriber subOdometry;
>
> 9. ros::Publisher pubImuOdometry;
>
> 10.
>
> 11. bool systemInitialized = false;
>
> 12.
>
> 13. gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
>
> 14. gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
>
> 15. gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
>
> 16. gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
>
> 17. gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;
>
> 18. gtsam::Vector noiseModelBetweenBias;
>
> 19.
>
> 20.
>
> 21. // `imuIntegratorImu_`是一个指向`gtsam::PreintegratedImuMeasurements`对象的指针。
>
> 22. //`gtsam::PreintegratedImuMeasurements`是GTSAM（Geometric Tracking and Mapping）库中的一个类，用于对IMU数据进行预积分（即将连续的IMU测量值转化为姿态和速度的增量）。
>
> 23. // 该类提供了用于预积分的函数和方法，可以方便地处理IMU数据并生成相应的姿态和速度增量。
>
> 24. //在给定IMU测量值和时间间隔的情况下，`imuIntegratorImu_`指针可以调用`integrateMeasurement()`函数来更新预积分状态，以及使用`predict()`函数来进行状态预测。
>
> 25. //通过使用`imuIntegratorImu_`对象，可以方便地进行IMU数据的处理、积分和状态预测，从而为后续的SLAM（Simultaneous Localization and Mapping）算法提供准确的姿态和速度信息。
>
> 26.
>
> 27. //更新预积分状态是指根据新的IMU测量值，将其积分到先前的状态中，以获得更新后的姿态和速度信息。
>
> 28. //
>
> 29. //在IMU数据预积分中，连续的IMU测量值（包括线性加速度和角速度）被积分成姿态和速度的增量。这些增量代表了从先前状态到当前状态的运动变化。
>
> 30. //
>
> 31. //当接收到新的IMU测量值时，通过调用`integrateMeasurement()`函数将这些测量值积分到先前的状态中。积分过程会考虑时间间隔、加速度、角速度以及先前的状态信息，从而更新预积分状态。
>
> 32. //
>
> 33. //通过更新预积分状态，我们可以获得更准确的姿态和速度信息，这对于后续的SLAM算法或运动估计非常重要。预积分状态的更新可以在每次接收到新的IMU测量值时进行，以保持状态的准确性和连续性。
>
> 34. gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
>
> 35. gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
>
> 36.
>
> 37. std::deque<sensor_msgs::Imu> imuQueOpt;
>
> 38. std::deque<sensor_msgs::Imu> imuQueImu;
>
> 39.
>
> 40. gtsam::Pose3 prevPose_;
>
> 41. gtsam::Vector3 prevVel_;
>
> 42. gtsam::NavState prevState_;
>
> 43. gtsam::imuBias::ConstantBias prevBias_;
>
> 44.
>
> 45. // `prevStateOdom`和`prevBiasOdom`是用于保存上一个时刻的姿态状态和IMU偏差的变量。
>
> 46. //
>
> 47. //- `prevStateOdom`是上一个时刻的导航状态（NavState），包括姿态和速度信息。它用于在预测过程中作为初始状态，根据当前的IMU测量值更新姿态和速度。
>
> 48. //
>
> 49. //- `prevBiasOdom`是上一个时刻的IMU偏差（ConstantBias），包括加速度计和陀螺仪的零偏。它用于在预测过程中作为初始偏差，根据当前的IMU测量值更新偏差。
>
> 50. //
>
> 51. // 这些变量的目的是跟踪先前时刻的状态和偏差，并在接收到新的IMU测量值时使用它们来进行预测和更新。通过维护先前的状态和偏差信息，可以实现连续的状态估计和更准确的运动预测。
>
> 52. gtsam::NavState prevStateOdom;
>
> 53. gtsam::imuBias::ConstantBias prevBiasOdom;
>
> 54.
>
> 55. bool doneFirstOpt = false;
>
> 56. double lastImuT_imu = -1;
>
> 57. double lastImuT_opt = -1;
>
> 58.
>
> 59. gtsam::ISAM2 optimizer;
>
> 60. gtsam::NonlinearFactorGraph graphFactors;
>
> 61. gtsam::Values graphValues;
>
> 62.
>
> 63. const double delta_t = 0;
>
> 64.
>
> 65. int key = 1;
>
> 66.
>
> 67. // T_bl: tramsform points from lidar frame to imu frame
>
> 68. gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
>
> 69. // T_lb: tramsform points from imu frame to lidar frame
>
> 70. gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
>
> 71.
>
> 72. IMUPreintegration()
>
> 73. {
>
> 74. // 订阅 IMU 话题，缓冲区大小为 2000，回调函数为 imuHandler()
>
> 75. subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
>
> 76.
>
> 77. // 订阅 Odometry 话题，缓冲区大小为 5，回调函数为 odometryHandler() lio_sam/mapping/odometry_incremental这个是后端优化节点发过来的关于LIDAR里程计的信息
>
> 78. subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
>
> 79.
>
> 80. // 声明发布 Odometry 话题，话题名称为 odomTopic+"_incremental"，队列大小为 2000
>
> 81. pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic+"_incremental", 2000);
>
> 82.
>
> 83. // 创建 gtsam::PreintegrationParams 对象 p，设置重力为 imuGravity
>
> 84. boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
>
> 85.
>
> 86. // 加速度积分噪声
>
> 87. p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
>
> 88. // 角速度积分噪声
>
> 89. p->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
>
> 90. // 速度积分得到位置的噪声
>
> 91. p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
>
> 92. gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
>
> 93.
>
> 94. // 初始位姿置信度设置比较高
>
> 95. priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
>
> 96. // 初始速度置信度设置差一些
>
> 97. priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
>
> 98. // 零偏的置信度设置高一些
>
> 99. priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
>
> 100. // 正常情况下LIDAR odom的协方差矩阵
>
> 101. correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
>
> 102. // lidar odom退化之后的协方差矩阵
>
> 103. correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
>
> 104. // 两帧bias的协方差矩阵
>
> 105. noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
>
> 106.
>
> 107. // 这里两个预积分量一个是用来做预积分优化，一个推算最新位姿的
>
> 108. imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
>
> 109. imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
>
> 110. }
>
> 111.
>
> 112. void resetOptimization()
>
> 113. {
>
> 114. // 创建一个用于配置优化器的参数对象
>
> 115. gtsam::ISAM2Params optParameters;
>
> 116.
>
> 117. // 设置参数对象中的属性，用于控制重线性化的阈值
>
> 118. optParameters.relinearizeThreshold = 0.1;
>
> 119.
>
> 120. // 设置参数对象中的属性，用于控制跳过重线性化的步长
>
> 121. optParameters.relinearizeSkip = 1;
>
> 122.
>
> 123. // 使用指定的参数对象创建一个ISAM2优化器
>
> 124. optimizer = gtsam::ISAM2(optParameters);
>
> 125.
>
> 126. // 创建一个新的非线性因子图对象
>
> 127. gtsam::NonlinearFactorGraph newGraphFactors;
>
> 128.
>
> 129. // 将新创建的非线性因子图对象赋值给图因子属性
>
> 130. graphFactors = newGraphFactors;
>
> 131.
>
> 132. // 创建一个新的值对象
>
> 133. gtsam::Values NewGraphValues;
>
> 134.
>
> 135. // 将新创建的值对象赋值给图值属性
>
> 136. graphValues = NewGraphValues;
>
> 137. }
>
> 138.
>
> 139. void resetParams()
>
> 140. {
>
> 141. lastImuT_imu = -1;
>
> 142. doneFirstOpt = false;
>
> 143. systemInitialized = false;
>
> 144. }
>
> 145.
>
> 146. void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
>
> 147. {
>
> 148. std::lock_guard<std::mutex> lock(mtx);
>
> 149.
>
> 150. // ROS_TIME函数用于获取当前odomMsg的时间戳的值
>
> 151. double currentCorrectionTime = ROS_TIME(odomMsg);
>
> 152.
>
> 153. // make sure we have imu data to integrate
>
> 154. // 判断IMU队列是否有数据
>
> 155. if (imuQueOpt.empty())
>
> 156. return;
>
> 157.
>
> 158. // 获取里程计 平移 XYZ 旋转xyzw
>
> 159. float p_x = odomMsg->pose.pose.position.x;
>
> 160. float p_y = odomMsg->pose.pose.position.y;
>
> 161. float p_z = odomMsg->pose.pose.position.z;
>
> 162. float r_x = odomMsg->pose.pose.orientation.x;
>
> 163. float r_y = odomMsg->pose.pose.orientation.y;
>
> 164. float r_z = odomMsg->pose.pose.orientation.z;
>
> 165. float r_w = odomMsg->pose.pose.orientation.w;
>
> 166.
>
> 167. // 有退化风险（里程计的精度有风险下降）
>
> 168. bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
>
> 169.
>
> 170. // 位姿转化成gtsam格式（旋转 + 平移）
>
> 171. gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
>
> 172.
>
> 173.
>
> 174. // 0. 初始化
>
> 175. if (systemInitialized == false)
>
> 176. {
>
> 177. // 将优化问题复位
>
> 178. resetOptimization();
>
> 179.
>
> 180. // 把IMU数据扔掉
>
> 181. while (!imuQueOpt.empty())
>
> 182. {
>
> 183. // 如果IMU数据的时间戳 < 当前里程计的时间戳
>
> 184. if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
>
> 185. {
>
> 186. // 被扔掉IMU数据的时间戳，这个变量最终保存刚好小于当前里程计时间戳的帧
>
> 187. lastImuT_opt = ROS_TIME(&imuQueOpt.front());
>
> 188. imuQueOpt.pop_front();
>
> 189. }
>
> 190. else
>
> 191. break;
>
> 192. }
>
> 193.
>
> 194. 添加约束
>
> 195.
>
> 196. // 将LIDAR的位姿转换到IMU坐标系下
>
> 197. prevPose_ = lidarPose.compose(lidar2Imu);
>
> 198. gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
>
> 199. graphFactors.add(priorPose);
>
> 200.
>
> 201. // gtsam::Vector3表示三自由度速度，由于刚开始确实不知道是多少，直接置为0了
>
> 202. prevVel_ = gtsam::Vector3(0, 0, 0);
>
> 203. // 设置其初始值和置信度
>
> 204. gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
>
> 205. // 约束加入到因子中
>
> 206. graphFactors.add(priorVel);
>
> 207.
>
> 208.
>
> 209.
>
> 210. // gtsam::imuBias::ConstantBias表示IMU零偏 X(Pose3 xyzrpy) Y(Vel xyz) B(BIAS axayazgxgygz)
>
> 211. prevBias_ = gtsam::imuBias::ConstantBias();
>
> 212. // 这段代码是使用GTSAM库中的PriorFactor类创建一个先验因子（PriorFactor），用于对IMU偏差（imuBias::ConstantBias）进行约束。
>
> 213. //具体解释如下：
>
> 214. // gtsam::PriorFactor<gtsam::imuBias::ConstantBias>：这是一个GTSAM库中的PriorFactor类的模板实例化，指定了模板参数为gtsam::imuBias::ConstantBias，即IMU偏差的类型。
>
> 215. // priorBias：这是创建的先验因子对象的名称。
>
> 216. // B(0)：这是一个表示IMU偏差的变量，其中(0)是索引号，用于区分多个偏差变量。这里使用索引号为0的偏差变量。
>
> 217. // prevBias_：这是一个先前的IMU偏差的变量，它被用作先验因子的先验值。
>
> 218. // priorBiasNoise：这是一个表示偏差噪声的对象，用于指定对IMU偏差的不确定性。
>
> 219. // 综合起来，这段代码创建了一个先验因子，它约束了IMU偏差变量B(0)的值必须接近于先前的偏差值prevBias_，并且在计算图优化过程中使用了指定的偏差噪声来表示对这个约束的不确定性。
>
> 220. gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
>
> 221. // 将约束添加到因子中
>
> 222. graphFactors.add(priorBias);
>
> 223.
>
> 224.
>
> 225. 添加状态
>
> 226.
>
> 227. // 添加状态量
>
> 228. graphValues.insert(X(0), prevPose_);
>
> 229. graphValues.insert(V(0), prevVel_);
>
> 230. graphValues.insert(B(0), prevBias_);
>
> 231.
>
> 232. 约束graphFactors + 状态 graphValues
>
> 233. // 约束和状态添加完了，开始优化
>
> 234. optimizer.update(graphFactors, graphValues);
>
> 235. // 清空变量 方便下一次进行优化
>
> 236. graphFactors.resize(0);
>
> 237. graphValues.clear();
>
> 238.
>
> 239. // 预积分的接口，使用初始零值进行初始化
>
> 240. imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
>
> 241. imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
>
> 242.
>
> 243. key = 1;
>
> 244. // 初始化标志位置为true
>
> 245. systemInitialized = true;
>
> 246. return;
>
> 247. }
>
> 248.
>
> 249.
>
> 250. // reset graph for speed
>
> 251. // 执行100次 当isam优化器中加入较多的约束后，为了避免运算时间变长，直接清空 初始化因子图优化问题
>
> 252. if (key == 100)
>
> 253. {
>
> 254. // get updated noise before reset
>
> 255. // 取出最新时刻位姿 速度 零偏的协方差矩阵
>
> 256. gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
>
> 257. gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
>
> 258. gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
>
> 259. // reset graph
>
> 260. // 复位整个优化问题
>
> 261. resetOptimization();
>
> 262. // add pose
>
> 263. gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
>
> 264. graphFactors.add(priorPose);
>
> 265. // add velocity
>
> 266. gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
>
> 267. graphFactors.add(priorVel);
>
> 268. // add bias
>
> 269. gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
>
> 270. graphFactors.add(priorBias);
>
> 271. // add values
>
> 272. graphValues.insert(X(0), prevPose_);
>
> 273. graphValues.insert(V(0), prevVel_);
>
> 274. graphValues.insert(B(0), prevBias_);
>
> 275. // optimize once
>
> 276. optimizer.update(graphFactors, graphValues);
>
> 277. graphFactors.resize(0);
>
> 278. graphValues.clear();
>
> 279.
>
> 280. key = 1;
>
> 281. }
>
> 282.
>
> 283.
>
> 284. // 1. 初始化结束后，我们已经有一个LIDAR坐标系转换到IMU坐标系下的一个位姿，下面开始形成预积分的约束
>
> 285. while (!imuQueOpt.empty())
>
> 286. {
>
> 287. // pop and integrate imu data that is between two optimizations
>
> 288. sensor_msgs::Imu *thisImu = &imuQueOpt.front();
>
> 289. double imuTime = ROS_TIME(thisImu);
>
> 290. // IMU的时间戳要小于LIDAR的时间戳
>
> 291. if (imuTime < currentCorrectionTime - delta_t)
>
> 292. {
>
> 293. // 计算两个IMU之间的时间戳
>
> 294. double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
>
> 295. // 调用预积分接口将IMU数据送进去处理
>
> 296. imuIntegratorOpt_->integrateMeasurement(
>
> 297. gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
>
> 298. gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
>
> 299.
>
> 300. lastImuT_opt = imuTime;
>
> 301. imuQueOpt.pop_front();
>
> 302. }
>
> 303. else
>
> 304. break;
>
> 305. }
>
> 306.
>
> 307. // 两帧间IMU预积分完成(LIDAR两帧，IMU很多帧)后，将其转换成预积分约束
>
> 308. const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
>
> 309.
>
> 310. // 预积分的约束对相邻两帧之间的位姿 速度 零偏形成约束
>
> 311. // 上一帧的位姿、速度 当前帧的位姿、速度 上一帧的零偏
>
> 312. gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
>
> 313. // 约束加入因子图
>
> 314. graphFactors.add(imu_factor);
>
> 315.
>
> 316. // 这段代码是使用GTSAM库中的BetweenFactor类创建一个因子（BetweenFactor），用于表示两个IMU偏差变量之间的关系。
>
> 317. // graphFactors：这是一个图（graph）对象，用于存储因子（factor）。
>
> 318. // gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>：这是一个GTSAM库中的BetweenFactor类的模板实例化，指定了模板参数为gtsam::imuBias::ConstantBias，即IMU偏差的类型。
>
> 319. // B(key - 1)：这是表示先前的IMU偏差变量的变量，其中key - 1是索引号，用于区分多个偏差变量。这里使用索引号为key - 1的偏差变量。
>
> 320. // B(key)：这是表示当前的IMU偏差变量的变量，其中key是索引号，用于区分多个偏差变量。这里使用索引号为key的偏差变量。
>
> 321. // gtsam::imuBias::ConstantBias()：这是一个默认构造的IMU偏差对象，作为两个偏差变量之间的初始估计。
>
> 322. // gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)：这是一个表示噪声模型的对象，用于表示两个偏差变量之间的不确定性。imuIntegratorOpt_->deltaTij()表示IMU积分器在两个时间步之间的时间差，noiseModelBetweenBias是一个噪声模型，通过对两个时间步之间的时间差进行标准化来定义偏差之间的不确定性。
>
> 323. // 综合起来，这段代码创建了一个因子，它表示了两个IMU偏差变量之间的关系，并使用指定的噪声模型来表示这个关系的不确定性。然后，该因子被添加到一个图对象中，以便在后续的图优化过程中使用。
>
> 324. graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
>
> 325. gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
>
> 326.
>
> 327. // 这段代码将激光雷达位姿(lidarPose)与激光雷达坐标系到IMU坐标系之间的变换关系(lidar2Imu)进行组合操作，得到了当前的位姿(curPose)。
>
> 328. // 这通常用于将激光雷达位姿转换到IMU坐标系下的位姿
>
> 329. gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
>
> 330. // LIDAR位姿补偿到IMU坐标系下同时根据是否退化选择不同的置信度作为这一帧的先验估计
>
> 331. gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
>
> 332. // 加入到因子图
>
> 333. graphFactors.add(pose_factor);
>
> 334.
>
> 335. // 根据上一时刻的状态 结合预积分的结果 对当前状态进行预测 prevState_上一关键帧的位姿 + prevBias_上一关键帧的零偏
>
> 336. // 这段代码使用GTSAM库中的ImuIntegrator类的predict()函数，基于先前的状态和偏差预测生成一个新的导航状态。
>
> 337. // gtsam::NavState propState_：这是一个NavState类型的对象，用于存储生成的预测位姿
>
> 338. // imuIntegratorOpt_：这是一个ImuIntegrator类的指针或引用，用于执行IMU积分和状态预测操作。
>
> 339. // prevState_：这是一个先前的导航状态对象，作为IMU积分的起始状态。
>
> 340. // prevBias_：这是一个先前的IMU偏差对象，用作IMU积分的起始偏差。
>
> 341. // predict(prevState_, prevBias_)：这是ImuIntegrator类的成员函数，根据先前的状态和偏差，通过IMU积分预测生成一个新的位姿状态。
>
> 342. // 综合起来，这段代码使用ImuIntegrator类的predict()函数，基于先前的导航状态和偏差，进行IMU积分预测，生成一个新的导航状态，并将其存储在propState_中。
>
> 343. // 这个预测的导航状态通常用于在导航和状态估计问题中进行运动预测和滤波更新。
>
> 344. gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
>
> 345.
>
> 346. // 预测量作为初始值插入到因子图中
>
> 347. graphValues.insert(X(key), propState_.pose());
>
> 348. graphValues.insert(V(key), propState_.v());
>
> 349. graphValues.insert(B(key), prevBias_);
>
> 350.
>
> 351. // 执行因子图优化
>
> 352. optimizer.update(graphFactors, graphValues);
>
> 353. optimizer.update();
>
> 354.
>
> 355. // 清空
>
> 356. graphFactors.resize(0);
>
> 357. graphValues.clear();
>
> 358.
>
> 359. // 获取优化结果
>
> 360. gtsam::Values result = optimizer.calculateEstimate();
>
> 361.
>
> 362. // 获取优化后的当前状态作为当前帧的最佳估计
>
> 363. prevPose_ = result.at<gtsam::Pose3>(X(key));
>
> 364. prevVel_ = result.at<gtsam::Vector3>(V(key));
>
> 365. prevState_ = gtsam::NavState(prevPose_, prevVel_);
>
> 366. prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
>
> 367.
>
> 368. // 当前约束任务已完成，预积分约束复位，同时需要设置一下零偏作为下一次积分的先诀条件
>
> 369. imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
>
> 370. // check optimization
>
> 371. // 一个简单的失败检测
>
> 372. if (failureDetection(prevVel_, prevBias_))
>
> 373. {
>
> 374. // 状态异常直接复位
>
> 375. resetParams();
>
> 376. return;
>
> 377. }
>
> 378.
>
> 379.
>
> 380. // 2. 优化之后，根据最新的IMU状态进行传播
>
> 381. // 因为我们“同时”获得IMU和LIDAR里程计位姿，通常IMU数据会来的更早一点，LIDAR数据并不是原始数据而是一个里程计的数据（比如我们10s只能拿到第9s的odom的数据）
>
> 382. // 而IMU数据是实时得到的（系统时间100s 可以收到99s的IMU 只能收到90s的odom），然而我们需要实时性，利用余下的IMU推算位姿
>
> 383. prevStateOdom = prevState_;
>
> 384. prevBiasOdom = prevBias_;
>
> 385. // first pop imu message older than current correction data
>
> 386. double lastImuQT = -1;
>
> 387.
>
> 388. // 首先把LIDAR帧之前的IMU数据全部弹出去（和之前的IMU队列不是一个，需要注意）
>
> 389. while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
>
> 390. {
>
> 391. lastImuQT = ROS_TIME(&imuQueImu.front());
>
> 392. imuQueImu.pop_front();
>
> 393. }
>
> 394.
>
> 395. // 如果有新于LIAR状态的IMU数据
>
> 396. if (!imuQueImu.empty())
>
> 397. {
>
> 398. // 将这个预积分变量复位
>
> 399. imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
>
> 400.
>
> 401. for (int i = 0; i < (int)imuQueImu.size(); ++i)
>
> 402. {
>
> 403. sensor_msgs::Imu *thisImu = &imuQueImu[i];
>
> 404. double imuTime = ROS_TIME(thisImu);
>
> 405. double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);
>
> 406.
>
> 407. imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
>
> 408. gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
>
> 409. lastImuQT = imuTime;
>
> 410. }
>
> 411. }
>
> 412.
>
> 413. // 每做一次优化 ++key 到key==100 执行前段代码
>
> 414. ++key;
>
> 415. doneFirstOpt = true;
>
> 416. }
>
> 417.
>
> 418. bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
>
> 419. {
>
> 420. Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
>
> 421. // 当前速度超过30m/s 108km/h认为是异常状态
>
> 422. if (vel.norm() > 30)
>
> 423. {
>
> 424. ROS_WARN("Large velocity, reset IMU-preintegration!");
>
> 425. return true;
>
> 426. }
>
> 427.
>
> 428. Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
>
> 429. Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
>
> 430. // 零偏太大，那也不太正常
>
> 431. if (ba.norm() > 1.0 || bg.norm() > 1.0)
>
> 432. {
>
> 433. ROS_WARN("Large bias, reset IMU-preintegration!");
>
> 434. return true;
>
> 435. }
>
> 436.
>
> 437. return false;
>
> 438. }
>
> 439.
>
> 440. // 输入参数为IMU的原始数据
>
> 441. void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
>
> 442. {
>
> 443. std::lock_guard<std::mutex> lock(mtx);
>
> 444.
>
> 445. // 把IMU数据做一个简单的转换
>
> 446. sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
>
> 447.
>
> 448. // 用来优化
>
> 449. imuQueOpt.push_back(thisImu);
>
> 450. // 用来存储IMU状态
>
> 451. imuQueImu.push_back(thisImu);
>
> 452.
>
> 453. // 在里程计回调函数中置true
>
> 454. if (doneFirstOpt == false)
>
> 455. return;
>
> 456.
>
> 457. 如果执行过优化执行后面代码
>
> 458. double imuTime = ROS_TIME(&thisImu);
>
> 459. double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
>
> 460. lastImuT_imu = imuTime;
>
> 461.
>
> 462. // integrate this single imu message
>
> 463. // 这块结合代码是515行之后 加入预积分状态中
>
> 464. // 更新预积分状态是指根据新的IMU测量值，将其积分到先前的状态中，以获得更新后的姿态和速度信息。
>
> 465. // 在IMU数据预积分中，连续的IMU测量值（包括线性加速度和角速度）被积分成姿态和速度的增量。这些增量代表了从先前状态到当前状态的运动变化。
>
> 466. // 当接收到新的IMU测量值时，通过调用`integrateMeasurement()`函数将这些测量值积分到先前的状态中。积分过程会考虑时间间隔、加速度、角速度以及先前的状态信息，从而更新预积分状态。
>
> 467. // 通过更新预积分状态，我们可以获得更准确的姿态和速度信息，这对于后续的SLAM算法或运动估计非常重要。预积分状态的更新可以在每次接收到新的IMU测量值时进行，以保持状态的准确性和连续性。
>
> 468. imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
>
> 469. gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);
>
> 470.
>
> 471.
>
> 472. //在这段代码中，首先创建了一个名为`odometry`的`nav_msgs::Odometry`变量，并对其进行了一些设置。
>
> 473. //
>
> 474. //- `odometry.header.stamp`被设置为当前IMU消息的时间戳(`thisImu.header.stamp`)，表示该Odometry消息的时间。
>
> 475. //
>
> 476. //- `odometry.header.frame_id`被设置为`odometryFrame`，表示Odometry消息所属的坐标系。
>
> 477. //
>
> 478. //- `odometry.child_frame_id`被设置为"odom_imu"，表示Odometry消息的子坐标系。
>
> 479. //
>
> 480. // 接下来，通过使用当前的IMU状态(`currentState`)和IMU到Lidar的变换(`imu2Lidar`)，将IMU的姿态转换为Lidar的姿态。
>
> 481. //
>
> 482. // 具体而言，将当前的IMU姿态(`currentState.quaternion()`和`currentState.position()`)与IMU到Lidar的变换进行组合，得到Lidar的姿态(`lidarPose`)。
>
> 483. //
>
> 484. // 然后，将Lidar姿态的位置和方向信息分别赋给`odometry.pose.pose.position`和`odometry.pose.pose.orientation`，用于表示Lidar的位姿。
>
> 485. //
>
> 486. // 同时，将当前的IMU速度(`currentState.velocity()`)赋给`odometry.twist.twist.linear`，表示Lidar的线速度。
>
> 487. //
>
> 488. // 最后，将当前IMU的角速度与之前的偏置进行相加，并赋给`odometry.twist.twist.angular`，表示Lidar的角速度。
>
> 489. //
>
> 490. // 最后，通过`pubImuOdometry`发布`odometry`消息，将IMU的位姿和速度信息转化为Odometry消息，并以IMU的频率发布。
>
> 491. // predict odometry
>
> 492. // 预测
>
> 493. gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
>
> 494.
>
> 495. nav_msgs::Odometry odometry;
>
> 496. odometry.header.stamp = thisImu.header.stamp; // 设置Odometry消息的时间戳为当前IMU消息的时间戳
>
> 497. odometry.header.frame_id = odometryFrame; // 设置Odometry消息的坐标系
>
> 498. odometry.child_frame_id = "odom_imu"; // 设置Odometry消息的子坐标系
>
> 499.
>
> 500. // 将IMU的姿态转换为Lidar的姿态
>
> 501. gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position()); // 当前IMU的姿态
>
> 502. gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar); // Lidar的姿态
>
> 503.
>
> 504. // 设置Odometry消息的位姿信息
>
> 505. odometry.pose.pose.position.x = lidarPose.translation().x();
>
> 506. odometry.pose.pose.position.y = lidarPose.translation().y();
>
> 507. odometry.pose.pose.position.z = lidarPose.translation().z();
>
> 508. odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
>
> 509. odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
>
> 510. odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
>
> 511. odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
>
> 512.
>
> 513. // 设置Odometry消息的速度信息
>
> 514. odometry.twist.twist.linear.x = currentState.velocity().x();
>
> 515. odometry.twist.twist.linear.y = currentState.velocity().y();
>
> 516. odometry.twist.twist.linear.z = currentState.velocity().z();
>
> 517.
>
> 518. // 添加角速度偏置修正
>
> 519. odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
>
> 520. odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
>
> 521. odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
>
> 522.
>
> 523. pubImuOdometry.publish(odometry); // 发布Odometry消息，将IMU的位姿和速度信息转化为Odometry消息，并以IMU的频率发布
>
> 524. }
>
> 525. };
>
>
>
>
> 代码解读2.2 分块讲解--构造函数讲解
2.2.1 以IMU信息怎么来的为例介绍ROS通信机制
我们先看rosbag包里面的IMU话题是什么：可以看到，叫IMU_raw
我们在module_navsat.launch中将IMU信息映射为imu_correct了。
> <remap from="imu/data" to="imu_correct" />
>
>
> 代码解读在参数服务器中：
> 1. nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
>
> 2. nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");
>
> 3. nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu");
>
> 4. nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps");
>
>
>
>
> 代码解读这段代码使用了ROS参数服务器来获取参数值。在ROS中，参数可以在参数服务器上进行设置，然后在程序中使用
ros::NodeHandle::param方法来获取参数值。具体来说，这段代码通过
ros::NodeHandle对象nh的param方法来获取参数值，并将其存储在相应的变量中。以下是对每个参数的解释：-
lio_sam/pointCloudTopic: 该参数用于指定点云数据的话题名称，默认值为"points_raw"。如果在参数服务器上没有设置该参数，或者无法获取到参数值，将使用默认值。-
lio_sam/imuTopic: 该参数用于指定IMU数据的话题名称，默认值为"imu_correct"。如果在参数服务器上没有设置该参数，或者无法获取到参数值，将使用默认值。-
lio_sam/odomTopic: 该参数用于指定里程计数据的话题名称，默认值为"odometry/imu"。如果在参数服务器上没有设置该参数，或者无法获取到参数值，将使用默认值。-
lio_sam/gpsTopic: 该参数用于指定GPS数据的话题名称，默认值为"odometry/gps"。如果在参数服务器上没有设置该参数，或者无法获取到参数值，将使用默认值。在实际使用中，可以在ROS启动文件（launch文件）中设置这些参数，或者在终端中通过
rosparam set命令来设置参数值。这样，程序在运行时就可以根据参数服务器上的设置来获取相应的话题名称。我们将数据成功读入了。
2.2.2 构造函数讲解
> 1. IMUPreintegration()
>
> 2. {
>
> 3. // 订阅 IMU 话题，缓冲区大小为 2000，回调函数为 imuHandler()
>
> 4. subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
>
> 5.
>
> 6. // 订阅 Odometry 话题，缓冲区大小为 5，回调函数为 odometryHandler() lio_sam/mapping/odometry_incremental这个是后端优化节点发过来的关于LIDAR里程计的信息
>
> 7. subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
>
> 8.
>
> 9. // 声明发布 Odometry 话题，话题名称为 odomTopic+"_incremental"，队列大小为 2000
>
> 10. pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic+"_incremental", 2000);
>
> 11.
>
> 12. // 创建 gtsam::PreintegrationParams 对象 p，设置重力为 imuGravity
>
> 13. boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
>
> 14.
>
> 15. // 加速度积分噪声
>
> 16. p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
>
> 17. // 角速度积分噪声
>
> 18. p->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
>
> 19. // 速度积分得到位置的噪声
>
> 20. p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
>
> 21. gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
>
> 22.
>
> 23. // 初始位姿置信度设置比较高
>
> 24. priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
>
> 25. // 初始速度置信度设置差一些
>
> 26. priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
>
> 27. // 零偏的置信度设置高一些
>
> 28. priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
>
> 29. // 正常情况下LIDAR odom的协方差矩阵
>
> 30. correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
>
> 31. // lidar odom退化之后的协方差矩阵
>
> 32. correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
>
> 33. // 两帧bias的协方差矩阵
>
> 34. noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
>
> 35.
>
> 36. // 这里两个预积分量一个是用来做预积分优化，一个推算最新位姿的
>
> 37. imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
>
> 38. imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
>
> 39. }
>
>
>
>
> 代码解读2.2.3 收到IMU信息的回调函数
> 1. // 输入参数为IMU的原始数据
>
> 2. void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
>
> 3. {
>
> 4. std::lock_guard<std::mutex> lock(mtx);
>
> 5.
>
> 6. // 把IMU数据做一个简单的转换
>
> 7. sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
>
> 8.
>
> 9. // 用来优化
>
> 10. imuQueOpt.push_back(thisImu);
>
> 11. // 用来存储IMU状态
>
> 12. imuQueImu.push_back(thisImu);
>
> 13.
>
> 14. // 在里程计回调函数中置true
>
> 15. if (doneFirstOpt == false)
>
> 16. return;
>
> 17.
>
> 18. 如果执行过优化执行后面代码
>
> 19. double imuTime = ROS_TIME(&thisImu);
>
> 20. double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
>
> 21. lastImuT_imu = imuTime;
>
> 22.
>
> 23. // integrate this single imu message
>
> 24. // 这块结合代码是515行之后 加入预积分状态中
>
> 25. // 更新预积分状态是指根据新的IMU测量值，将其积分到先前的状态中，以获得更新后的姿态和速度信息。
>
> 26. // 在IMU数据预积分中，连续的IMU测量值（包括线性加速度和角速度）被积分成姿态和速度的增量。这些增量代表了从先前状态到当前状态的运动变化。
>
> 27. // 当接收到新的IMU测量值时，通过调用`integrateMeasurement()`函数将这些测量值积分到先前的状态中。积分过程会考虑时间间隔、加速度、角速度以及先前的状态信息，从而更新预积分状态。
>
> 28. // 通过更新预积分状态，我们可以获得更准确的姿态和速度信息，这对于后续的SLAM算法或运动估计非常重要。预积分状态的更新可以在每次接收到新的IMU测量值时进行，以保持状态的准确性和连续性。
>
> 29. imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
>
> 30. gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);
>
> 31.
>
> 32.
>
> 33. //在这段代码中，首先创建了一个名为`odometry`的`nav_msgs::Odometry`变量，并对其进行了一些设置。
>
> 34. //
>
> 35. //- `odometry.header.stamp`被设置为当前IMU消息的时间戳(`thisImu.header.stamp`)，表示该Odometry消息的时间。
>
> 36. //
>
> 37. //- `odometry.header.frame_id`被设置为`odometryFrame`，表示Odometry消息所属的坐标系。
>
> 38. //
>
> 39. //- `odometry.child_frame_id`被设置为"odom_imu"，表示Odometry消息的子坐标系。
>
> 40. //
>
> 41. // 接下来，通过使用当前的IMU状态(`currentState`)和IMU到Lidar的变换(`imu2Lidar`)，将IMU的姿态转换为Lidar的姿态。
>
> 42. //
>
> 43. // 具体而言，将当前的IMU姿态(`currentState.quaternion()`和`currentState.position()`)与IMU到Lidar的变换进行组合，得到Lidar的姿态(`lidarPose`)。
>
> 44. //
>
> 45. // 然后，将Lidar姿态的位置和方向信息分别赋给`odometry.pose.pose.position`和`odometry.pose.pose.orientation`，用于表示Lidar的位姿。
>
> 46. //
>
> 47. // 同时，将当前的IMU速度(`currentState.velocity()`)赋给`odometry.twist.twist.linear`，表示Lidar的线速度。
>
> 48. //
>
> 49. // 最后，将当前IMU的角速度与之前的偏置进行相加，并赋给`odometry.twist.twist.angular`，表示Lidar的角速度。
>
> 50. //
>
> 51. // 最后，通过`pubImuOdometry`发布`odometry`消息，将IMU的位姿和速度信息转化为Odometry消息，并以IMU的频率发布。
>
> 52. // predict odometry
>
> 53. // 预测
>
> 54. gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
>
> 55.
>
> 56. nav_msgs::Odometry odometry;
>
> 57. odometry.header.stamp = thisImu.header.stamp; // 设置Odometry消息的时间戳为当前IMU消息的时间戳
>
> 58. odometry.header.frame_id = odometryFrame; // 设置Odometry消息的坐标系
>
> 59. odometry.child_frame_id = "odom_imu"; // 设置Odometry消息的子坐标系
>
> 60.
>
> 61. // 将IMU的姿态转换为Lidar的姿态
>
> 62. gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position()); // 当前IMU的姿态
>
> 63. gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar); // Lidar的姿态
>
> 64.
>
> 65. // 设置Odometry消息的位姿信息
>
> 66. odometry.pose.pose.position.x = lidarPose.translation().x();
>
> 67. odometry.pose.pose.position.y = lidarPose.translation().y();
>
> 68. odometry.pose.pose.position.z = lidarPose.translation().z();
>
> 69. odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
>
> 70. odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
>
> 71. odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
>
> 72. odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
>
> 73.
>
> 74. // 设置Odometry消息的速度信息
>
> 75. odometry.twist.twist.linear.x = currentState.velocity().x();
>
> 76. odometry.twist.twist.linear.y = currentState.velocity().y();
>
> 77. odometry.twist.twist.linear.z = currentState.velocity().z();
>
> 78.
>
> 79. // 添加角速度偏置修正
>
> 80. odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
>
> 81. odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
>
> 82. odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
>
> 83.
>
> 84. pubImuOdometry.publish(odometry); // 发布Odometry消息，将IMU的位姿和速度信息转化为Odometry消息，并以IMU的频率发布
>
> 85. }
>
>
>
>
> 代码解读先来看gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;变量：
imuIntegratorImu_是一个指向gtsam::PreintegratedImuMeasurements对象的指针。
gtsam::PreintegratedImuMeasurements是GTSAM（Geometric Tracking and Mapping）库中的一个类，用于对IMU数据进行预积分（即将连续的IMU测量值转化为姿态和速度的增量）。该类提供了用于预积分的函数和方法，可以方便地处理IMU数据并生成相应的姿态和速度增量。在给定IMU测量值和时间间隔的情况下，
imuIntegratorImu_指针可以调用integrateMeasurement()函数来更新预积分状态，以及使用predict()函数来进行状态预测。通过使用
imuIntegratorImu_对象，可以方便地进行IMU数据的处理、积分和状态预测，从而为后续的SLAM（Simultaneous Localization and Mapping）算法提供准确的姿态和速度信息。更新预积分状态是指根据新的IMU测量值，将其积分到先前的状态中，以获得更新后的姿态和速度信息。
在IMU数据预积分中，连续的IMU测量值（包括线性加速度和角速度）被积分成姿态和速度的增量。这些增量代表了从先前状态到当前状态的运动变化。
当接收到新的IMU测量值时，通过调用
integrateMeasurement()函数将这些测量值积分到先前的状态中。积分过程会考虑时间间隔、加速度、角速度以及先前的状态信息，从而更新预积分状态。通过更新预积分状态，我们可以获得更准确的姿态和速度信息，这对于后续的SLAM算法或运动估计非常重要。预积分状态的更新可以在每次接收到新的IMU测量值时进行，以保持状态的准确性和连续性。
**prevStateOdom**和**prevBiasOdom**是用于保存上一个时刻的姿态状态和IMU偏差的变量。-
**prevStateOdom**是上一个时刻的导航状态（NavState），包括姿态和速度信息。它用于在预测过程中作为初始状态，根据当前的IMU测量值更新姿态和速度。-
**prevBiasOdom**是上一个时刻的IMU偏差（ConstantBias），包括加速度计和陀螺仪的零偏。它用于在预测过程中作为初始偏差，根据当前的IMU测量值更新偏差。这些变量的目的是跟踪先前时刻的状态和偏差，并在接收到新的IMU测量值时使用它们来进行预测和更新。通过维护先前的状态和偏差信息，可以实现连续的状态估计和更准确的运动预测。
我们说一下流程：
1.将IMU信息做一个简单的变换放在队列imuQueOpt、imuQueImu 中供后文操作。
2.如果里程计发来了数据doneFirstOpt == true，则往下进行，否则直接返回。
3.计算两个IMU之间的时间戳，更新预积分的状态。并根据prevStateOdom,、prevBiasOdom更新当前的IMU的位姿。
4.用话题pubImuOdometry发布当前LIDAR的位姿，线速度和角速度。
2.2.4 收到IMU信息的回调函数
> 1. void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
>
> 2. {
>
> 3. std::lock_guard<std::mutex> lock(mtx);
>
> 4.
>
> 5. // ROS_TIME函数用于获取当前odomMsg的时间戳的值
>
> 6. double currentCorrectionTime = ROS_TIME(odomMsg);
>
> 7.
>
> 8. // make sure we have imu data to integrate
>
> 9. // 判断IMU队列是否有数据
>
> 10. if (imuQueOpt.empty())
>
> 11. return;
>
> 12.
>
> 13. // 获取里程计 平移 XYZ 旋转xyzw
>
> 14. float p_x = odomMsg->pose.pose.position.x;
>
> 15. float p_y = odomMsg->pose.pose.position.y;
>
> 16. float p_z = odomMsg->pose.pose.position.z;
>
> 17. float r_x = odomMsg->pose.pose.orientation.x;
>
> 18. float r_y = odomMsg->pose.pose.orientation.y;
>
> 19. float r_z = odomMsg->pose.pose.orientation.z;
>
> 20. float r_w = odomMsg->pose.pose.orientation.w;
>
> 21.
>
> 22. // 有退化风险（里程计的精度有风险下降）
>
> 23. bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
>
> 24.
>
> 25. // 位姿转化成gtsam格式（旋转 + 平移）
>
> 26. gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
>
> 27.
>
> 28.
>
> 29. // 0. 初始化
>
> 30. if (systemInitialized == false)
>
> 31. {
>
> 32. // 将优化问题复位
>
> 33. resetOptimization();
>
> 34.
>
> 35. // 把IMU数据扔掉
>
> 36. while (!imuQueOpt.empty())
>
> 37. {
>
> 38. // 如果IMU数据的时间戳 < 当前里程计的时间戳
>
> 39. if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
>
> 40. {
>
> 41. // 被扔掉IMU数据的时间戳，这个变量最终保存刚好小于当前里程计时间戳的帧
>
> 42. lastImuT_opt = ROS_TIME(&imuQueOpt.front());
>
> 43. imuQueOpt.pop_front();
>
> 44. }
>
> 45. else
>
> 46. break;
>
> 47. }
>
> 48.
>
> 49. 添加约束
>
> 50.
>
> 51. // 将LIDAR的位姿转换到IMU坐标系下
>
> 52. prevPose_ = lidarPose.compose(lidar2Imu);
>
> 53. gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
>
> 54. graphFactors.add(priorPose);
>
> 55.
>
> 56. // gtsam::Vector3表示三自由度速度，由于刚开始确实不知道是多少，直接置为0了
>
> 57. prevVel_ = gtsam::Vector3(0, 0, 0);
>
> 58. // 设置其初始值和置信度
>
> 59. gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
>
> 60. // 约束加入到因子中
>
> 61. graphFactors.add(priorVel);
>
> 62.
>
> 63.
>
> 64.
>
> 65. // gtsam::imuBias::ConstantBias表示IMU零偏 X(Pose3 xyzrpy) Y(Vel xyz) B(BIAS axayazgxgygz)
>
> 66. prevBias_ = gtsam::imuBias::ConstantBias();
>
> 67. // 这段代码是使用GTSAM库中的PriorFactor类创建一个先验因子（PriorFactor），用于对IMU偏差（imuBias::ConstantBias）进行约束。
>
> 68. //具体解释如下：
>
> 69. // gtsam::PriorFactor<gtsam::imuBias::ConstantBias>：这是一个GTSAM库中的PriorFactor类的模板实例化，指定了模板参数为gtsam::imuBias::ConstantBias，即IMU偏差的类型。
>
> 70. // priorBias：这是创建的先验因子对象的名称。
>
> 71. // B(0)：这是一个表示IMU偏差的变量，其中(0)是索引号，用于区分多个偏差变量。这里使用索引号为0的偏差变量。
>
> 72. // prevBias_：这是一个先前的IMU偏差的变量，它被用作先验因子的先验值。
>
> 73. // priorBiasNoise：这是一个表示偏差噪声的对象，用于指定对IMU偏差的不确定性。
>
> 74. // 综合起来，这段代码创建了一个先验因子，它约束了IMU偏差变量B(0)的值必须接近于先前的偏差值prevBias_，并且在计算图优化过程中使用了指定的偏差噪声来表示对这个约束的不确定性。
>
> 75. gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
>
> 76. // 将约束添加到因子中
>
> 77. graphFactors.add(priorBias);
>
> 78.
>
> 79.
>
> 80. 添加状态
>
> 81.
>
> 82. // 添加状态量
>
> 83. graphValues.insert(X(0), prevPose_);
>
> 84. graphValues.insert(V(0), prevVel_);
>
> 85. graphValues.insert(B(0), prevBias_);
>
> 86.
>
> 87. 约束graphFactors + 状态 graphValues
>
> 88. // 约束和状态添加完了，开始优化
>
> 89. optimizer.update(graphFactors, graphValues);
>
> 90. // 清空变量 方便下一次进行优化
>
> 91. graphFactors.resize(0);
>
> 92. graphValues.clear();
>
> 93.
>
> 94. // 预积分的接口，使用初始零值进行初始化
>
> 95. imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
>
> 96. imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
>
> 97.
>
> 98. key = 1;
>
> 99. // 初始化标志位置为true
>
> 100. systemInitialized = true;
>
> 101. return;
>
> 102. }
>
> 103.
>
> 104.
>
> 105. // reset graph for speed
>
> 106. // 执行100次 当isam优化器中加入较多的约束后，为了避免运算时间变长，直接清空 初始化因子图优化问题
>
> 107. if (key == 100)
>
> 108. {
>
> 109. // get updated noise before reset
>
> 110. // 取出最新时刻位姿 速度 零偏的协方差矩阵
>
> 111. gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
>
> 112. gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
>
> 113. gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
>
> 114. // reset graph
>
> 115. // 复位整个优化问题
>
> 116. resetOptimization();
>
> 117. // add pose
>
> 118. gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
>
> 119. graphFactors.add(priorPose);
>
> 120. // add velocity
>
> 121. gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
>
> 122. graphFactors.add(priorVel);
>
> 123. // add bias
>
> 124. gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
>
> 125. graphFactors.add(priorBias);
>
> 126. // add values
>
> 127. graphValues.insert(X(0), prevPose_);
>
> 128. graphValues.insert(V(0), prevVel_);
>
> 129. graphValues.insert(B(0), prevBias_);
>
> 130. // optimize once
>
> 131. optimizer.update(graphFactors, graphValues);
>
> 132. graphFactors.resize(0);
>
> 133. graphValues.clear();
>
> 134.
>
> 135. key = 1;
>
> 136. }
>
> 137.
>
> 138.
>
> 139. // 1. 初始化结束后，我们已经有一个LIDAR坐标系转换到IMU坐标系下的一个位姿，下面开始形成预积分的约束
>
> 140. while (!imuQueOpt.empty())
>
> 141. {
>
> 142. // pop and integrate imu data that is between two optimizations
>
> 143. sensor_msgs::Imu *thisImu = &imuQueOpt.front();
>
> 144. double imuTime = ROS_TIME(thisImu);
>
> 145. // IMU的时间戳要小于LIDAR的时间戳
>
> 146. if (imuTime < currentCorrectionTime - delta_t)
>
> 147. {
>
> 148. // 计算两个IMU之间的时间戳
>
> 149. double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
>
> 150. // 调用预积分接口将IMU数据送进去处理
>
> 151. imuIntegratorOpt_->integrateMeasurement(
>
> 152. gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
>
> 153. gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
>
> 154.
>
> 155. lastImuT_opt = imuTime;
>
> 156. imuQueOpt.pop_front();
>
> 157. }
>
> 158. else
>
> 159. break;
>
> 160. }
>
> 161.
>
> 162. // 两帧间IMU预积分完成(LIDAR两帧，IMU很多帧)后，将其转换成预积分约束
>
> 163. const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
>
> 164.
>
> 165. // 预积分的约束对相邻两帧之间的位姿 速度 零偏形成约束
>
> 166. // 上一帧的位姿、速度 当前帧的位姿、速度 上一帧的零偏
>
> 167. gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
>
> 168. // 约束加入因子图
>
> 169. graphFactors.add(imu_factor);
>
> 170.
>
> 171. // 这段代码是使用GTSAM库中的BetweenFactor类创建一个因子（BetweenFactor），用于表示两个IMU偏差变量之间的关系。
>
> 172. // graphFactors：这是一个图（graph）对象，用于存储因子（factor）。
>
> 173. // gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>：这是一个GTSAM库中的BetweenFactor类的模板实例化，指定了模板参数为gtsam::imuBias::ConstantBias，即IMU偏差的类型。
>
> 174. // B(key - 1)：这是表示先前的IMU偏差变量的变量，其中key - 1是索引号，用于区分多个偏差变量。这里使用索引号为key - 1的偏差变量。
>
> 175. // B(key)：这是表示当前的IMU偏差变量的变量，其中key是索引号，用于区分多个偏差变量。这里使用索引号为key的偏差变量。
>
> 176. // gtsam::imuBias::ConstantBias()：这是一个默认构造的IMU偏差对象，作为两个偏差变量之间的初始估计。
>
> 177. // gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)：这是一个表示噪声模型的对象，用于表示两个偏差变量之间的不确定性。imuIntegratorOpt_->deltaTij()表示IMU积分器在两个时间步之间的时间差，noiseModelBetweenBias是一个噪声模型，通过对两个时间步之间的时间差进行标准化来定义偏差之间的不确定性。
>
> 178. // 综合起来，这段代码创建了一个因子，它表示了两个IMU偏差变量之间的关系，并使用指定的噪声模型来表示这个关系的不确定性。然后，该因子被添加到一个图对象中，以便在后续的图优化过程中使用。
>
> 179. graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
>
> 180. gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
>
> 181.
>
> 182. // 这段代码将激光雷达位姿(lidarPose)与激光雷达坐标系到IMU坐标系之间的变换关系(lidar2Imu)进行组合操作，得到了当前的位姿(curPose)。
>
> 183. // 这通常用于将激光雷达位姿转换到IMU坐标系下的位姿
>
> 184. gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
>
> 185. // LIDAR位姿补偿到IMU坐标系下同时根据是否退化选择不同的置信度作为这一帧的先验估计
>
> 186. gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
>
> 187. // 加入到因子图
>
> 188. graphFactors.add(pose_factor);
>
> 189.
>
> 190. // 根据上一时刻的状态 结合预积分的结果 对当前状态进行预测 prevState_上一关键帧的位姿 + prevBias_上一关键帧的零偏
>
> 191. // 这段代码使用GTSAM库中的ImuIntegrator类的predict()函数，基于先前的状态和偏差预测生成一个新的导航状态。
>
> 192. // gtsam::NavState propState_：这是一个NavState类型的对象，用于存储生成的预测位姿
>
> 193. // imuIntegratorOpt_：这是一个ImuIntegrator类的指针或引用，用于执行IMU积分和状态预测操作。
>
> 194. // prevState_：这是一个先前的导航状态对象，作为IMU积分的起始状态。
>
> 195. // prevBias_：这是一个先前的IMU偏差对象，用作IMU积分的起始偏差。
>
> 196. // predict(prevState_, prevBias_)：这是ImuIntegrator类的成员函数，根据先前的状态和偏差，通过IMU积分预测生成一个新的位姿状态。
>
> 197. // 综合起来，这段代码使用ImuIntegrator类的predict()函数，基于先前的导航状态和偏差，进行IMU积分预测，生成一个新的导航状态，并将其存储在propState_中。
>
> 198. // 这个预测的导航状态通常用于在导航和状态估计问题中进行运动预测和滤波更新。
>
> 199. gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
>
> 200.
>
> 201. // 预测量作为初始值插入到因子图中
>
> 202. graphValues.insert(X(key), propState_.pose());
>
> 203. graphValues.insert(V(key), propState_.v());
>
> 204. graphValues.insert(B(key), prevBias_);
>
> 205.
>
> 206. // 执行因子图优化
>
> 207. optimizer.update(graphFactors, graphValues);
>
> 208. optimizer.update();
>
> 209.
>
> 210. // 清空
>
> 211. graphFactors.resize(0);
>
> 212. graphValues.clear();
>
> 213.
>
> 214. // 获取优化结果
>
> 215. gtsam::Values result = optimizer.calculateEstimate();
>
> 216.
>
> 217. // 获取优化后的当前状态作为当前帧的最佳估计
>
> 218. prevPose_ = result.at<gtsam::Pose3>(X(key));
>
> 219. prevVel_ = result.at<gtsam::Vector3>(V(key));
>
> 220. prevState_ = gtsam::NavState(prevPose_, prevVel_);
>
> 221. prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
>
> 222.
>
> 223. // 当前约束任务已完成，预积分约束复位，同时需要设置一下零偏作为下一次积分的先诀条件
>
> 224. imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
>
> 225. // check optimization
>
> 226. // 一个简单的失败检测
>
> 227. if (failureDetection(prevVel_, prevBias_))
>
> 228. {
>
> 229. // 状态异常直接复位
>
> 230. resetParams();
>
> 231. return;
>
> 232. }
>
> 233.
>
> 234.
>
> 235. // 2. 优化之后，根据最新的IMU状态进行传播
>
> 236. // 因为我们“同时”获得IMU和LIDAR里程计位姿，通常IMU数据会来的更早一点，LIDAR数据并不是原始数据而是一个里程计的数据（比如我们10s只能拿到第9s的odom的数据）
>
> 237. // 而IMU数据是实时得到的（系统时间100s 可以收到99s的IMU 只能收到90s的odom），然而我们需要实时性，利用余下的IMU推算位姿
>
> 238. prevStateOdom = prevState_;
>
> 239. prevBiasOdom = prevBias_;
>
> 240. // first pop imu message older than current correction data
>
> 241. double lastImuQT = -1;
>
> 242.
>
> 243. // 首先把LIDAR帧之前的IMU数据全部弹出去（和之前的IMU队列不是一个，需要注意）
>
> 244. while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
>
> 245. {
>
> 246. lastImuQT = ROS_TIME(&imuQueImu.front());
>
> 247. imuQueImu.pop_front();
>
> 248. }
>
> 249.
>
> 250. // 如果有新于LIAR状态的IMU数据
>
> 251. if (!imuQueImu.empty())
>
> 252. {
>
> 253. // 将这个预积分变量复位
>
> 254. imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
>
> 255.
>
> 256. for (int i = 0; i < (int)imuQueImu.size(); ++i)
>
> 257. {
>
> 258. sensor_msgs::Imu *thisImu = &imuQueImu[i];
>
> 259. double imuTime = ROS_TIME(thisImu);
>
> 260. double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);
>
> 261.
>
> 262. imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
>
> 263. gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
>
> 264. lastImuQT = imuTime;
>
> 265. }
>
> 266. }
>
> 267.
>
> 268. // 每做一次优化 ++key 到key==100 执行前段代码
>
> 269. ++key;
>
> 270. doneFirstOpt = true;
>
> 271. }
>
>
>
>
> 代码解读判断IMU队列imuQueOpt 是否存在数据，并从中提取里程计的数据；随后将odom位姿转换为gtsam格式以备后续处理。
若系统尚未完成初始化（即systemInitialized == false) ，则需执行以下操作：
首先设置优化问题的基础架构（包括初始化图因子和图值），并对当前时刻之前的所有IMU数据进行筛选以去除过时信息。
随后计算初始估计值：
LIDAR位姿基于 incoming ODOM信息转换至LIDAR坐标系作为初始估计；
速度初值设为0；
同时进行IMU零偏差校准；
最后通过预积分类接口（分别对应两种传感器）进行预积分配置，并设定初始状态参数。
完成初始化后即可生成预积分约束条件。
> 1. using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
>
> 2. using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot)
>
> 3. using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
>
>
>
>
> 代码解读从imuQueOpt 中提取一帧数据（位于两个ODOM框架之间的IMU数据帧）。然后使用**imuIntegratorOpt_**执行预积分计算。
> const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
>
> 代码解读首先将两帧之间计算得到的预积分类结果存储到变量preint_imu中。
然后在因子图中增加新的约束条件：通过构造一个gtsam::ImuFactor对象来描述相邻两个状态点间的运动学参数间的相互制约关系。该因子基于前一状态点处的姿态与速度信息（X(key - 1)和V(key - 1)），以及当前时刻的状态量信息（X(key)、V(key)与前一时刻零偏估计值B(key - 1)）。同时结合预积分类结果来构建这些约束条件。
> 1. // 两帧间IMU预积分完成(LIDAR两帧，IMU很多帧)后，将其转换成预积分约束
>
> 2. const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
>
> 3.
>
> 4. // 预积分的约束对相邻两帧之间的位姿 速度 零偏形成约束
>
> 5. // 上一帧的位姿、速度 当前帧的位姿、速度 上一帧的零偏
>
> 6. gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
>
> 7. // 约束加入因子图
>
> 8. graphFactors.add(imu_factor);
>
>
>
>
> 代码解读将预积分结果设为约束初始值，并进行因子图优化。
第四步更新当前帧的最佳优化值。
> 1. prevPose_ = result.at<gtsam::Pose3>(X(key));
>
> 2. prevVel_ = result.at<gtsam::Vector3>(V(key));
>
> 3. prevState_ = gtsam::NavState(prevPose_, prevVel_);
>
> 4. prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
>
>
>
>
> 代码解读5.当前约束任务已实现,预积分约束已解除,同时需设置零偏以作为下次积分的前提条件。
> imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
>
> 代码解读- 实现故障检测：基于速度传感器和加速度计校准参数（IMU偏置），进行初步判断。一旦检测到故障就重新初始化系统状态。
> 1. bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
>
> 2. {
>
> 3. Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
>
> 4. // 当前速度超过30m/s 108km/h认为是异常状态
>
> 5. if (vel.norm() > 30)
>
> 6. {
>
> 7. ROS_WARN("Large velocity, reset IMU-preintegration!");
>
> 8. return true;
>
> 9. }
>
> 10.
>
> 11. Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
>
> 12. Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
>
> 13. // 零偏太大，那也不太正常
>
> 14. if (ba.norm() > 1.0 || bg.norm() > 1.0)
>
> 15. {
>
> 16. ROS_WARN("Large bias, reset IMU-preintegration!");
>
> 17. return true;
>
> 18. }
>
> 19.
>
> 20. return false;
>
> 21. }
>
>
>
>
> 代码解读> 1. void resetParams()
>
> 2. {
>
> 3. lastImuT_imu = -1;
>
> 4. doneFirstOpt = false;
>
> 5. systemInitialized = false;
>
> 6. }
>
>
>
>
> 代码解读7.在优化方案下,依据最新的IMU状态实现信息传播.由于我们能够"同步获取"来自IMU和LIDAR两种传感器的里程计位姿信息,其中IMU数据具有略微滞后性,而LIDAR提供的是一些基于里程计处理后的结果（例如,在10秒时只能获取到第9秒的odometry数据）.相比之下,在系统时间运行至第100秒时仅能获取最新至第99秒的状态信息,并且无法提供早期时间段的状态更新.
> 1. prevStateOdom = prevState_;
>
> 2. prevBiasOdom = prevBias_;
>
> 3. // first pop imu message older than current correction data
>
> 4. double lastImuQT = -1;
>
> 5.
>
> 6. // 首先把LIDAR帧之前的IMU数据全部弹出去（和之前的IMU队列不是一个，需要注意）
>
> 7. while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
>
> 8. {
>
> 9. lastImuQT = ROS_TIME(&imuQueImu.front());
>
> 10. imuQueImu.pop_front();
>
> 11. }
>
> 12.
>
> 13. // 如果有新于LIAR状态的IMU数据
>
> 14. if (!imuQueImu.empty())
>
> 15. {
>
> 16. // 将这个预积分变量复位
>
> 17. imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
>
> 18.
>
> 19. for (int i = 0; i < (int)imuQueImu.size(); ++i)
>
> 20. {
>
> 21. sensor_msgs::Imu *thisImu = &imuQueImu[i];
>
> 22. double imuTime = ROS_TIME(thisImu);
>
> 23. double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);
>
> 24.
>
> 25. imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
>
> 26. gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
>
> 27. lastImuQT = imuTime;
>
> 28. }
>
> 29. }
>
> 30.
>
> 31. // 每做一次优化 ++key 到key==100 执行前段代码
>
> 32. ++key;
>
> 33. doneFirstOpt = true;
>
>
>
>
> 代码解读8.经过多次测试 当isam优化器在增加诸多限制条件时 为了防止运算耗时过长 从而使得初始化因子图的优化工作得以清除。
> 1. if (key == 100)
>
> 2. {
>
> 3. // get updated noise before reset
>
> 4. // 取出最新时刻位姿 速度 零偏的协方差矩阵
>
> 5. gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
>
> 6. gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
>
> 7. gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
>
> 8. // reset graph
>
> 9. // 复位整个优化问题
>
> 10. resetOptimization();
>
> 11. // add pose
>
> 12. gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
>
> 13. graphFactors.add(priorPose);
>
> 14. // add velocity
>
> 15. gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
>
> 16. graphFactors.add(priorVel);
>
> 17. // add bias
>
> 18. gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
>
> 19. graphFactors.add(priorBias);
>
> 20. // add values
>
> 21. graphValues.insert(X(0), prevPose_);
>
> 22. graphValues.insert(V(0), prevVel_);
>
> 23. graphValues.insert(B(0), prevBias_);
>
> 24. // optimize once
>
> 25. optimizer.update(graphFactors, graphValues);
>
> 26. graphFactors.resize(0);
>
> 27. graphValues.clear();
>
> 28.
>
> 29. key = 1;
>
> 30. }
>
>
>
>
> 代码解读为了解决内存溢出问题，在当前时刻提取协方差矩阵作为下次迭代时的初始值，并将整个优化过程重置后重新初始化并重构该优化问题
程序流程图大概如下。
2.2.5 优化函数置位 resetOptimization
> 1. void resetOptimization()
>
> 2. {
>
> 3. // 创建一个用于配置优化器的参数对象
>
> 4. gtsam::ISAM2Params optParameters;
>
> 5.
>
> 6. // 设置参数对象中的属性，用于控制重线性化的阈值
>
> 7. optParameters.relinearizeThreshold = 0.1;
>
> 8.
>
> 9. // 设置参数对象中的属性，用于控制跳过重线性化的步长
>
> 10. optParameters.relinearizeSkip = 1;
>
> 11.
>
> 12. // 使用指定的参数对象创建一个ISAM2优化器
>
> 13. optimizer = gtsam::ISAM2(optParameters);
>
> 14.
>
> 15. // 创建一个新的非线性因子图对象
>
> 16. gtsam::NonlinearFactorGraph newGraphFactors;
>
> 17.
>
> 18. // 将新创建的非线性因子图对象赋值给图因子属性
>
> 19. graphFactors = newGraphFactors;
>
> 20.
>
> 21. // 创建一个新的值对象
>
> 22. gtsam::Values NewGraphValues;
>
> 23.
>
> 24. // 将新创建的值对象赋值给图值属性
>
> 25. graphValues = NewGraphValues;
>
> 26. }
>
>
>
>
> 代码解读函数resetOptimization的主要职责是重置优化器及其相关因子图状态。该过程包括多个关键步骤：首先生成一个名为optParameters的对象，并将其赋值给相应的属性；接着创建一个名为optimizer的对象，并将其配置参数应用到实际运算中；随后生成一个新的因子图对象，并将其赋值给指定属性以清除旧的数据；最后初始化一个新的值对象以替代现有数据并准备进行新一轮优化计算。
其中涉及两个重要参数：
较小的optParameters.relinearizeThreshold设置会触发更频繁的线性化操作，在非线性优化中这是一种平衡效率与准确性的重要手段。
较大的该属性设置则能够减少线性化发生的频率从而提高整体计算速度但可能导致一定的精度损失。
同样另一个关键参数是控制跳过的步长次数：
较小的值会使得算法在每一步迭代后立即执行线性化操作从而保证较高的计算精度；
较大的步长设置则能够在一定程度上减少线性化的频率进而加快收敛速度但可能会降低最终结果的质量。
因此根据具体的应用需求这两个参数的选择都应在实验基础上进行合理的调节以达到最佳的性能与准确性的结合。
3 TF类
3.1 流程图
3.2 代码详细注释
> 1. // 输出最终IMU推算位姿的结果
>
> 2. // 发布两个位姿 无回环检测 先前的单靠增量的平滑位姿 被回环检测的优化到更加正确的位姿
>
> 3. class TransformFusion : public ParamServer
>
> 4. {
>
> 5. public:
>
> 6. std::mutex mtx;
>
> 7.
>
> 8. ros::Subscriber subImuOdometry;
>
> 9. ros::Subscriber subLaserOdometry;
>
> 10.
>
> 11. ros::Publisher pubImuOdometry;
>
> 12. ros::Publisher pubImuPath;
>
> 13.
>
> 14. Eigen::Affine3f lidarOdomAffine;
>
> 15. Eigen::Affine3f imuOdomAffineFront;
>
> 16. Eigen::Affine3f imuOdomAffineBack;
>
> 17.
>
> 18. tf::TransformListener tfListener;
>
> 19. tf::StampedTransform lidar2Baselink;
>
> 20.
>
> 21. double lidarOdomTime = -1;
>
> 22. deque<nav_msgs::Odometry> imuOdomQueue;
>
> 23.
>
> 24. TransformFusion()
>
> 25. {
>
> 26. // 如果LIDAR帧和baselink帧不是同一个坐标系
>
> 27. // baselink指车体坐标系
>
> 28. if(lidarFrame != baselinkFrame)
>
> 29. {
>
> 30. try
>
> 31. {
>
> 32. // 查询一下LIDAR和baselink之间的tf变换
>
> 33. tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
>
> 34. tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
>
> 35. }
>
> 36. catch (tf::TransformException ex)
>
> 37. {
>
> 38. ROS_ERROR("%s",ex.what());
>
> 39. }
>
> 40. }
>
> 41.
>
> 42. // 订阅地图优化节点的全局位姿和预积分节点的增量位姿
>
> 43. subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
>
> 44. subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
>
> 45.
>
> 46. pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
>
> 47. pubImuPath = nh.advertise<nav_msgs::Path> ("lio_sam/imu/path", 1);
>
> 48. }
>
> 49.
>
> 50. Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
>
> 51. {
>
> 52. double x, y, z, roll, pitch, yaw;
>
> 53. x = odom.pose.pose.position.x;
>
> 54. y = odom.pose.pose.position.y;
>
> 55. z = odom.pose.pose.position.z;
>
> 56. tf::Quaternion orientation;
>
> 57. tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
>
> 58. tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
>
> 59. return pcl::getTransformation(x, y, z, roll, pitch, yaw);
>
> 60. }
>
> 61.
>
> 62. 将带回环的关键帧位姿保存下来了
>
> 63. void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
>
> 64. {
>
> 65. std::lock_guard<std::mutex> lock(mtx);
>
> 66.
>
> 67. lidarOdomAffine = odom2affine(*odomMsg);
>
> 68.
>
> 69. lidarOdomTime = odomMsg->header.stamp.toSec();
>
> 70. }
>
> 71.
>
> 72. // 最新的LIADR到IMU的增量补偿到位姿
>
> 73. void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
>
> 74. {
>
> 75. // static tf
>
> 76. static tf::TransformBroadcaster tfMap2Odom;
>
> 77. static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
>
> 78.
>
> 79. // 发送静态tf odom系和map系将他们重合
>
> 80. tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
>
> 81.
>
> 82. std::lock_guard<std::mutex> lock(mtx);
>
> 83.
>
> 84. // imu得到的里程计结果送入这个队列中
>
> 85. imuOdomQueue.push_back(*odomMsg);
>
> 86.
>
> 87. // get latest odometry (at current IMU stamp)
>
> 88. // 如果没有受到lidar位姿（带回环的）就return
>
> 89. if (lidarOdomTime == -1)
>
> 90. return;
>
> 91.
>
> 92. // 弹出LIDAR帧之前的IMU数据
>
> 93. while (!imuOdomQueue.empty())
>
> 94. {
>
> 95. if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
>
> 96. imuOdomQueue.pop_front();
>
> 97. else
>
> 98. break;
>
> 99. }
>
> 100.
>
> 101. // 将imuOdomQueue队列的第一个元素转换为Affine3f类型的变量imuOdomAffineFront
>
> 102. Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
>
> 103.
>
> 104. // 将imuOdomQueue队列的最后一个元素转换为Affine3f类型的变量imuOdomAffineBack
>
> 105. Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
>
> 106.
>
> 107. // 计算imuOdomAffineFront和imuOdomAffineBack之间的增量，并将结果保存在imuOdomAffineIncre中
>
> 108. Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
>
> 109.
>
> 110. // 将lidarOdomAffine和imuOdomAffineIncre进行组合，得到最终的imuOdomAffineLast
>
> 111. // lidarOdomAffine保存的是回环检测得到的位姿
>
> 112. Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
>
> 113.
>
> 114. float x, y, z, roll, pitch, yaw;
>
> 115. // 分离平移 + 欧拉角
>
> 116. pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
>
> 117.
>
> 118. // 发布里程计数据
>
> 119. nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
>
> 120. laserOdometry.pose.pose.position.x = x;
>
> 121. laserOdometry.pose.pose.position.y = y;
>
> 122. laserOdometry.pose.pose.position.z = z;
>
> 123. laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
>
> 124. pubImuOdometry.publish(laserOdometry);
>
> 125.
>
> 126. // 更新tf
>
> 127. static tf::TransformBroadcaster tfOdom2BaseLink;
>
> 128. tf::Transform tCur;
>
> 129. tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
>
> 130. if(lidarFrame != baselinkFrame)
>
> 131. tCur = tCur * lidar2Baselink;
>
> 132. // 更新odom到baselink的tf
>
> 133. tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
>
> 134. tfOdom2BaseLink.sendTransform(odom_2_baselink);
>
> 135.
>
> 136. // publish IMU path
>
> 137. // 发送IMU里程计轨迹
>
> 138. static nav_msgs::Path imuPath;
>
> 139. static double last_path_time = -1;
>
> 140. double imuTime = imuOdomQueue.back().header.stamp.toSec();
>
> 141. // 控制一下频率 不超过10HZ
>
> 142. if (imuTime - last_path_time > 0.1)
>
> 143. {
>
> 144. last_path_time = imuTime;
>
> 145. geometry_msgs::PoseStamped pose_stamped;
>
> 146. pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
>
> 147. pose_stamped.header.frame_id = odometryFrame;
>
> 148. // 将最新位姿送入轨迹中
>
> 149. pose_stamped.pose = laserOdometry.pose.pose;
>
> 150.
>
> 151. imuPath.poses.push_back(pose_stamped);
>
> 152. while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
>
> 153. imuPath.poses.erase(imuPath.poses.begin());
>
> 154. if (pubImuPath.getNumSubscribers() != 0)
>
> 155. {
>
> 156. imuPath.header.stamp = imuOdomQueue.back().header.stamp;
>
> 157. imuPath.header.frame_id = odometryFrame;
>
> 158. pubImuPath.publish(imuPath);
>
> 159. }
>
> 160. }
>
> 161. }
>
> 162. };
>
>
>
>
> 代码解读全部评论 (0)
还没有任何评论哟~