IMU Preintegration Started.�33[0m" ); // 打印消息 ros::MultiThreadedSpinner spinner( 4 ); // 开四个线程 通过并发的方式使得速度得到提升 spinner.spin(); // 程序执行到这个地方 则等待 topic 回调函数执行 return 0 ;} main函数部分很简洁,功能主要完成部分都在定义的两个类中进行。 在main函数中进行 节点初始" />
0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看威廉希尔官方网站 视频
  • 写文章/发帖/加入社区
会员中心
创作中心

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

3天内不再提示

IMU预积分功能数据初始化代码解读

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

代码解读

int main(int argc, char** argv){    ros::init(argc, argv, "roboat_loam");    IMUPreintegration ImuP;//IMUPreintegration 类的实例    TransformFusion TF;//TransformFusion 类的实例    ROS_INFO("�33[1;32m---- > IMU Preintegration Started.�33[0m");//打印消息    ros::MultiThreadedSpinner spinner(4);//开四个线程 通过并发的方式使得速度得到提升    spinner.spin();//程序执行到这个地方 则等待 topic 回调函数执行    return 0;}

main函数部分很简洁,功能主要完成部分都在定义的两个类中进行。

在main函数中进行

  • 节点初始化
  • IMUPreintegration 类的实例
  • TransformFusion 类的实例
  • 打印消息
  • 开四个线程 通过并发的方式使得速度得到提升
  • 等待 topic 回调函数执行
  • 之后则看 IMUPreintegration 这个类,先看构造函数部分

在里面首先进行了 订阅imu信息

subImu      = nh.subscribe< sensor_msgs::Imu >  (imuTopic,2000, &IMUPreintegration::imuHandler,this, ros::TransportHints().tcpNoDelay());

imuTopic 为 imu_correct, imu原始数据,这个imuTopic是从参数服务器读取的,具体的配置在prams.yaml中

图片

如果你的imu的topic和默认的不一致则需要修改

然后可以看其具体的回调函数 imuHandler

void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)    {        std::lock_guard< std::mutex > lock(mtx);        //首先把imu的状态做一个简单的转换        sensor_msgs::Imu thisImu = imuConverter(*imu_raw);        // 注意这里有两个imu队列,作用不相同,一个用来执行预积分和位姿变换的优化,一个用来更新最新imu状态          imuQueOpt.push_back(thisImu);        imuQueImu.push_back(thisImu);        // 如果没有发生过优化 则 return        if (doneFirstOpt == false)            return;

回调函数先把imu的状态做一个简单的转换,转到lidar坐标系 下

将转换后的imu数据存入两个队列中,注意这里有两个imu队列,作用不相同,一个用来执行预积分和位姿变换的优化,一个用来更新最新imu状态

如果没有发生过优化 则 retur,doneFirstOpt这个标志位,在接受到帧间里程计信息后,则至为true,imuConverter函数在utility.h文件中

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()) &lt; 0.1)        {            ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");            ros::shutdown();        }        return imu_out;//返回变换后的imu数据    }};

进行加速度坐标旋转、进行陀螺仪坐标旋转、进行姿态角坐标旋转、检测姿态数据是否正常、返回变换后的imu数据

在进行加速度和陀螺仪变换的时候,使用的是extRot,该参数的根源来源于prams.yaml中

图片

进行姿态角坐标旋转,使用的是extQRPY,该参数的根源来源于prams.yaml中

图片

所有终于明白为什么在配置文件中有两个外参了!

imuHandler这个回调函数,先看到这部分,后面的之后再看,需要回到上面的IMUPreintegration的构造函数,看订阅到帧间里程计信息做了哪些事情。

subOdometry = nh.subscribe< nav_msgs::Odometry >("lio_sam/mapping/odometry_incremental", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());

订阅雷达里程计信息,lio_sam/mapping/odometry_incremental 是mapOptmization发出的,odometryHandler回调函数,走起

double currentCorrectionTime = ROS_TIME(odomMsg);

通过ROS_TIME函数把消息中的时间戳取了出来

if (imuQueOpt.empty())            return;

保证imu队列中有数据

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;

该位姿是否出现退化 pose.covariance[0] 为1 则 雷达里程计有退化风险,该帧位姿精度有一定程序下降

gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));

把位姿转成 gtsam的格式,进入系统的初始化,下面部分仅执行一次

resetOptimization();

在函数内部 初始化gtsam的一些量

