节点数据传递
从上面的节点之间话题传递图可以看出imu预积分节点(红色) /lio_sam_imuPreintegration 接收了话题(蓝色) /imu, /odometry/imu_incremental, /lio_sam/mapping/odometry, /lio_sam/mapping/odometry_incremental，发布了被其它节点使用的话题(绿色) /odometry/imu_incremental, /lio_sam/imu/path，其中发布的话题 /lio_sam/imu/path 只是被用于rviz显示，所以可以忽略．因此，整个预积分节点所做的事情可以概括为接收话题 /imu, /odometry/imu_incremental, /lio_sam/mapping/odometry, /lio_sam/mapping/odometry_incremental 用于干什么，话题 /odometry/imu_incremental 内容是什么，怎样生成的．
接收 /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的测量信息
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里程计状态信息PVQ
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); // 获得估计的雷达位姿信息
// 发布通过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);
}
从上面的源码可以看出，订阅 /imu 话题的作用就是对imu的测量数据进行预积分，并发布出预积分结果imu里程计增量话题 /odometry/imu_incremental.
接收 /lio_sam/mapping/odometry_incremental 话题干什么?
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;
// 转换消息数据为gtsam的3d位姿信息
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));
// 0. initialize system
// 矫正过程的初始化
if (systemInitialized == false)
{
resetOptimization(); // 初始化isam2优化器及非线性因子图
// pop old IMU message
// 丢弃老的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
// 通过最终优化过的雷达位姿初始化先验的位姿信息并添加到因子图中
prevPose_ = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
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);
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;
}
// add imu factor to graph
// 将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); // 该因子涉及的优化变量包括:上一帧和当前帧的位姿和速度，上一帧的偏置，相邻两关键帧之间的预积分结果
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
// 添加当前关键帧位姿因子
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);
// 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_);
// 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
// 丢弃早于矫正时间的imu帧
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
// 将优化后的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;
}
从代码中可以看出，接收 /lio_sam/mapping/odometry_incremental 话题，为的就是对IMU的偏置进行矫正．该部分是imu预积分节点中最重要的也是LIO-SAM相对于LeGO-LOAM、LOAM的主要区别之一．这部分通过订阅通过地图优化节点优化后的雷达里程计信息对IMU的偏置进行矫正，并对矫正后的预积分数据重新进行预积分，有效的抑制了imu预积分的漂移，明显的提高预积分的精度．预积分精度提高就能够提供扫描匹配较精确的初始估计值，从而提高地图优化节点中的匹配精度．(形象一点的解释是这个过程就是一个反馈，告诉这个节点之前给的估计值不太对，需要调整一下用于估计的偏置参数)．
接收 /odometry/imu_incremental, /lio_sam/mapping/odometry 话题是干什么?
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)
{
// 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); // 记录通过imu估计的雷达里程计信息(后面简称imu里程计信息)
// get latest odometry (at current IMU stamp)
// 当没有订阅到最终优化后的里程计信息时，直接返回
if (lidarOdomTime == -1)
return;
// 当订阅到最终优化后的里程计信息时，剔除掉比该帧还老的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();
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);
}
}
}
从上面的代码注释中可以看出，该部分主要是对IMU里程计进行矫正，为了保证与全局雷达里程计一致．思路就是将优化后的雷达里程计信息加上从雷达里程计时间戳到当前IMU里程计时间戳之间的IMU里程计增量作为当前IMU里程计，这样的话就能保证矫正后的IMU里程计信息与全局雷达里程计信息之间只差了一个短时间的IMU里程计增量．该部分代码其实对整体的LIO-SAM建图功能没有任何影响，从代码中可以看出，该部分内容只发布了lio_sam/imu/path和odometry/imu两个话题，第一个话题只被rviz节点调用，单纯做显示用，第二个话题没有被任何节点调用，因此可以证明该部分内容产生的结果对建图是没有影响的．
/odometry/imu_incremental 是什么内容?
从第二部分的内容分析可知，该话题发布的内容就是预积分的结果，也即IMU里程计．
426
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
