这里同样是imuPreintegration.cpp文件中的一个类 TransformFusion TF
订阅的消息话题
- 地图优化节点的全局位姿 “lio_sam/mapping/odometry”
- 预积分节点的增量位姿 odomTopic+“_incremental”
订阅地图优化节点的全局位姿的回调函数
将位姿解算乘欧拉角保存起来并返回到 变量lidarOdomAffine 中存储
// 将全局位姿保存下来
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
lidarOdomAffine = odom2affine(*odomMsg);
lidarOdomTime = odomMsg->header.stamp.toSec();
}
// 将位姿解算乘欧拉角保存起来并返回到lidarOdomAffine 中
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);
}
订阅预积分节点增量位姿的回调函数
这里才是位姿融合的关键函数
实际操作简述: 说白了就是拿到优化后的全局位姿和imu的局部增量位姿,然后加起来就是当前时刻的位姿了,具体操作在下面第4点,这样就能高频输出比较准确的位姿了,全局位姿在后端优化比较久
- 发送静态tf,odom系和map系将他们重合
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
- 将imu得到的里程计结果存入队列中
imuOdomQueue.push_back(*odomMsg);
- 弹出时间戳小于最新lidar位姿时刻之前的imu里程记数据
while (!imuOdomQueue.empty())
{
if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
imuOdomQueue.pop_front();
else
break;
}
- 计算最新队列里imu里程记的增量
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
// 增量补偿到lidar的位姿上去,就得到了最新的预测的位姿
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre; //全局位姿+局部增量
float x, y, z, roll, pitch, yaw;
// 分解成平移+欧拉角的形式
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
- 发送全局一致位姿的最新位姿
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);
- 更新TF和发布imu里程计的轨迹
// publish tf
// 更新tf
static tf::TransformBroadcaster tfOdom2BaseLink;
tf::Transform tCur;
tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
if(lidarFrame != baselinkFrame)
tCur = tCur * lidar2Baselink;
// 更新odom到baselink的tf
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时间戳之前的轨迹全部擦除
while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
imuPath.poses.erase(imuPath.poses.begin());
// 发布轨迹,这个轨迹实际上是可视化imu预积分节点输出的预测值
if (pubImuPath.getNumSubscribers() != 0)
{
imuPath.header.stamp = imuOdomQueue.back().header.stamp;
imuPath.header.frame_id = odometryFrame;
pubImuPath.publish(imuPath);
}
}