LIO-SAM框架位姿融合输出
在imu预积分的节点中，在main函数里面 还有一个类的实例对象，那就是transformfusion tf。
其主要功能是做位姿融合输出，最终输出imu的预测结果，与上节中的imu预测结果的区别就是：
该对象的融合输出是基于全局位姿的基础上再进行imu的预测输出。全局位姿就是 经过回环检测后的lidar位姿。
建图优化会输出两种激光雷达的位姿：
lidar 增量位姿lidar 全局位姿其中lidar 增量位姿就是 通过 lidar的匹配功能，优化出的帧间的相对位姿，通过相对位姿的累积，形成世界坐标系下的位姿
lidar全局位姿 则是在 帧间位姿的基础上，通过 回环检测，再次进行优化的 世界坐标系下的位姿，所以对于增量位姿，全局位姿更加精准
在前面提到的发布的imu的预测位姿是在lidar的增量位姿上基础上预测的，那么为了更加准确，本部分功能就预测结果，计算到基于全局位姿的基础上面。首先看构造函数
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()); } }判断lidar帧和baselink帧是不是同一个坐标系，通常baselink指车体系，如果不是，查询 一下 lidar 和baselink 之间的 tf变换 ros::time(0) 表示最新的，等待两个坐标系有了变换，更新两个的变换 lidar2baselink
sublaserodometry = nh.subscribe(lio_sam/mapping/odometry, 5, &transformfusion::lidarodometryhandler, this, ros::transporthints().tcpnodelay()); subimuodometry = nh.subscribe(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);发布两个信息 odomtopic imupath，然后看第一个回调函数 lidarodometryhandler
void lidarodometryhandler(const nav_msgs::odometry::constptr& odommsg) { std::lock_guard lock(mtx); lidarodomaffine = odom2affine(*odommsg); lidarodomtime = odommsg- >header.stamp.tosec(); }将全局位姿保存下来，将ros的odom格式转换成 eigen::affine3f 的形式，将最新帧的时间保存下来，第二个回调函数是 imuodometryhandler，imu预积分之后所发布的imu频率的预测位姿
void imuodometryhandler(const nav_msgs::odometry::constptr& odommsg) {static tf::transformbroadcaster tfmap2odom; static tf::transform map_to_odom = tf::transform(tf::createquaternionfromrpy(0, 0, 0), tf::vector3(0, 0, 0));建图的话 可以认为 map坐标系和odom坐标系 是重合的（初始化时刻）
tfmap2odom.sendtransform(tf::stampedtransform(map_to_odom, odommsg- >header.stamp, mapframe, odometryframe));发布静态tf，odom系和map系 他们是重合的
imuodomqueue.push_back(*odommsg);imu得到的里程计结果送入到这个队列中
if (lidarodomtime == -1) return;如果没有收到lidar位姿 就returen
while (!imuodomqueue.empty()) { if (imuodomqueue.front().header.stamp.tosec() header.stamp, odometryframe, baselinkframe); tfodom2baselink.sendtransform(odom_2_baselink);更新odom到baselink的tf
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); } } }发布imu里程计的轨迹，控制一下更新频率，不超过10hz，将最新的位姿送入轨迹中，把lidar时间戳之前的轨迹全部擦除，发布轨迹，这个轨迹实践上是可视化imu预积分节点输出的预测值。
各种有可能成为下一个大事物的颠覆性技术汇编
电解电容与瓷片电容的区别是什么
LED显示屏COB封装与GOB封装的区别及优势对比
RZ/G2x微处理器使HMI开发更简单、更快、更小
仿生科技成为机器人技术发展最快的领域之一
LIO-SAM框架位姿融合输出
SMT回流焊のPCBA底填胶水UNDERFILL
努比亚红魔3评测 一款真正为游戏而生的手机
智慧交通大爆发的原因是什么
波峰焊助焊剂喷头的工作原理
2020年的机圈，真的创新已死吗
大功率信号发生器的原理是什么
三星成功开发LPDDR5X DRAM，将扩大超高速数据服务市场
物联网平台类型的基本组成以及在物联网中的作用
华硕WIFI6路由 助力ROG DAY粉丝嘉年华掀起2.0时代
业界首款NVMe-oF SSD转换器控制器能够降低总体拥有成本
直流电能表在光伏储能系统中的应用说明
拓展智慧医疗应用，触控一体机电磁兼容EMC认证
工业主板的几大产品优势
FreeBSD对WiFi 5支持比较落后 用户催基金会出手赞助
其主要功能是做位姿融合输出，最终输出imu的预测结果，与上节中的imu预测结果的区别就是：
该对象的融合输出是基于全局位姿的基础上再进行imu的预测输出。全局位姿就是 经过回环检测后的lidar位姿。
建图优化会输出两种激光雷达的位姿：
lidar 增量位姿lidar 全局位姿其中lidar 增量位姿就是 通过 lidar的匹配功能，优化出的帧间的相对位姿，通过相对位姿的累积，形成世界坐标系下的位姿
lidar全局位姿 则是在 帧间位姿的基础上，通过 回环检测，再次进行优化的 世界坐标系下的位姿，所以对于增量位姿，全局位姿更加精准
在前面提到的发布的imu的预测位姿是在lidar的增量位姿上基础上预测的，那么为了更加准确，本部分功能就预测结果，计算到基于全局位姿的基础上面。首先看构造函数
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()); } }判断lidar帧和baselink帧是不是同一个坐标系，通常baselink指车体系，如果不是，查询 一下 lidar 和baselink 之间的 tf变换 ros::time(0) 表示最新的，等待两个坐标系有了变换，更新两个的变换 lidar2baselink
sublaserodometry = nh.subscribe(lio_sam/mapping/odometry, 5, &transformfusion::lidarodometryhandler, this, ros::transporthints().tcpnodelay()); subimuodometry = nh.subscribe(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);发布两个信息 odomtopic imupath，然后看第一个回调函数 lidarodometryhandler
void lidarodometryhandler(const nav_msgs::odometry::constptr& odommsg) { std::lock_guard lock(mtx); lidarodomaffine = odom2affine(*odommsg); lidarodomtime = odommsg- >header.stamp.tosec(); }将全局位姿保存下来，将ros的odom格式转换成 eigen::affine3f 的形式，将最新帧的时间保存下来，第二个回调函数是 imuodometryhandler，imu预积分之后所发布的imu频率的预测位姿
void imuodometryhandler(const nav_msgs::odometry::constptr& odommsg) {static tf::transformbroadcaster tfmap2odom; static tf::transform map_to_odom = tf::transform(tf::createquaternionfromrpy(0, 0, 0), tf::vector3(0, 0, 0));建图的话 可以认为 map坐标系和odom坐标系 是重合的（初始化时刻）
tfmap2odom.sendtransform(tf::stampedtransform(map_to_odom, odommsg- >header.stamp, mapframe, odometryframe));发布静态tf，odom系和map系 他们是重合的
imuodomqueue.push_back(*odommsg);imu得到的里程计结果送入到这个队列中
if (lidarodomtime == -1) return;如果没有收到lidar位姿 就returen
while (!imuodomqueue.empty()) { if (imuodomqueue.front().header.stamp.tosec() header.stamp, odometryframe, baselinkframe); tfodom2baselink.sendtransform(odom_2_baselink);更新odom到baselink的tf
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); } } }发布imu里程计的轨迹，控制一下更新频率，不超过10hz，将最新的位姿送入轨迹中，把lidar时间戳之前的轨迹全部擦除，发布轨迹，这个轨迹实践上是可视化imu预积分节点输出的预测值。
各种有可能成为下一个大事物的颠覆性技术汇编
电解电容与瓷片电容的区别是什么
LED显示屏COB封装与GOB封装的区别及优势对比
RZ/G2x微处理器使HMI开发更简单、更快、更小
仿生科技成为机器人技术发展最快的领域之一
LIO-SAM框架位姿融合输出
SMT回流焊のPCBA底填胶水UNDERFILL
努比亚红魔3评测 一款真正为游戏而生的手机
智慧交通大爆发的原因是什么
波峰焊助焊剂喷头的工作原理
2020年的机圈，真的创新已死吗
大功率信号发生器的原理是什么
三星成功开发LPDDR5X DRAM，将扩大超高速数据服务市场
物联网平台类型的基本组成以及在物联网中的作用
华硕WIFI6路由 助力ROG DAY粉丝嘉年华掀起2.0时代
业界首款NVMe-oF SSD转换器控制器能够降低总体拥有成本
直流电能表在光伏储能系统中的应用说明
拓展智慧医疗应用，触控一体机电磁兼容EMC认证
工业主板的几大产品优势
FreeBSD对WiFi 5支持比较落后 用户催基金会出手赞助