while (!imuQueOpt.empty())            {                if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)                {                    lastImuT_opt = ROS_TIME(&imuQueOpt.front());                    imuQueOpt.pop_front();                }                else                    break;            }

将这个雷达里程计之前的imu信息全部扔掉,整个LIO-SAM中作者对时间同步这块的思想都是这样的,保证imu与odometry消息时间同步 因为imu是高频数据所以这是必要的

prevPose_ = lidarPose.compose(lidar2Imu);

将lidar的位姿移到imu坐标系下,lidar2Imu 是lidar到imu的外参,compose是gtsam的一个功能函数,VIO和LIO的框架都在在IMU坐标系下进行的

gtsam::PriorFactor< gtsam::Pose3 > priorPose(X(0), prevPose_, priorPoseNoise);            graphFactors.add(priorPose);

设置其初始位姿和置信度,约束加入到因子中

  • gtsam::PriorFactor 模块涉及到的变量结点
  • gtsam::Pose3 表示六自由度位姿
  • gtsam::Vector3 表示三自由度速度
  • gtsam::imuBias::ConstantBias 表示IMU零偏

以上也是预积分模型中涉及到的三种状态变量

gtsam::PriorFactor 为先验因子,表示对某个状态量T的一个先验估计,约束某个状态变量的状态不会离该先验值过远。

其中的X(0)的,初始定义如下。事先的符号

图片

priorPoseNoise 是先验位姿的噪声,该值为

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

初始 位姿 置信度 设置 比较高 后面构成协方差矩阵 值越小 表示 置信度越高

prevVel_ = gtsam::Vector3(0, 0, 0);            gtsam::PriorFactor< gtsam::Vector3 > priorVel(V(0), prevVel_, priorVelNoise);               graphFactors.add(priorVel);

和上面位姿基本一样初始化速度,这里直接赋 0 了,将速度约束加到因子图中,其中priorVelNoise 速度的噪声是

priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s

初始化速度 置信度 设置 差些 因为速度一开始设置的是0,不知道是多少

prevBias_ = gtsam::imuBias::ConstantBias();            gtsam::PriorFactor< gtsam::imuBias::ConstantBias > priorBias(B(0), prevBias_, priorBiasNoise);                  graphFactors.add(priorBias);

初始化IMU 零偏 ,将零偏约束加到因子图中,gtsam::imuBias::ConstantBias()是gtsam做好的一个imu零偏,其中都是0,所以对应bias的噪声置信度也要设置的高些

priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good

以上把约束加入完毕,下面就开始添加状态量

graphValues.insert(X(0), prevPose_);            graphValues.insert(V(0), prevVel_);            graphValues.insert(B(0), prevBias_);

给各个状态量赋成初始值

optimizer.update(graphFactors, graphValues);

约束和状态量更新 进isam优化器

graphFactors.resize(0);            graphValues.clear();

进优化器之后 保存约束和状态量的变量就清零

imuIntegratorImu_-&gt;resetIntegrationAndSetBias(prevBias_);            imuIntegratorOpt_-&gt;resetIntegrationAndSetBias(prevBias_);

预积分的接口,使用初始零偏进行初始化 之前imu有两个队列,每个队列对应预积分处理器

key = 1;
            systemInitialized = true;//系统初始化完成
            return;

系统初始化完成

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

    关注

    3

    文章

    4329

    浏览量

    62578
  • 代码
    +关注

    关注

    30

    文章

    4780

    浏览量

    68540
  • 激光雷达
    +关注

    关注

    968

    文章

    3971

    浏览量

    189859
  • IMU
    IMU
    +关注

    关注

    6

    文章

    311

    浏览量

    45731
  • 3D激光
    +关注

    关注

    0

    文章

    30

    浏览量

    7468
