前言：
对于lio-sam中IMU预积分源码的理解。预积分在imuPreintegration.cpp里面。
一、main函数
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);
spinner.spin();
return 0;
}初始化roboat_loam节点；
利用IMUPreintegration生成ImuP对象；
利用TransformFusion生成TF对象；
使用多线程并行执行MultiThreadedSpinner；
二、ParamServer类
类IMUPreintegration继承自ParamServer类，所以先分析一下ParamServer类的内容。
ParamServer类的定义在include/utility.h里面。主要是使用ros参数服务器对于一些常量的定义。
挑选一下于IMU相关的变量。
//Topics
string imuTopic;
// IMU
float imuAccNoise;
float imuGyrNoise;
float imuAccBiasN;
float imuGyrBiasN;
float imuGravity;
float imuRPYWeight;
vector<double> extRotV;
vector<double> extRPYV;
vector<double> extTransV;
Eigen::Matrix3d extRot;
Eigen::Matrix3d extRPY;
Eigen::Vector3d extTrans;
Eigen::Quaterniond extQRPY;这是和IMU相关的变量：
imuTopic定义为topic；
imuAccNoise为imu线加速度噪声；
imuGyrNoise为imu陀螺仪噪声，也是角加速度噪声；
imuAccBiasN为imu线加速度零偏；
imuGyrBiasN为imu角加速度零偏；
imuGravity为重力；
imuRPYWeight为好像是权重？
后面的参数就是一些变化表示。
nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");设置imuTopic为"lio_sam/imuTopic"。
nh.param<float>("lio_sam/imuAccNoise", imuAccNoise, 0.01);
nh.param<float>("lio_sam/imuGyrNoise", imuGyrNoise, 0.001);
nh.param<float>("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002);
nh.param<float>("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003);
nh.param<float>("lio_sam/imuGravity", imuGravity, 9.80511);
nh.param<float>("lio_sam/imuRPYWeight", imuRPYWeight, 0.01);
nh.param<vector<double>>("lio_sam/extrinsicRot", extRotV, vector<double>());
nh.param<vector<double>>("lio_sam/extrinsicRPY", extRPYV, vector<double>());
nh.param<vector<double>>("lio_sam/extrinsicTrans", extTransV, vector<double>());
extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);对前面的IMU相关变量进行设置。
sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{
sensor_msgs::Imu imu_out = imu_in;
// rotate acceleration
Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
acc = extRot * acc;
imu_out.linear_acceleration.x = acc.x();
imu_out.linear_acceleration.y = acc.y();
imu_out.linear_acceleration.z = acc.z();
// rotate gyroscope
Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
gyr = extRot * gyr;
imu_out.angular_velocity.x = gyr.x();
imu_out.angular_velocity.y = gyr.y();
imu_out.angular_velocity.z = gyr.z();
// rotate roll pitch yaw
Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
Eigen::Quaterniond q_final = q_from * extQRPY;
imu_out.orientation.x = q_final.x();
imu_out.orientation.y = q_final.y();
imu_out.orientation.z = q_final.z();
imu_out.orientation.w = q_final.w();
if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
{
ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
ros::shutdown();
}
return imu_out;
}
};imuConverter函数的作用是读取参数const sensor_msgs::Imu& imu_in，然后将它里面的
acc值乘上extRot；
gyr值乘上extRot；
rpy值乘上extQRPY；
再将这些值生成一个新的sensor_msgs::Imu imu_out，并返回imu_out。
就是将imu_in进行坐标变换然后输出坐标变换后的值。
template<typename T>
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
{
*angular_x = thisImuMsg->angular_velocity.x;
*angular_y = thisImuMsg->angular_velocity.y;
*angular_z = thisImuMsg->angular_velocity.z;
}将imu中的角速度分别赋予*angular_x，*angular_y，*angular_z。
template<typename T>
void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
{
*acc_x = thisImuMsg->linear_acceleration.x;
*acc_y = thisImuMsg->linear_acceleration.y;
*acc_z = thisImuMsg->linear_acceleration.z;
}
将imu中的线速度分别赋予*acc_x, *acc_y, *acc_z。
template<typename T>
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
double imuRoll, imuPitch, imuYaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
*rosRoll = imuRoll;
*rosPitch = imuPitch;
*rosYaw = imuYaw;
}将imu中的旋转量按每个轴分解，赋予 *rosRoll, *rosPitch, *rosYaw。
三、 IMUPreintegration类
IMUPreintegration类继承自ParamServer类。
成员变量：
std::mutex mtx;
ros::Subscriber subImu;
ros::Subscriber subOdometry;
ros::Publisher pubImuOdometry;
bool systemInitialized = false;设置一个线程锁mtx；
定义两个subscriber分别订阅subimu和subOdometry；
定义一个publisher发布pubImuOdometry；
定义一个系统初始化标志systemInitialized。
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_;priorPoseNoise是位姿的先验noise；
priorVelNoise是速度的先验noise；
priorBiasNoise是零偏的先验noise；
correctionNoise和correctionNoise2是什么退化的不同噪声；
noiseModelBetweenBias是零偏之间的噪声；
imuIntegratorOpt_和imuIntegratorImu_是gtsam两个不同的imu预积分的测量值。
std::deque<sensor_msgs::Imu> imuQueOpt;
std::deque<sensor_msgs::Imu> imuQueImu;两个imu队列。
gtsam::Pose3 prevPose_;
gtsam::Vector3 prevVel_;
gtsam::NavState prevState_;
gtsam::imuBias::ConstantBias prevBias_;prevPose_是前一状态的pose；
prevVel_是前一状态的速度；
prevState_是前一状态的NavState，它包含了Pose (rotation, translation) + velocity；
prevBias_前一状态的两个零偏，它包含了biasAcc_和biasGyro_。
gtsam::NavState prevStateOdom;
gtsam::imuBias::ConstantBias prevBiasOdom;里程计的前一帧的状态和零偏。
bool doneFirstOpt = false;
double lastImuT_imu = -1;
double lastImuT_opt = -1;doneFirstOpt第一此优化是否完成；
lastImuT_imu表示上一个imu；
lastImuT_opt表示上一个imu优化。
gtsam::ISAM2 optimizer;
gtsam::NonlinearFactorGraph graphFactors;
gtsam::Values graphValues;
优化器，图还有value值。
const double delta_t = 0;
int key = 1;delta_t表示设定的误差角度；
key表示imu中加入gtsam 的因子数量。
// T_bl: tramsform points from lidar frame to imu frame
gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
// T_lb: tramsform points from imu frame to lidar frame
gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));imu2Lidar表示imu转换到lidar坐标系的位姿变换；
lidar2Imu表示lidar转换到imu坐标系的位姿变换。
构造函数：
IMUPreintegration()
{
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);
boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
p->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
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
priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
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();
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
}两个订阅一个发布，回调函数后面分析；
设置预积分的参数p；
设置p的加速度协方差、角速度协方差和积分协方差；
设置imu先验零偏；
后面就是给前面的参数赋值。
回调函数：
先分析subOdometry的odometryHandler：
std::lock_guard<std::mutex> lock(mtx);
double currentCorrectionTime = ROS_TIME(odomMsg);设置一个线程锁；
读取odomMsg的时间到currentCorrectionTime。
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;
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));将odomMsg中位置和姿态值分别赋予上面的变量，之后利用上面的变量生成lidarPose。
初始化：
当systemInitialized为false时，初始化系统。
// 0. initialize system
if (systemInitialized == false)
{
resetOptimization();
// pop old IMU message
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
// initial pose
prevPose_ = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
graphFactors.add(priorPose);
// initial velocity
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel);
// initial bias
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
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
key = 1;
systemInitialized = true;
return;
}首先时resetOptimization函数：
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;
}它的主要作用是生成一些gtsam的初始化变量，并将它们赋予之前存在的gtsam优化量如：
optimizer、graphFactors和graphValues，起到清零的作用。
然后是确定这一帧imu的数据在这一帧lidar数据之后，记录上一帧imu数据的时间，并赋予lastImuT_opt。
将lidar的pose变换到imu坐标系下，并设置成先验pose。
然后就是将先验pose加入graph中，并设置symbol为X(0)。
设置先验速度为0，加入graph中，设置symbol为V(0)。
设置先验零偏，加入graph中，设置symbol为B(0)。
因为没有其他初值，就将3个先验值设置成第一个值prevPose_、prevVel_和prevBias_，加入初值中graphValues。
优化一次，之后就将graphFactors和graphValues清零。
使用prevBias_初始化两个imu预积分变量。
设置key为1，systemInitialized为true。表示这个初始化只执行一次。
k==100后面分析。
imu积分和优化
while (!imuQueOpt.empty())
{
// pop and integrate imu data that is between two optimizations
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
double imuTime = ROS_TIME(thisImu);
if (imuTime < currentCorrectionTime - delta_t)
{
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
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);
lastImuT_opt = imuTime;
imuQueOpt.pop_front();
}
else
break;
}同样利用判断这一帧imu的时间是否在这一帧里程计时间之前，如果在就确定dt，并将这一帧imu的信息加入gtsam为预积分设置好的integrateMeasurement中。
// add imu factor to graph
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);
graphFactors.add(imu_factor);生成imu因子并加入graphFactor中。
// add imu bias between factor
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));生成零偏限制并加入graphFactor中。
// add pose factor
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);加入pose因子。
// insert predicted values
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);计算初预积分的值，然后把结果赋予propState_，作为初值加入graphValues。
// optimize
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.
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
// check optimization
if (failureDetection(prevVel_, prevBias_))
{
resetParams();
return;
}将结果赋予result，并将这一帧的pose，速度，零偏分别赋予prevPose_，prevVel_和prevBias_，并生成prevState_。利用prevBias_设置imu预分初始化。
利用两帧雷达之间的imu积分值推算
// 2. after optiization, re-propagate imu odometry preintegration
prevStateOdom = prevState_;
prevBiasOdom = prevBias_;
// first pop imu message older than current correction data
double lastImuQT = -1;
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}
// repropogate
if (!imuQueImu.empty())
{
// reset bias use the newly optimized bias
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// integrate imu message from the beginning of this optimization
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;取出前面设置的prevStateOdom和prevBiasOdom，将imuQueImu队列中前面的全部pop，之后将imuQueImu的所有imu全部放入integrateMeasurement。之后key++.
key==100的情况：
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
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
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
key = 1;
}就是当key==100的时候，清空一下gtsam中因子的数量，避免太多数据影响运算，然后将key置1。内容除了noise是使用的上key为99的时候的值，其他都类似。
imuHander:
接下来分析订阅imuTopic的回调函数imuHander。
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
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);
lastImuT_imu = imuTime;
// integrate this single imu message
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
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
// publish odometry
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);
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);
}首先设置线程锁；
转换imu的数据；
将转换好的imu数据押入两个队列中imuQueOpt和imuQueImu；
设置dt，然后将这一帧imu的数据放入imuIntegratorImu_->integrateMeasurement里面，计算初预测值currentState。
然后设置一个odometry，将预测的位姿赋予这个odometry发布出去。
四、TransformFusion类
同样继承自ParamServer类。
成员变量：
std::mutex mtx;
ros::Subscriber subImuOdometry;
ros::Subscriber subLaserOdometry;
ros::Publisher pubImuOdometry;
ros::Publisher pubImuPath;
Eigen::Affine3f lidarOdomAffine;
Eigen::Affine3f imuOdomAffineFront;
Eigen::Affine3f imuOdomAffineBack;
tf::TransformListener tfListener;
tf::StampedTransform lidar2Baselink;
double lidarOdomTime = -1;
deque<nav_msgs::Odometry> imuOdomQueue;两个订阅变量，两个发布变量；
三个lidar里程计变量；
tf变换两个；
imu里程计队列一个。
构造函数：
TransformFusion()
{
if(lidarFrame != baselinkFrame)
{
try
{
tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
}
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());
subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
pubImuPath = nh.advertise<nav_msgs::Path> ("lio_sam/imu/path", 1);
}如果lidarFrame不是baselinkFrame。那么利用tfListener求的lidarFrame到baselinkFrame的坐标变换矩阵lidar2Baselink。
后面就是两个订阅和两个发送。
lidarOdometryHandler：
首先看odom2affine函数：
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);
}主要作用是将里程计数据取出里面的x, y, z, roll, pitch, yaw输出。
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
lidarOdomAffine = odom2affine(*odomMsg);
lidarOdomTime = odomMsg->header.stamp.toSec();
}线程锁，加将里程计信息中的x, y, z, roll, pitch, yaw输出给lidarOdomAffine。
imuOdometryHandler：
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
// static tf
static tf::TransformBroadcaster tfMap2Odom;
static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
std::lock_guard<std::mutex> lock(mtx);
imuOdomQueue.push_back(*odomMsg);
// get latest odometry (at current IMU stamp)
if (lidarOdomTime == -1)
return;
while (!imuOdomQueue.empty())
{
if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
imuOdomQueue.pop_front();
else
break;
}
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
// publish latest odometry
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
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
static nav_msgs::Path imuPath;
static double last_path_time = -1;
double imuTime = imuOdomQueue.back().header.stamp.toSec();
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);
while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
imuPath.poses.erase(imuPath.poses.begin());
if (pubImuPath.getNumSubscribers() != 0)
{
imuPath.header.stamp = imuOdomQueue.back().header.stamp;
imuPath.header.frame_id = odometryFrame;
pubImuPath.publish(imuPath);
}
}
}设置map to odom的广播器，并发布出去。
后面的是取两帧imu的值，然后计算两帧imu之间的差值，将差值加上最新一帧的imu上。最后分解出x, y, z, roll, pitch, yaw。然后发布出去，并发布tf变换关系，和imu的path。
五、总结
gtsam将imu预积分继承到了库里面，一些操作十分便捷。vins-momo就全部用ceres手写，以后有时间去看看这个的实现。
1809
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
