imuPreintegration.cpp（IMU预积分进程）
主函数
int main(int argc, char **argv)
{
ros::init(argc, argv, "roboat_loam");
IMUPreintegration ImuP;
TransformFusion TF;
ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
ros::MultiThreadedSpinner spinner(4); // 开辟四个线程以供处理四个subscriber的接收和回调函数，防止一个subscriber在运行时其他subscriber数据丢失
spinner.spin();
return 0;
}
IMU预积分类 IMUPreintegration
一、成员变量
| 数据类型 | 变量名 | 含义 | 注释 |
|---|---|---|---|
| std::mutex | mtx | 进程锁 | |
| ros::Subscriber | subImu | 订阅者 | |
| ros::Subscriber | subOdometry | 订阅者 | |
| ros::Publisher | pubImuOdometry | 发布者 | |
| bool | systemInitialized | 系统初始化标准位 | 是否进入过IMUPreintegration::odometryHandler函数，未进入过值为false |
public:
std::mutex mtx; // 进程锁
ros::Subscriber subImu; // imu信息订阅器
ros::Subscriber subOdometry; // 最终优化后的里程计增量信息(用来矫正imu的偏置)
ros::Publisher pubImuOdometry; // 估计的imu里程计信息发布器(其实是通过imu估计的雷达里程计信息)
bool systemInitialized = false; // 系统初始化标志位（判断是否是开机或重置后第一次进入雷达订阅者回调函数）
gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise; // 先验位置噪声
gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise; // 先验速度噪声
gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise; // 先验偏置噪声
gtsam::noiseModel::Diagonal::shared_ptr correctionNoise; // 非退化先验位姿噪声
gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2; // 退化先验位姿噪声
gtsam::Vector noiseModelBetweenBias; // 帧间偏置变化噪声
gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_; // 优化预积分器（雷达帧数据处理使用）
gtsam::PreintegratedImuMeasurements *imuIntegratorImu_; // 信息处理预积分器（IMU帧数据处理使用）
std::deque<sensor_msgs::Imu> imuQueOpt; // 优化IMU数据集队列
std::deque<sensor_msgs::Imu> imuQueImu; // 信息处理IMU数据集队列
gtsam::Pose3 prevPose_; // 上一帧估计imu的位姿信息
gtsam::Vector3 prevVel_; // 上一帧估计imu的速度信息
gtsam::NavState prevState_; // 上一帧估计的IMU状态信息（包括速度和位姿）
gtsam::imuBias::ConstantBias prevBias_; // 上一帧估计的IMU偏置信息
gtsam::NavState prevStateOdom; // 和prevState_一样
gtsam::imuBias::ConstantBias prevBiasOdom; // 和prevBias_一样
bool doneFirstOpt = false; // 是否进行过首次优化标志位
double lastImuT_imu = -1; // 上一帧IMU数据的时间（-1代表为首帧IMU数据，没有上一帧以供求差）
double lastImuT_opt = -1; // 上一帧IMU数据的时间（-1代表为首帧IMU数据，没有上一帧以供求差）
gtsam::ISAM2 optimizer; // isam2优化器（可以理解为因子图）
gtsam::NonlinearFactorGraph graphFactors; // 因子集合
gtsam::Values graphValues; // 变量集合
const double delta_t = 0; // 判断雷达帧时间所用的偏置，可认为雷达帧实际时间为 获取时间 - delta_t
int key = 1; // 迭代次数（进入雷达数据处理回调函数的次数，达到100时启动重置）
gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z())); // 从IMU到雷达的坐标变换
gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z())); // 从雷达到IMU的坐标变换
二、构造函数 IMUPreintegration()
初始化的订阅者和发布者
| 订阅者（s）or发布者（p） | 作用 | topic | 数据类型 | 回调函数 |
|---|---|---|---|---|
| subImu （s） | 订阅IMU传感器信息 | imuTopic | sensor_msgs::Imu | IMUPreintegration::imuHandler |
| subOdometry（s） | 订阅后端优化节点发布的，无回环修正的增量式lidar里程计位姿的信息 | “lio_sam/mapping/odometry_incremental” | nav_msgs::Odometry | IMUPreintegration::odometryHandler |
| pubImuOdometry （p） | 发布IMU预计分最新位姿信息 | odomTopic + “_incremental” | nav_msgs::Odometry | 无 |
定义IMU传感器信息
初始化IMU的重力加速度、加速度计和陀螺仪的观测噪声协方差矩阵、描述积分不确定性的噪声协方差矩阵，储存在预计分参数p中；将IMU偏置全部初始化为0，用于后续进行IMU积分器的定义。
初始化位姿先验噪声、速度先验噪声、偏置先验噪声、两个数值不同的校正噪声和帧间偏置噪声。
再用刚刚定义的p和IMU零偏定义两个IMU预计分器，分别用于IMU信息处理线程和优化线程。
IMUPreintegration()
{
// 订阅IMU传感器信息
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay()); // ros::TransportHints().tcpNoDelay() 指定网络传输提示???
// 订阅后端优化节点发布的 无回环修正的增量式lidar里程计位姿的信息
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
// 发布IMU预计分最新位姿信息
pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic + "_incremental", 2000);
// 定义进行imu积分的imu传感器信息
boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity); // 配置重力加速度
p->accelerometerCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuAccNoise, 2); // 加速度计的观测白噪声
p->gyroscopeCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuGyrNoise, 2); // 陀螺仪的观测白噪声
p->integrationCovariance = gtsam::Matrix33::Identity(3, 3) * pow(1e-4, 2); // 通过速度积分位置信息引入的噪声（描述积分不确定性的连续时间协方差）
gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()); // 初始化imu偏置信息
priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // 初始化位姿的噪声
priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // 初始化速度的噪声
priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 初始化偏置的噪声
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
correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
// 根据上面的参数，定义两个imu预积分器，一个用于imu信息处理线程，一个用于优化线程
imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
}
三、void resetOptimization()
配置变量重新线性化阈值和重新线性化频率参数，并根据配置参数重置ISAM2优化器，通过重新定义新的因子图和变量集合重置非线性因子图和变量。
void resetOptimization()
{
// 重置isam2优化器
gtsam::ISAM2Params optParameters;
optParameters.relinearizeThreshold = 0.1; // 规定对变量进行重新线性化的阈值
optParameters.relinearizeSkip = 1; // 决定调用几次ISAM2::update才考虑对变量进行重新线性化
optimizer = gtsam::ISAM2(optParameters);
// 重置初始化非线性因子图
gtsam::NonlinearFactorGraph newGraphFactors;
graphFactors = newGraphFactors;
gtsam::Values NewGraphValues;
graphValues = NewGraphValues;
}
四、void resetParams()
重置相关标志位。
void resetParams()
{
lastImuT_imu = -1;
doneFirstOpt = false;
systemInitialized = false;
}
五、void odometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg)
雷达里程计订阅者回调函数，订阅后端优化节点发布的无回环修正的增量式lidar里程计位姿的信息。
定义自解锁对互斥锁进行上锁
保护公共内存。
获取雷达帧的时间戳信息
通过ROS_TIME(odomMsg)将其转换为浮点型。
判断是否进行过IMU数据积分
如果已经进行过则继续执行后面的内容，如果没有进行过则直接返回退出函数。
获取里程计位姿信息
根据订阅者接收到的雷达里程计信息，获取雷达的三维坐标位置和旋转四元数。
判断位姿是否出现退化
根据接收的雷达里程计信息中的标志位判断雷达里程计优化的位姿是否出现退化。
将位姿转换为GTSAM格式
将从接收到的雷达里程计信息中的位姿信息，通过gtsam库的接口转换为gtsam的位姿格式。
判断IMU预计分器是否进行过初始化（是否为首次进入该函数）
通过系统初始化标志位systemInitialized判断是否进行过初始化。若为首次进入该函数，则需要对该函数进行初始化；如果非首次进入，则执行其他相关操作。
初始化操作（开机或重置后首次进入该函数）
通过resetOptimization()初始化isam2优化器和非线性因子图（图和变量）。丢弃优化使用的IMU信息队列中老的imu信息（将时间小于当前雷达帧时间戳的IMU信息从队列移除）。
提供IMU优化的先验因子并添加到因子图中
初始化位姿先验因子：
通过不带回环优化后雷达位姿和雷达到IMU的坐标变换，得到IMU的位姿。
根据刚刚得到的IMU位姿，和之前初始化的先验位姿噪声，初始化先验因子，并将其与第0个节点相关联，记为因子x0。
将该先验因子添加到因子集合中。
初始化速度先验因子：
将先验速度初始化为0，并设置噪声为之前定义的速度先验噪声，将其与第0个节点相关联，记为因子v0，添加到因子集合中。
初始化偏置先验因子：
将先验偏置初始化为0，并设置噪声为之前定义的偏置先验噪声，将其与第0个节点相关联，记为因子b0，添加到因子集合中。
设置变量的初始估计值
添加位姿、速度、IMU偏置变量到变量集合中，并将其值初始化为其先验值。
构建因子图
将因子和变量更新到因子图中，以构建因子图。
清空各集合并置位标志位
将因子集合和变量集合清空，以供下一次添加新的不同元素到因子图中使用。
置位系统初始化标志位systemInitialized，并将迭代次数key置1。
初始化IMU预计分器
用先验偏置重置（会将其中的预积分值清零）并初始化两个IMU预积分器。
重置操作（非首次进入该函数 且 进入次数等于阈值）
当进入函数次数key达到100次时，因子图规模太大，因子图中加入太多约束，为防止运算之间过长，进行边缘化，重置（清空）优化器和因子图。
更新噪声协方差矩阵
isam2在进行优化时会计算相关协方差矩阵，通过获取最新关键帧（第99帧）的位姿、速度、零偏的协方差矩阵，作为新的噪声协方差矩阵。
重置优化器
调用resetOptimization()来清空优化器和因子图。
根据最后一次（第99帧）优化所得的位姿、速度、偏置及其协方差矩阵，如首次进入函数时那样进行先验因子以及变量的初始化。
根据新初始化的变量集合和因子集合初始化因子图，再将各集合清空。
重置关键帧数量
令key=1将进入函数的次数重置为1。
优化操作（非首次进入该函数）
非首次进入该函数时，不论是否进行重置操作，都会进入到优化操作。
进行IMU预积分（使用优化IMU预积分信息队列）
当IMU数据非空时，逐个读取IMU数据队列（由于首次和否首次进入都会将时间戳在当前雷达帧时间戳之前的IMU数据pop出队列，因此IMU数据队列中只存在当前雷达帧和上一个雷达帧之间的IMU数据）中的元素；
如果时间在当前雷达帧之前（即第k-1帧和第k帧之间），使用ROS_TIME(thisImu)获取当前IMU帧的时间，计算与上一IMU帧之间的时间差dt，调用IMU预计分接口，使用优化使用的IMU预积分器，以每帧IMU数据的角速度、加速度和计算的时间差dt进行预积分。并设置最后一帧IMU数据帧时间为当前IMU数据帧时间，供下次计算时间差使用。将该帧IMU数据从队列中移除。
如果时间在当前帧雷达之后，则跳出IMU数据队列遍历循环，结束该次回调的IMU预积分。
将IMU因子添加到因子图并进行优化
用gtsam接口将刚刚得到的IMU预积分数据转换成添加因子图所需的数据格式（代码中好像本来就是所需数据格式）。
添加IMU预积分因子：将刚刚求得的IMU预积分数据与第k帧雷达帧的位姿速度、第k-1帧雷达帧的位姿速度、第k-1帧雷达帧的IMU零偏相关联，作为两帧之间IMU测得的相对变换，将该IMU预积分因子添加到因子图中。（定义IMU预积分因子不需要噪声协方差矩阵，因为参数IMU预积分器在定义时已经添加过相关的噪声协方差矩阵）
添加IMU偏置因子：在相邻两雷达帧之间添加IMU偏置因子，表示两个雷达帧之间IMU偏置的变化，将该因子的值设置为0，协方差矩阵以之前定义的帧间偏置协方差矩阵为基础，乘以优化预积分器的协方差矩阵开方，与第k帧和第k-1帧雷达帧的偏置相关联；直接添加到因子图中。
添加位姿先验因子：通过雷达到IMU的坐标变换，根据接收到的雷达数据计算IMU位姿，作为先验位姿值，并根据degenerate表示为判断是否发生退化来选择协方差矩阵，将该先验位姿与第k帧位姿相关联，将该因子添加到因子图中。
预测当前关键帧的信息并添加变量初值：使用优化IMU预积分器中的接口，利用上一帧的状态和偏置（定义时被初始化，在每次进入函数时会进行更新），预测该帧的状态和偏置信息；将预测得到的位姿、速度和偏置信息作为当前帧的变量初值添加到变量集合当中。
更新优化器并进行优化：将IMU预计分因子、先验因子、帧间偏置变化因子和变量添加到因子图中，构建因子图并进行优化（共两次，构建一次，调用update()一次）。
获取优化结果并更新变量值：获取当前优化结果，并将prevPose_更新为优化后的位姿、将prevVel_更新为优化后的速度，将prevBias_更新为优化后的偏置，并用prevPose_、prevVel_构造优化后的prevState_。
更新预计分器：将新的偏置prevBias_作为参数更新优化预计分器（会将之前的预积分量清零）。
检测优化结果：通过failureDetection(prevVel_, prevBias_)检测优化是否失败，若失败则使用resetParams()重置标志位，准备使系统重新进入初始化操作。
利用优化后的结果对IMU里程计进行预计分：将里程计状态prevStateOdom和偏置prevBiasOdom更新为优化后的位姿和偏置。剔除信息处理IMU数据集（是一个与优化IMU数据集内容相同的IMU数据集，保存着每次到来的IMU数据，在上边的优化操作中使用的是优化IMU数据集，每使用一帧IMU数据，便从数据集队列中除去一个数据，因此在此时重新进行预积分时需要使用另一个数据相同且未被除去元素的信息处理IMU数据集）中时间在当前雷达帧之前的数据（仅剩余当前第k个雷达帧之后的数据），使用优化后的第k帧偏置参数重新配置信息处理IMU预积分器（会清空预积分量），用gtsam的预积分接口使用信息处理IMU预积分器对信息处理IMU数据集中的数据进行预积分，以供IMU回调函数作为基本预积分量使用。
调整标志位：将迭代次数key加一，并将完成首次优化doneFirstOpt标志位置true。
void odometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
//取出时间戳
double currentCorrectionTime = ROS_TIME(odomMsg);
// make sure we have imu data to integrate
// 确保我们已经进行过imu数据积分了
if (imuQueOpt.empty())
return;
// 获取里程计位姿信息
float p_x = odomMsg->pose.pose.position.x;
float p_y = odomMsg->pose.pose.position.y;
float p_z = odomMsg->pose.pose.position.z;
float r_x = odomMsg->pose.pose.orientation.x;
float r_y = odomMsg->pose.pose.orientation.y;
float r_z = odomMsg->pose.pose.orientation.z;
float r_w = odomMsg->pose.pose.orientation.w;
// 该位姿是否出现退化（发生退化会在后端优化中将该值置1）
// 退化会导致里程计准确性和精度有一定的下降
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
// 将位姿转换为gtsam格式
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
// 0. initialize system
// 矫正过程的初始化
if (systemInitialized == false)
{
resetOptimization(); // 初始化isam2优化器及非线性因子图
// pop old IMU message
// 丢弃老的imu信息（将时间小于当前时间戳的IMU信息移除队列）
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
// initial pose
// 通过最终优化过的雷达位姿初始化先验的位姿信息并添加到因子图中
// 补偿从雷达到IMU的坐标变换lidar2Imu，把lidar位姿转换到IMU坐标系下（通过lidar里程计的雷达位姿得到IMU的位姿？）
prevPose_ = lidarPose.compose(lidar2Imu); // lidarPose * lidar2Imu
// 添加IMU位姿的先验约束以及置信度priorPoseNoise，置信度由协方差矩阵决定，协方差矩阵越小代表置信度越高
// 先验约束的作用是限制优化结果不能离先验证太远，限制权重由置信度决定
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);// 对第0个位姿产生的先验约束
// 将该约束加入到因子图中
graphFactors.add(priorPose);
// initial velocity
// 初始化先验速度信息为0并添加到因子图中，操作同上
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel);
// initial bias
// 初始化先验偏置信息为0并添加到因子图中，操作同上
prevBias_ = gtsam::imuBias::ConstantBias();
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
graphFactors.add(priorBias);
// add values
// 设置变量的初始估计值
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// optimize once
// 将因子图状态和约束更新到isam2优化器中
optimizer.update(graphFactors, graphValues);
// 将状态和约束清零，方便下一次添加因子
graphFactors.resize(0);
graphValues.clear();
// 使用初始零偏对预计分接口进行初始化
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
key = 1;
systemInitialized = true;
return; // 第一次进入函数初始化后即跳出函数
}
// reset graph for speed
// 当isam2规模太大时，因子图中加入太多约束，为防止运算之间过长，进行边缘化，重置（清空）优化器和因子图
if (key == 100)
{
// get updated noise before reset
// 获取最新关键帧的位姿、速度、零偏的协方差矩阵
gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key - 1)));
gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key - 1)));
gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key - 1)));
// reset graph
// 重置isam2优化器和因子图
resetOptimization();
// 按最新关键帧的协方差将位姿、速度、偏置因子添加到因子图中（用最新关键帧的位姿、速度、零偏及其协方差矩阵，对优化器和因子图进行初始化）
// 对位姿先验进行初始化 add pose
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
graphFactors.add(priorPose);
// 对速度先验进行初始化 add velocity
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
graphFactors.add(priorVel);
// 对零偏先验进行初始化 add bias
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
graphFactors.add(priorBias);
// add values
// 并用最新关键帧的位姿、速度、偏置初始化对应的变量因子
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// optimize once
// 并将最新初始化的因子图更新到重置的isam2优化器中
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
key = 1; // 重置关键帧数量
}
// 1. integrate imu data and optimize
// 1. 预积分imu数据并进行优化
// 当存在imu数据时
while (!imuQueOpt.empty())
{
// pop and integrate imu data that is between two optimizations
// 对相邻两次优化之间的imu帧进行积分，并移除
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
double imuTime = ROS_TIME(thisImu);
// 在第n-1帧进入该函数时，已经剔除n-1帧之前的数据，此时第n帧进入时，取出当前时间戳之前的数据，相当于从第n-1帧到第n帧的数据，并对其进行预计分
if (imuTime < currentCorrectionTime - delta_t)
{
// 计算两帧IMU之间的时间差，如果是首帧IMU则时间差为负值，将其赋值为1/500，相当于一个趋近于0的数
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
// 调用IMU预计分接口进行预计分处理
imuIntegratorOpt_->integrateMeasurement(
gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
// 记录当前IMU时间
lastImuT_opt = imuTime;
// 把处理过的IMU数据从队列里剔除
imuQueOpt.pop_front();
}
else
break;
}
// add imu factor to graph
// 用gtsam的数据类型转换工具把所求得的IMU预计分数据 转换为 添加因子图时所需要的数据类型
const gtsam::PreintegratedImuMeasurements &preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements &>(*imuIntegratorOpt_);
// 该因子涉及的优化变量包括:上一帧和当前帧的位姿和速度，上一帧的偏置（零偏），相邻两关键帧之间的预积分结果
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
// 将imu因子添加到因子图中
graphFactors.add(imu_factor);
// add imu bias between factor
// 将imu偏置(零偏)因子添加到因子图中
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
// add pose factor
// 添加当前关键帧位姿因子
// 通过lidar里程计的结果得到该关键帧IMU坐标
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
// 作为先验因子加入到因子图优化的框架当中，作为第key帧的先验，并根据是否退化（degenerate是否为1）进行置信度的选取
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
// 将位姿因子（里程计约束）加入到因子图框架当中
graphFactors.add(pose_factor);
// insert predicted values
// 设置当前关键帧位姿因子、速度因子和偏置因子的初始值
// 用上一帧的状态（位姿、速度）和零偏以及两帧之间的IMU预计分结果，通过gtsam的接口对当前帧的状态（位姿、速度）进行估计，作为位姿因子的初始值（有的地方直接将lidar里程计的结果作为初始值）
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_); // 定义prevState_的时候会调用默认构造函数并赋0初值
// 将估计出的初始值插入到因子图当中
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
// 零偏初始值使用上一帧的零偏值
graphValues.insert(B(key), prevBias_);
// optimize
// 将最新关键帧相关的因子图更新到isam2优化器中，并进行优化（优化了两次）
optimizer.update(graphFactors, graphValues);
optimizer.update();
// 将约束因子和变量因子清零
graphFactors.resize(0);
graphValues.clear();
// Overwrite the beginning of the preintegration for the next step.
// 获取当前关键帧的优化结果，并将先前值更新为此结果值
gtsam::Values result = optimizer.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));
// Reset the optimization preintegration object.
// 利用优化后的imu零偏信息复位（重置）imu预积分（约束）对象，
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
// check optimization
// 对优化结果进行失败检测: 当速度和偏置太大时，则认为优化失败
if (failureDetection(prevVel_, prevBias_))
{
resetParams();
return;
}
// 2. after optiization, re-propagate imu odometry preintegration
// 2. 优化后，重新对imu里程计进行预积分
// 利用优化结果更新prev状态
prevStateOdom = prevState_;
prevBiasOdom = prevBias_;
// first pop imu message older than current correction data
// 定义变量并赋任意初值
double lastImuQT = -1;
// 对于第n个关键帧之前的IMU数据进行剔除，丢弃早于矫正时间的imu帧
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}
// repropogate
// 重新进行预积分，从矫正时间开始
// 将第n帧lidar里程计数据到来之后的IMU数据进行预计分，此时的零偏使用优化后的零偏
if (!imuQueImu.empty())
{
// reset bias use the newly optimized bias
// 将优化后的imu偏置信息更新到预积分器内
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// integrate imu message from the beginning of this optimization
// 从矫正时间开始，对imu数据重新进行预积分，得到预计分结果
for (int i = 0; i < (int)imuQueImu.size(); ++i)
{
sensor_msgs::Imu *thisImu = &imuQueImu[i];
double imuTime = ROS_TIME(thisImu);
double dt = (lastImuQT < 0) ? (1.0 / 500.0) : (imuTime - lastImuQT);
imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
lastImuQT = imuTime;
}
}
++key;
doneFirstOpt = true;
}
六、bool failureDetection(const gtsam::Vector3 &velCur, const gtsam::imuBias::ConstantBias &biasCur)
优化结果失败检测函数。构造三个三维向量，分别是优化后的位姿向量、加速度计偏差向量、陀螺仪偏差向量，设定阈值并计算各向量的模长是否大于阈值，若大于阈值则认为优化结果过大，优化失败，返回true，否则返回false。
bool failureDetection(const gtsam::Vector3 &velCur, const gtsam::imuBias::ConstantBias &biasCur)
{
Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
// 当速度太大，则认为失败
if (vel.norm() > 30) // norm()对于向量为取模操作，即自身点积的平方根
{
ROS_WARN("Large velocity, reset IMU-preintegration!");
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)
{
ROS_WARN("Large bias, reset IMU-preintegration!");
return true;
}
return false;
}
七、void imuHandler(const sensor_msgs::Imu::ConstPtr &imu_raw)
IMU订阅者回调函数。
定义自解锁对互斥锁进行上锁
保护公共内存。
转换坐标系形式（貌似）
通过imuConverter(*imu_raw)将信息从IMU坐标轴定义形式转到雷达坐标轴定义形式。
添加新的IMU数据
将新接收的IMU数据添加到优化IMU数据集队列和信息处理IMU数据集队列中。
判断是否完成首次优化
通过标志位doneFirstOpt判断是否完成首次优化，如果没有完成首次优化则直接退出程序，不进行后续操作。
进行预积分并预测此时的位姿
使用数据处理IMU预积分器利用新的IMU数据在之前的预积分量的基础上进行一次IMU预积分，并利用当前预积分量（预积分器中从上一个雷达帧时间戳累计预积分到现在的结果），根据最新的雷达帧频率IMU状态和偏置，估计当前IMU帧的IMU状态。
将数据转换成ROS格式并进行发布
定义消息odometry，将IMU数据的数据头信息thisImu.header.stamp赋值给odometry，并设置坐标系信息。
通过从IMU到雷达的坐标变换得到雷达位姿，并将位姿信息和速度信息赋值到ROS消息中，通过发布者pubImuOdometry进行发布，实现以IMU速率的位姿更新。
void imuHandler(const sensor_msgs::Imu::ConstPtr &imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
sensor_msgs::Imu thisImu = imuConverter(*imu_raw); // 将imu信息转换到雷达坐标系（前左上）下表达,其实也就是获得雷达运动的加速度、角速度和姿态信息
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
if (doneFirstOpt == false)
return;
double imuTime = ROS_TIME(&thisImu);
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu); // 获取相邻两帧imu数据时间差
lastImuT_imu = imuTime;
// integrate this single imu message
// 记录imu的测量信息，进行IMU预积分
imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);
// predict odometry
// 利用上一时刻的imu里程计状态信息PVQ和偏置信息，根据最近得到的IMU预积分，估计当前时刻imu里程计状态信息PVQ
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
// publish odometry
// 将数据转换为ROS格式并进行发布，实现以IMU频率进行位姿更新
nav_msgs::Odometry odometry;
odometry.header.stamp = thisImu.header.stamp;
odometry.header.frame_id = odometryFrame;
odometry.child_frame_id = "odom_imu";
// transform imu pose to ldiar
gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar); // 获得估计的雷达位姿信息
// 发布通过imu估计的雷达里程计信息(后面都称为imu里程计信息)
odometry.pose.pose.position.x = lidarPose.translation().x();
odometry.pose.pose.position.y = lidarPose.translation().y();
odometry.pose.pose.position.z = lidarPose.translation().z();
odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
odometry.twist.twist.linear.x = currentState.velocity().x();
odometry.twist.twist.linear.y = currentState.velocity().y();
odometry.twist.twist.linear.z = currentState.velocity().z();
odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
pubImuOdometry.publish(odometry);
}
位姿数据转换发布类 TransformFusion
用于建图结果可视化
一、成员变量
public:
std::mutex mtx; // 互斥锁
ros::Subscriber subImuOdometry; // 通过imu积分估计的雷达里程计信息订阅器
ros::Subscriber subLaserOdometry; // 最终优化后的里程计信息订阅器
ros::Publisher pubImuOdometry; // imu里程计信息发布器
ros::Publisher pubImuPath; // imu路径发布器
Eigen::Affine3f lidarOdomAffine; // 带回环的激光雷达里程计信息
Eigen::Affine3f imuOdomAffineFront; // 最老的IMU里程计信息
Eigen::Affine3f imuOdomAffineBack; // 最新的IMU里程计信息
tf::TransformListener tfListener; // tf树监听者
tf::StampedTransform lidar2Baselink; // 从雷达到船体基坐标系的坐标变换
double lidarOdomTime = -1; // 带回环的激光雷达里程计帧时间戳
deque<nav_msgs::Odometry> imuOdomQueue; // IMU里程计信息队列
二、构造函数 TransformFusion()
获取坐标变换
判断雷达坐标系是否和船体基坐标系重合，如果不重合则通过ROS的tf监听者查找从雷达到基坐标系的坐标变换，保存在lidar2Baselink中。
初始化订阅者和发布者
| 订阅者（s）or发布者（p） | 作用 | 节点 | 数据类型 | 回调函数 |
|---|---|---|---|---|
| subLaserOdometry（s） | 订阅带回环修正的雷达里程计信息 | “lio_sam/mapping/odometry” | nav_msgs::Odometry | TransformFusion::lidarOdometryHandler |
| subImuOdometry（s） | 订阅IMU预计分节点发布的最新位姿信息 | odomTopic + “_incremental” | nav_msgs::Odometry | TransformFusion::imuOdometryHandler |
| pubImuOdometry（p） | imu里程计信息发布器 | odomTopic | nav_msgs::Odometry | |
| pubImuPath | imu路径发布器 | “lio_sam/imu/path” | nav_msgs::Path |
TransformFusion()
{
// 如果雷达坐标系（lidar）和基座标系（baselink一般为船体坐标系）不一致，则获取雷达坐标系相对于基座标系的转换关系
if (lidarFrame != baselinkFrame)
{
// 有关tf的操作建议写成try catch的形式，以便抛出异常防止程序崩掉
try
{
// 查询lidar和baselink之间的坐标变换
tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink); // ros::Time(0)表示最新结果
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
}
}
// 订阅带回环修正的雷达里程计信息
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅IMU预计分节点发布的最新位姿信息
subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic + "_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
// TransformFusion这个类产生的数据没有被其它节点使用，只是单纯的为了rviz显示用，所以这个类可以去掉，不影响最后的建图结果
pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000); // 该话题没有被任何其它节点利用
pubImuPath = nh.advertise<nav_msgs::Path>("lio_sam/imu/path", 1); // 该话题只为显示用
}
三、Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
将ros的数据格式转换为eigen的数据格式
Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
{
double x, y, z, roll, pitch, yaw;
x = odom.pose.pose.position.x;
y = odom.pose.pose.position.y;
z = odom.pose.pose.position.z;
tf::Quaternion orientation;
tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
return pcl::getTransformation(x, y, z, roll, pitch, yaw);
}
四、void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg)
带回环修正的雷达里程计信息订阅者回调函数。
定义自解锁给进程上锁，保护公共内存。
将读取到的带回环的激光雷达里程计信息转换成eigen格式。
记录该激光雷达里程计帧的时间戳，并转换成浮点型。
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
lidarOdomAffine = odom2affine(*odomMsg);
lidarOdomTime = odomMsg->header.stamp.toSec();
}
五、void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg)
IMU里程计信息订阅者回调函数。该消息由IMU预积分类中的IMU里程计发布者发布，内容为通过IMU预积分和上一个雷达帧得到的当前雷达帧的状态信息。
发送里程计坐标系和世界坐标系的静态坐标变换
发送从地图坐标系mapFrame到里程计坐标系odometryFrame的坐标变换，通常情况下世界坐标系和里程计坐标系是重合的，因此通过发布者向tf发布旋转和平移均为0的坐标变换，时间戳设置为读取到的IMU里程计的时间戳。
定义自解锁对互斥锁进行上锁
保护公共内存。
保存里程计数据
将接收的IMU里程计信息加入到IMU里程计信息队列当中。
判断是否收到带回环优化的IMU里程计信息
若时间lidarOdomTime存在且不为负数，则说明收到了带回环优化的IMU里程计信息，则继续后续操作，否则返回并退出函数。
计算最新的里程计信息
将时间在lidarOdomTime之前的IMU里程计信息剔除，取出IMU信息队列当中队首和队尾（最老和最新）的IMU里程计信息，计算二者的相对变换（位姿增量），将该位姿增量叠加到最新的带回环检测雷达里程计位姿上，即可得到最新位姿。将得到的位姿转换为位置和欧拉角的形式。
注意IMU预积分优化时的先验位姿使用的是不带回环检测的雷达里程计位姿，因为此位姿的连续性较强，在优化时不会因为先验和观测相差太大而导致优化失败，但由于不带回环检测，因此优化出的位姿绝对位置的可信度不高，但优化出的两帧之间的位姿增量可信度较高，可将可信度较高的位姿增量叠加到可信度较高的带回环检测雷达里程计位姿上，即可得到可信度较高的当前位姿。
发布最新的IMU里程计信息
定义里程计消息，将IMU最后一个数据的消息头赋给里程计消息，并将最新里程计的平移和旋转赋值给里程计消息，通过pubImuOdometry进行发布。
注意
IMUPreintegration类和TransformFusion类中均有发布者pubImuOdometry，区别在于IMUPreintegration中的发布者发布的是以不含回环优化的雷达里程计位姿为基础的IMU里程计位姿，而pubImuOdometry中发布者发布的是以含回环优化的雷达里程计位姿为基础的IMU里程计位姿，因而IMUPreintegration发布的相当于中间变量，pubImuOdometry发布的才是最终结果。
更新船体与世界坐标系之间的坐标变换
定义一个发布者和坐标变换消息，将刚刚通过增量叠加得到的雷达里程计位姿信息转换成TF消息的形式；判断雷达坐标系和机器人基坐标系是否重合，如果不重合，则将刚刚得到的TF消息乘上从雷达到基坐标系的坐标变换；将最终得到的TF消息作为从里程计坐标系odometryFrame到船体基坐标系baselinkFrame之间的坐标变换，将时间戳设置为收到的IMU里程计数据的时间戳，使用发布者发布到TF树。
发布船体运动轨迹
获取IMU里程计队列中最后一个IMU数据的时间戳，如果时间和上一个添加到轨迹当中的IMU里程计数据的时间差大于0.1s（频率控制在10Hz一下），则将此雷达位姿添加到位姿轨迹消息队列中；剔除位姿轨迹消息队列中时间小于当前雷达帧时间-1s的轨迹消息；将轨迹消息的时间戳赋值为IMU里程计最后一个数据的时间戳，坐标系设置为里程计坐标系odometryFrame，通过发布者pubImuPath将轨迹消息发布出去。
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr &odomMsg)
{
// static tf
static tf::TransformBroadcaster tfMap2Odom; // map坐标系一般以上电时刻为原点
static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0)); // 一般建图算法初始时刻map坐标系和odom坐标系是重合的
// 发送静态坐标变换，使map和odom坐标系重合
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame)); // 发送坐标变换 参数：1坐标变换本身 2时间戳信息 3父坐标系 4子坐标系
std::lock_guard<std::mutex> lock(mtx);
imuOdomQueue.push_back(*odomMsg); // 记录通过imu估计的雷达里程计信息(后面简称imu里程计信息)，加入到队列里面
// get latest odometry (at current IMU stamp)
// 当没有订阅到最终优化后的里程计信息时，直接返回
if (lidarOdomTime == -1)
return;
// 当订阅到最终优化后的里程计信息时，剔除掉比该帧还老的imu里程计信息帧（即当有回环校正的lidar里程计位姿到来时，以该位姿为准，将之前的IMU前端估计的位姿去除）
while (!imuOdomQueue.empty())
{
if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
imuOdomQueue.pop_front();
else
break;
}
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front()); // 获取最老的imu里程计信息
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back()); // 获取最新的imu里程计信息
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack; // 获取最老最新帧之间的位姿增量
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre; // 在最新的最终优化后的里程计信息基础上叠加imu里程计位姿增量，获得最新的imu里程计信息
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw); // 通过仿射变换提取出位置信息和用欧拉角表示的姿态信息
// publish latest odometry
// 发布最新的imu里程计信息
nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
laserOdometry.pose.pose.position.x = x;
laserOdometry.pose.pose.position.y = y;
laserOdometry.pose.pose.position.z = z;
laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
pubImuOdometry.publish(laserOdometry);
// publish tf
// 更新船体与世界坐标系（里程计坐标系）之间的坐标变换
// 发布最新的odom与base_link之间的转换关系，为了rviz显示imu里程计路径用
static tf::TransformBroadcaster tfOdom2BaseLink;
tf::Transform tCur;
tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
if (lidarFrame != baselinkFrame)
tCur = tCur * lidar2Baselink;
tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
tfOdom2BaseLink.sendTransform(odom_2_baselink);
// publish IMU path
// 发布imu对应的路径信息（运动轨迹）
static nav_msgs::Path imuPath;
static double last_path_time = -1;
double imuTime = imuOdomQueue.back().header.stamp.toSec();
// 控制更新频率，不超过10hz
if (imuTime - last_path_time > 0.1)
{
last_path_time = imuTime;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
pose_stamped.header.frame_id = odometryFrame;
pose_stamped.pose = laserOdometry.pose.pose;
// 将最新的位姿送入轨迹中
imuPath.poses.push_back(pose_stamped);
// 将lidar时间戳之前的轨迹全部删除，仅保留IMU预测的轨迹
while (!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
imuPath.poses.erase(imuPath.poses.begin());
// 发布轨迹，实际上为imu预计分的预测轨迹（imu节点计算的轨迹）
if (pubImuPath.getNumSubscribers() != 0)
{
imuPath.header.stamp = imuOdomQueue.back().header.stamp;
imuPath.header.frame_id = odometryFrame;
pubImuPath.publish(imuPath);
}
}
}
数据结构
sensor_msgs::Imu 需要传入的IMU测量值数据结构
sensor_msgs/Imu
std_msgs/Header header // 消息头
geometry_msgs/Quaternion orientation // 姿态四元数
float64[9] orientation_covariance // 姿态观测噪声协方差矩阵
geometry_msgs/Vector3 angular_velocity // 角速度
float64[9] angular_velocity_covariance // 角速度观测噪声协方差矩阵
geometry_msgs/Vector3 linear_acceleration // 加速度
float64[9] linear_acceleration_covariance // 加速度观测噪声协方差矩阵
std_msgs/Header
uint32 seq // 连续递增的消息ID号
time stamp
string frame_id
geometry_msgs/Quaternion
float64 x
float64 y
float64 z
float64 w
geometry_msgs/Vector3
float64 x
float64 y
float64 z
GTSAM相关函数
GTSAM_EXPORT
用于在windows下编译GTSAM，可以放在函数名前或者类前，但如果定义类时使用了GTSAM_EXPORT，则类中的函数不能使用GTSAM_EXPORT。
具体使用方法见：https://gitee.com/syzhen123/gtsam/blob/master/Using-GTSAM-EXPORT.md
ROS相关函数
ros::MultiThreadedSpinner spinner(4);
开辟四个线程以供处理四个subscriber的接收和回调函数，防止一个subscriber在运行时其他subscriber数据丢失。
Eigen相关函数
(gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished()
在Eigen中重载了"<<"操作符，通过该操作符即可以一个一个元素的进行赋值。
其中finish()的官方注释为
/** \returns the built matrix once all its coefficients have been set.
* Calling finished is 100% optional. Its purpose is to write expressions
* like this:
* \code
* quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
* \endcode
*/
一旦所有的系数都设置好了，就建立了矩阵；
调用finished是100%可选的。 它的目的是写表达式。（有道翻译）
norm()
norm()对于向量为取模操作，即自身点积的平方根
其他相关内容
自解锁lock_guard
将互斥锁mutex放入自解锁lock_guard来定义自解锁，将自动上锁mutex对象，此时的线程将占用进程公共内存，直到离开自解锁的作用域后，自动调用自解锁的析构函数对互斥锁进行解锁。
同一个进程可以在不同的类中有多个互斥锁对象，但统一进程中不同互斥锁对象上锁时，锁住的进程都为同一个进程。
std::mutex mtx; // 定义互斥锁
std::lock_guard<std::mutex> lock(mtx); // 定义自解锁lock，并将进程锁mtx放入自解锁中
类型转换运算符static_cast
将value转换为typename格式，条件是二者之间至少有一个方向可以进行隐式转换。
// 使用格式如下
static_cast<typename>(value);
// Lie.h中使用了static_cast的方法
const Class & derived() const {
return static_cast<const Class&>(*this);
}
未解决问题
从IMU位姿到雷达位姿的坐标变换
在IMU订阅者回调函数中，每次接收到IMU信息时，使用imuConverter(*imu_raw)对IMU位姿进行过一次从IMU到雷达的坐标变换，并在进行预积分估计后，对估计结果再使用imuPose.compose(imu2Lidar)进行一次从IMU到雷达的坐标变换。不明白为什么要进行两次坐标变换，由于看不懂utility.h文件中的extRot旋转矩阵定义是什么意思，所以未解决此问题，猜测第一次是将坐标轴转换（xyz轴定义不同？），第二次是在相同的坐标轴定义的前提下进行坐标变换。
1644
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