收藏 人收藏

    评论

    相关推荐

    LINUX系统引导和初始化-LINUX内核解读

    Linux 的系统引导和初始化 ----------Linux2.4.22内核解读之一 一、 系统引导和初始化概述 相关代码(引导扇区的程序及其辅助程序,以 x86体系为例): \li
    发表于 11-03 22:31 53次下载

    RM68171配BOE3.97玻璃的初始化代码

    RM68171配BOE3.97玻璃的初始化代码,测试OK,确定没问题!
    发表于 06-17 17:07 2次下载

    使用QCS生成DPAA初始化代码

    本文档指导用户如何使用从qc DPAA生成的代码,用户应用程序的上下文中利用USDPAA司机。该文档解释了反射器应用程序,并演示了帮助用户快速插入QCS的步骤,这些插件在应用程序中生成DPAA初始化代码
    发表于 09-07 17:21 0次下载
    使用QCS生成DPAA<b class='flag-5'>初始化</b><b class='flag-5'>代码</b>

    如何排除代码编写器Studio 2和2.10的初始化问题

    此应用程序报告使读者熟悉XDSPo探测实用程序。这个实用程序可能是用于排除代码编写器Studio 2和2.10的初始化问题通常表现为指示目标DSP的错误消息。无法初始化。此问题可能是由于配置或硬件不正确造成的。
    发表于 04-25 09:59 4次下载
    如何排除<b class='flag-5'>代码</b>编写器Studio 2和2.10的<b class='flag-5'>初始化</b>问题

    8259a初始化的步骤及代码介绍

    本文首先介绍了8259a工作初始化的步骤及程序,其次介绍了通过OCW对8259A进行操作方法,最后介绍了8259A初始化编程。
    的头像 发表于 05-23 14:24 3w次阅读
    8259a<b class='flag-5'>初始化</b>的步骤及<b class='flag-5'>代码</b>介绍

    lcd NT35512的手册及初始化代码免费下载

    本文档的主要内容详细介绍的是lcd NT35512的手册及初始化代码免费下载
    发表于 06-29 08:00 95次下载
    lcd NT35512的手册及<b class='flag-5'>初始化</b><b class='flag-5'>代码</b>免费下载

    IIC总线初始化基本驱动参考程序应用代码免费下载

    本文档的主要内容详细介绍的是IIC总线初始化基本驱动参考程序应用代码免费下载。程序功能:为IIC总线的基本驱动程序(此程序没有写主函数,只是一些IIC必备的子函数)
    发表于 01-16 08:00 5次下载
    IIC总线<b class='flag-5'>初始化</b>基本驱动参考程序应用<b class='flag-5'>代码</b>免费下载

    STM32执行代码初始化卡住,或者上电卡住,或者复位卡住,导致代码不执行

    STM32的板子上电或者复位,接有显示屏或者LED指示灯的都会卡住解决:1、检查自己的代码是否有中断,有中断的话,其初始化放在其他硬件初始化之后 即:中断的初始化放在进入while
    发表于 12-09 09:21 22次下载
    STM32执行<b class='flag-5'>代码</b><b class='flag-5'>初始化</b>卡住,或者上电卡住,或者复位卡住,导致<b class='flag-5'>代码</b>不执行

    BF533 AD73360初始化程序代码

    BF533 AD73360初始化程序代码源码分享
    发表于 10-08 14:59 0次下载

    初始化的if和switch语句详解

    在上面的代码中,初始化语句是int s = check()。s的生命周期是整个if语句,这里也包含else语句。
    的头像 发表于 10-14 10:50 1376次阅读

    DB2163_STM32配置和初始化C代码生成

    DB2163_STM32配置和初始化C代码生成
    发表于 11-23 20:29 0次下载
    DB2163_STM32配置和<b class='flag-5'>初始化</b>C<b class='flag-5'>代码</b>生成

    使用STM32CubeMX生成初始化代码

    我使用STM32CubeMX生成初始化代码,使用LL库,这里只介绍跟i2c相关的部分,其他必要的初始化需要自己完成。芯片使用stm32f042。本文的代码不能到手即用,只提供思路。
    的头像 发表于 03-22 15:26 2977次阅读

    CPU CACHE策略的初始化

    build_mem_type_table()函数的功能是获取当前CPU的CACHE类型,据此初始化mem_type。
    的头像 发表于 06-05 15:03 1419次阅读
    CPU CACHE策略的<b class='flag-5'>初始化</b>

    rt-thread线程栈初始化参数分析

    RT-Thread 在线程初始化代码内有一段初始化线程堆栈的代码
    的头像 发表于 08-14 16:50 1722次阅读
    rt-thread线程栈<b class='flag-5'>初始化</b>参数分析

    MCU单片机GPIO初始化该按什么顺序配置?为什么初始化时有电平跳变?

    GPIO初始化时有时钟配置、模式配置、输出配置、复用配置,那么在编写初始化代码时,到底该按什么顺序执行呢?如果顺序不当那初始化过程可能会出现短暂的电平跳变。
    的头像 发表于 02-22 11:07 1528次阅读
    MCU单片机GPIO<b class='flag-5'>初始化</b>该按什么顺序配置?为什么<b class='flag-5'>初始化</b>时有电平跳变?