0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看技术视频
  • 写文章/发帖/加入社区
会员中心
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

LIO-SAM框架位姿融合输出

麦辣鸡腿堡 来源:古月居 作者:月照银海似蛟龙 2023-11-24 17:28 次阅读

在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< 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&lt;nav_msgs::Odometry&gt;(odomTopic, 2000);        pubImuPath       = nh.advertise&lt;nav_msgs::Path&gt;    ("lio_sam/imu/path", 1);

发布两个信息 odomTopic ImuPath,然后看第一个回调函数 lidarOdometryHandler

void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)    {        std::lock_guard< std::mutex > 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() <= lidarOdomTime)                imuOdomQueue.pop_front();            else                break;        }

弹出时间戳 小于 最新 lidar位姿时刻之前的imu里程计数据

Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;

计算最新队列里imu里程计的增量

Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;

增量补偿到lidar的位姿上去,就得到了最新的预测的位姿

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);

发送全局一致位姿的 最新位姿

static tf::TransformBroadcaster tfOdom2BaseLink;        tf::Transform tCur;        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);        if(lidarFrame != baselinkFrame)            tCur = tCur * lidar2Baselink;

更新tf

tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg- >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预积分节点输出的预测值。

声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • 框架
    +关注

    关注

    0

    文章

    398

    浏览量

    17400
  • 检测
    +关注

    关注

    5

    文章

    4412

    浏览量

    91301
  • SAM
    SAM
    +关注

    关注

    0

    文章

    111

    浏览量

    33478
  • 激光雷达
    +关注

    关注

    967

    文章

    3919

    浏览量

    189409
收藏 人收藏

    评论

    相关推荐

    激光SLAM:Faster-Lio算法编译与测试

    Faster-LIO是基于FastLIO2开发的。FastLIO2是开源LIO中比较优秀的一个,前端用了增量的kdtree(ikd-tree),后端用了迭代ESKF(IEKF),流程短,计算快
    的头像 发表于 01-12 10:22 2626次阅读
    激光SLAM:Faster-<b class='flag-5'>Lio</b>算法编译与测试

    #硬声创作季 LIO-SAM:一种紧耦合激光雷达-惯性里程计

    激光SAM计算机视觉
    Mr_haohao
    发布于 :2022年10月12日 15:21:47

    基于多传感器信息融合的物体位姿检测方法

    提出了利用摄像机和超声传感器信息融合对机器人操作对象进行姿检测的方法。该方法通过单幅图像获取目标点在图像中的理想坐标,求得投影矢量的方向,然后通过机器人末端的
    发表于 06-23 10:43 13次下载

    一种移动机器人的姿检测模块设计_方庆山

    一种移动机器人的姿检测模块设计_方庆山
    发表于 03-19 11:45 1次下载

    工业机器人姿准确度_重复性测量仪设计

    激光 姿测量
    发表于 08-07 17:36 4次下载

    装配机器人姿误差建模与补偿研究

    姿精度是研究机器人的关键性能指标。国内外学者一直关注并不断研究发现多种多样的误差补偿方法如硬件补偿法、摄动补偿算法和直接修正算法对机器人的姿误差进行补偿。陈明哲与张启先采用雅可比矩
    发表于 04-20 16:56 1次下载
    装配机器人<b class='flag-5'>位</b><b class='flag-5'>姿</b>误差建模与补偿研究

    Microchip为PIC®和SAM单片机提供统一的软件开发框架_MPLAB Harmony v3

    Microchip Technology 今日宣布推出最新版本的统一软件框架MPLAB® Harmony 3.0(v3),首次为SAM MCU提供支持。
    发表于 03-25 16:50 1283次阅读

    基于自适应模糊控制方法的喷杆姿主动控制器

    基于自适应模糊控制方法的喷杆姿主动控制器
    发表于 07-01 16:11 8次下载

    一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架

    LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。实现了高精度、实时的移动机器人的轨迹估计和建图。
    的头像 发表于 10-31 09:25 2323次阅读

    如何去使用深度学习的model SLAM姿估计的自训练方法

    如何进行鲁棒的姿图优化来得到比较可靠的SLAM估计?提出了一种自动协方差调整的姿图优化,这里如果展开讲可能需要很长时间,在这边只做一个比较宏观的介绍。
    发表于 03-29 09:53 764次阅读

    常见姿估计算法的比较:三角测量、PNP、ICP

    相机标定工程用到的是DLT(直接线性变换算法) ,它是一类PnP问题 (3D-2D) 。请参考【姿估计 | 视觉SLAM| 笔记】常见姿估计算法的比较 PnP
    发表于 06-07 11:56 0次下载
    常见<b class='flag-5'>位</b><b class='flag-5'>姿</b>估计算法的比较:三角测量、PNP、ICP

    从8AVR到32SAM D21 MCU的应用程序移植

    电子发烧友网站提供《从8AVR到32SAM D21 MCU的应用程序移植.pdf》资料免费下载
    发表于 09-20 11:41 1次下载
    从8<b class='flag-5'>位</b>AVR到32<b class='flag-5'>位</b><b class='flag-5'>SAM</b> D21 MCU的应用程序移植

    从8PIC18F到32SAM D21 MCU的应用程序移植

    电子发烧友网站提供《从8PIC18F到32SAM D21 MCU的应用程序移植.pdf》资料免费下载
    发表于 09-20 11:22 0次下载
    从8<b class='flag-5'>位</b>PIC18F到32<b class='flag-5'>位</b><b class='flag-5'>SAM</b> D21 MCU的应用程序移植

    3d激光SLAMLIO-SAM框架介绍

    里程计的框架。 实现了高精度、实时的移动机器人的轨迹估计和建图。 本篇博客重点解读LIO-SAM框架下IMU预积分功能数据初始化代码部分 LIO-SAM 的代码主要在其主目录内的src
    的头像 发表于 11-22 15:04 982次阅读
    3d激光SLAMLIO-<b class='flag-5'>SAM</b><b class='flag-5'>框架</b>介绍

    LIO-SAM框架是什么

    里程计的框架。实现了高精度、实时的移动机器人的轨迹估计和建图。在之前的博客讲解了imu如何进行预积分,最终以imu的频率发布了imu的预测位姿里程计。 本篇博客主要讲解,最终是如何进行姿
    的头像 发表于 11-24 17:08 1118次阅读
    <b class='flag-5'>LIO-SAM</b><b class='flag-5'>框架</b>是什么