devbox
小蓝xlanll
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

LIO-SAM紧耦合imu阅读笔记_liosam imu因子作用

作者：小蓝xlanll | 2024-02-19 20:38:13

赞

踩

liosam imu因子作用

1.1 概述

        本文的书写参考了许多zzk大佬文章SLAM学习笔记（二十）LIO-SAM流程及代码详解（最全）以及知乎卢涛大佬的文章LIO-SAM源码解析(三)：IMUPreintegration，在二者的基础上对源代码进行了梳理，关于GTSAM图优化部分的理解基本都是在学习两位大佬的解释，有读到这篇文章并在imu紧耦合方面有志于学的读者可以多多参考两位的文章。

1.2 整体梳理

        整体梳理了一遍发现还是对图优化的部分云里雾里看不太明白，目前仅仅是对整体流程有了一点理解，这种main函数的书写方式也确实是令我耳目一新，主要起作用的部分是两个类TransformFusion类和ImuPreintegration类，前者的功能是利用高帧率的imu数据实现了对低帧率的雷达点云数据的修正，替代了一般意义上松耦合imu对雷达点云的线性外推与旋转补正功能，节约内存和计算时间，写的很好；后者的功能是整个LIO-SAM代码的精髓内容，这里引用卢涛大佬的说法，用激光里程计，两帧激光里程计之间的imu预计分量构建因子图，优化当前帧的状态（包括位姿、速度、偏置），以优化后的状态为基础，施加imu预计分量，得到每一时刻的imu里程计。这部分是mapOptmization的一个前馈修正，里面关于imu预积分和构建因子图的方式十分的精炼高效，对初学者的学习有很大的帮助。

1.3 代码阅读笔记

        

  1. #include "utility.h"
  2.  
  3. #include <gtsam/geometry/Rot3.h>
  4. #include <gtsam/geometry/Pose3.h>
  5. #include <gtsam/slam/PriorFactor.h>
  6. #include <gtsam/slam/BetweenFactor.h>
  7. #include <gtsam/navigation/GPSFactor.h>
  8. #include <gtsam/navigation/ImuFactor.h>
  9. #include <gtsam/navigation/CombinedImuFactor.h>
  10. #include <gtsam/nonlinear/NonlinearFactorGraph.h>
  11. #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
  12. #include <gtsam/nonlinear/Marginals.h>
  13. #include <gtsam/nonlinear/Values.h>
  14. #include <gtsam/inference/Symbol.h>
  15.  
  16. #include <gtsam/nonlinear/ISAM2.h>
  17. #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
  18.  
  19. using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
  20. using gtsam::symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
  21. using gtsam::symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)
  22.  
  23. //这个类主要是实现利用高帧率的imu数据对低帧率的雷达里程计进行精度修正
  24. class TransformFusion : public ParamServer
  25. {
  26. public:
  27.     std::mutex mtx;
  28.  
  29.     ros::Subscriber subImuOdometry;
  30.     ros::Subscriber subLaserOdometry;
  31.  
  32.     ros::Publisher pubImuOdometry;
  33.     ros::Publisher pubImuPath;
  34.  
  35.     //激光里程计对应旋转变换矩阵
  36.     Eigen::Affine3f lidarOdomAffine;
  37.     //imu里程计队列中最早的一帧对应旋转变换矩阵
  38.     Eigen::Affine3f imuOdomAffineFront;
  39.     //imu里程计队列中最新的一帧对应旋转变换矩阵
  40.     Eigen::Affine3f imuOdomAffineBack;
  41.  
  42.     tf::TransformListener tfListener;
  43.     tf::StampedTransform lidar2Baselink;
  44.  
  45.     double lidarOdomTime = -1;
  46.     deque<nav_msgs::Odometry> imuOdomQueue;
  47.  
  48.     TransformFusion()
  49.     {
  50.         if(lidarFrame != baselinkFrame)
  51.         {
  52.             try
  53.             {
  54.                 tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
  55.                 tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
  56.             }
  57.             catch (tf::TransformException ex)
  58.             {
  59.                 ROS_ERROR("%s",ex.what());
  60.             }
  61.         }
  62.  
  63.         subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
  64.         subImuOdometry   = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental",   2000, &TransformFusion::imuOdometryHandler,   this, ros::TransportHints().tcpNoDelay());
  65.  
  66.         pubImuOdometry   = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
  67.         pubImuPath       = nh.advertise<nav_msgs::Path>    ("lio_sam/imu/path", 1);
  68.     }
  69.  
  70.     //将里程计里面的位姿信息转化为旋转变换矩阵
  71.     Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
  72.     {   
  73.         //xyz信息直接从里程计中提取
  74.         double x, y, z, roll, pitch, yaw;
  75.         x = odom.pose.pose.position.x;
  76.         y = odom.pose.pose.position.y;
  77.         z = odom.pose.pose.position.z;
  78.         //rpy角信息先从里程计中获取四元数，利用tf库转换为3X3的矩阵，然后再转化为rpy三个角
  79.         tf::Quaternion orientation;
  80.         tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
  81.         tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
  82.         //将给定的xyzrpy角创建平移旋转变换矩阵
  83.         return pcl::getTransformation(x, y, z, roll, pitch, yaw);
  84.     }
  85.  
  86.     //雷达里程计回调函数，将位姿信息转化为旋转变换矩阵
  87.     void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
  88.     {
  89.         std::lock_guard<std::mutex> lock(mtx);
  90.         lidarOdomAffine = odom2affine(*odomMsg);
  91.         lidarOdomTime = odomMsg->header.stamp.toSec();
  92.     }
  93.  
  94.     //imu里程计回调函数，输入里程计信息 核心功能是保存imu队列，并计算imu里程计增量变换，用得到的增量旋转变换矩阵来修正雷达里程计，提高精度
  95.     void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
  96.     {
  97.         //创建一个tf发布对象tfMap2Odom，创建一个tf对象map_to_odom，初始值全部是0  这块我没理清楚，感觉里面并没有赋值的过程，一直在发0，也就是两个坐标系重合在一块？
  98.         static tf::TransformBroadcaster tfMap2Odom;
  99.         static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
  100.  
  101.         //通过sendTransform发布tf变换
  102.         //tf::StampedTransform是tf的一种特殊情况：它需要frame_id和stamp以及child_frame_id
  103.         tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
  104.  
  105.         std::lock_guard<std::mutex> lock(mtx);
  106.  
  107.         //imu里程计装入最新一帧
  108.         imuOdomQueue.push_back(*odomMsg);
  109.  
  110.         //保证雷达里程计里面有数，没有的话直接返回了，只有初始化的时候触发一次
  111.         if (lidarOdomTime == -1)
  112.             return;
  113.  
  114.         while (!imuOdomQueue.empty())
  115.         {   
  116.             //把imu里程计中，时间戳小于当前雷达里程计时间戳的数据全都pop出去
  117.             if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
  118.                 imuOdomQueue.pop_front();
  119.             else
  120.                 break;
  121.         }
  122.         //imu里程计队列中最早的一帧，也就是和当前雷达里程计时间最接近的那一帧，转化为旋转矩阵
  123.         Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
  124.         //imu里程计队列中最新的一帧，转化为旋转矩阵
  125.         Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
  126.         //矩阵运算front到back的旋转变换量，T_back = T_front * T_inc,那么T_inc = T_front逆 * T_back
  127.         Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
  128.         //矩阵运算，将雷达里程计根据imu里程计的增量作修正 T_lidar_now = T_lidar * T_inc
  129.         Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
  130.         float x, y, z, roll, pitch, yaw;
  131.         //把上面修正过的雷达里程计中的xyzrpy取出来，进行后面的运算
  132.         pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
  133.         
  134.         //把修正过的雷达里程计发布出来，初值用的是imuOdomQueue，这个我个人感觉无所谓，用空的nav_msgs::Odometry应该也可以
  135.         nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
  136.         laserOdometry.pose.pose.position.x = x;
  137.         laserOdometry.pose.pose.position.y = y;
  138.         laserOdometry.pose.pose.position.z = z;
  139.         laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
  140.         pubImuOdometry.publish(laserOdometry);
  141.  
  142.         //发布修正过后的雷达里程计到baselink的tf变换
  143.         static tf::TransformBroadcaster tfOdom2BaseLink;
  144.         //修正过后的雷达里程计装入tCur
  145.         tf::Transform tCur;
  146.         tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
  147.         if(lidarFrame != baselinkFrame)
  148.             tCur = tCur * lidar2Baselink;
  149.         tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
  150.         tfOdom2BaseLink.sendTransform(odom_2_baselink);
  151.  
  152.         //发布imu里程计path，用来在rviz实时显示轨迹，不过根据while中的删选条件能够看到显示的是一秒内的imu轨迹
  153.         static nav_msgs::Path imuPath;
  154.         static double last_path_time = -1;
  155.         double imuTime = imuOdomQueue.back().header.stamp.toSec();
  156.         if (imuTime - last_path_time > 0.1)
  157.         {
  158.             last_path_time = imuTime;
  159.             geometry_msgs::PoseStamped pose_stamped;
  160.             pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
  161.             pose_stamped.header.frame_id = odometryFrame;
  162.             pose_stamped.pose = laserOdometry.pose.pose;
  163.             imuPath.poses.push_back(pose_stamped);
  164.             while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
  165.                 imuPath.poses.erase(imuPath.poses.begin());
  166.             if (pubImuPath.getNumSubscribers() != 0)
  167.             {
  168.                 imuPath.header.stamp = imuOdomQueue.back().header.stamp;
  169.                 imuPath.header.frame_id = odometryFrame;
  170.                 pubImuPath.publish(imuPath);
  171.             }
  172.         }
  173.     }
  174. };
  175.  
  176. //这个类的主要功能是构建因子图,把imu加入其中，实现紧耦合
  177. class IMUPreintegration : public ParamServer
  178. {
  179. public:
  180.  
  181.     std::mutex mtx;
  182.  
  183.     ros::Subscriber subImu;
  184.     ros::Subscriber subOdometry;
  185.     ros::Publisher pubImuOdometry;
  186.  
  187.     bool systemInitialized = false;
  188.     
  189.     //gtsam构建的白噪声
  190.     gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
  191.     gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
  192.     gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
  193.     gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
  194.     gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;
  195.     gtsam::Vector noiseModelBetweenBias;
  196.  
  197.     //imu预积分器，用来记录两帧激光里程计之间的imu数据预积分
  198.     gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
  199.     //imu预积分器，根据最新接收到的激光雷达里程计，预测下一帧激光雷达里程计到达时的imu积分增量，用作因子图优化
  200.     gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
  201.  
  202.     std::deque<sensor_msgs::Imu> imuQueOpt;
  203.     std::deque<sensor_msgs::Imu> imuQueImu;
  204.  
  205.     gtsam::Pose3 prevPose_;
  206.     gtsam::Vector3 prevVel_;
  207.     gtsam::NavState prevState_;
  208.     gtsam::imuBias::ConstantBias prevBias_;
  209.  
  210.     gtsam::NavState prevStateOdom;
  211.     gtsam::imuBias::ConstantBias prevBiasOdom;
  212.  
  213.     bool doneFirstOpt = false;
  214.     double lastImuT_imu = -1;
  215.     double lastImuT_opt = -1;
  216.  
  217.     gtsam::ISAM2 optimizer;
  218.     gtsam::NonlinearFactorGraph graphFactors;
  219.     gtsam::Values graphValues;
  220.  
  221.     const double delta_t = 0;
  222.  
  223.     int key = 1;
  224.     
  225.     // T_bl:雷达坐标系转化到imu坐标系
  226.     gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
  227.     // T_lb:imu坐标系转化到雷达坐标系
  228.     gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
  229.  
  230.     IMUPreintegration()
  231.     {   
  232.  
  233.         subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic,                   2000, &IMUPreintegration::imuHandler,      this, ros::TransportHints().tcpNoDelay());
  234.         subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
  235.  
  236.         pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);
  237.  
  238.         boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
  239.         p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
  240.         p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
  241.         p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
  242.         gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
  243.  
  244.         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
  245.         priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
  246.         priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
  247.         correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
  248.         correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
  249.         noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
  250.         
  251.         imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
  252.         imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization        
  253.     }
  254.  
  255.     //重置ISAM优化器
  256.     void resetOptimization()
  257.     {
  258.         gtsam::ISAM2Params optParameters;
  259.         optParameters.relinearizeThreshold = 0.1;
  260.         optParameters.relinearizeSkip = 1;
  261.         optimizer = gtsam::ISAM2(optParameters);
  262.  
  263.         gtsam::NonlinearFactorGraph newGraphFactors;
  264.         graphFactors = newGraphFactors;
  265.  
  266.         gtsam::Values NewGraphValues;
  267.         graphValues = NewGraphValues;
  268.     }
  269.  
  270.     //重置参数，主要是几个回掉函数的标志位
  271.     void resetParams()
  272.     {
  273.         lastImuT_imu = -1;
  274.         doneFirstOpt = false;
  275.         systemInitialized = false;
  276.     }
  277.  
  278.     //激光里程计回调函数
  279.     void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
  280.     {
  281.         std::lock_guard<std::mutex> lock(mtx);
  282.  
  283.         double currentCorrectionTime = ROS_TIME(odomMsg);
  284.  
  285.         // make sure we have imu data to integrate
  286.         if (imuQueOpt.empty())
  287.             return;
  288.  
  289.         float p_x = odomMsg->pose.pose.position.x;
  290.         float p_y = odomMsg->pose.pose.position.y;
  291.         float p_z = odomMsg->pose.pose.position.z;
  292.         float r_x = odomMsg->pose.pose.orientation.x;
  293.         float r_y = odomMsg->pose.pose.orientation.y;
  294.         float r_z = odomMsg->pose.pose.orientation.z;
  295.         float r_w = odomMsg->pose.pose.orientation.w;
  296.         bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
  297.         gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
  298.  
  299.         //初始化
  300.         if (systemInitialized == false)
  301.         {
  302.             resetOptimization();
  303.  
  304.             //delta_t在类中声明为0，所以实质上是删除imu队列中早于激光雷达里程计时间的帧
  305.             while (!imuQueOpt.empty())
  306.             {
  307.                 if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
  308.                 {
  309.                     lastImuT_opt = ROS_TIME(&imuQueOpt.front());
  310.                     imuQueOpt.pop_front();
  311.                 }
  312.                 else
  313.                     break;
  314.             }
  315.             //雷达坐标系转化到imu坐标系
  316.             prevPose_ = lidarPose.compose(lidar2Imu);
  317.             gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
  318.             graphFactors.add(priorPose);
  319.             //速度因子
  320.             prevVel_ = gtsam::Vector3(0, 0, 0);
  321.             gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
  322.             graphFactors.add(priorVel);
  323.             //噪声因子
  324.             prevBias_ = gtsam::imuBias::ConstantBias();
  325.             gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
  326.             graphFactors.add(priorBias);
  327.             //graphValues赋值，把构建好的graphFactors赋进去
  328.             graphValues.insert(X(0), prevPose_);
  329.             graphValues.insert(V(0), prevVel_);
  330.             graphValues.insert(B(0), prevBias_);
  331.             //进行一次图优化
  332.             optimizer.update(graphFactors, graphValues);
  333.             graphFactors.resize(0);
  334.             graphValues.clear();
  335.             //reset
  336.             imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
  337.             imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
  338.             
  339.             key = 1;
  340.             systemInitialized = true;
  341.             return;
  342.         }
  343.  
  344.  
  345.         //每当前面的优化进行一百次，将imu积分器内的数全部清0，减少累计误差
  346.         if (key == 100)
  347.         {
  348.             // get updated noise before reset
  349.             gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
  350.             gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
  351.             gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
  352.             // reset graph
  353.             resetOptimization();
  354.             // add pose
  355.             gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
  356.             graphFactors.add(priorPose);
  357.             // add velocity
  358.             gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
  359.             graphFactors.add(priorVel);
  360.             // add bias
  361.             gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
  362.             graphFactors.add(priorBias);
  363.             // add values
  364.             graphValues.insert(X(0), prevPose_);
  365.             graphValues.insert(V(0), prevVel_);
  366.             graphValues.insert(B(0), prevBias_);
  367.             // optimize once
  368.             optimizer.update(graphFactors, graphValues);
  369.             graphFactors.resize(0);
  370.             graphValues.clear();
  371.  
  372.             key = 1;
  373.         }
  374.  
  375.  
  376.         //引用知乎卢涛大佬的解释，计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
  377.         while (!imuQueOpt.empty())
  378.         {
  379.             //delta_t在类中声明为0，所以实质上是删除imu队列中早于激光雷达里程计时间的帧
  380.             sensor_msgs::Imu *thisImu = &imuQueOpt.front();
  381.             double imuTime = ROS_TIME(thisImu);
  382.             if (imuTime < currentCorrectionTime - delta_t)
  383.             {
  384.                 //将imu信息的线加速度与角速度加入预积分器中
  385.                 double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
  386.                 imuIntegratorOpt_->integrateMeasurement(
  387.                         gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
  388.                         gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
  389.                 
  390.                 lastImuT_opt = imuTime;
  391.                 imuQueOpt.pop_front();
  392.             }
  393.             else
  394.                 break;
  395.         }
  396.         //imufactor中存入前一帧位姿与速度，当前帧位姿与速度，以及之前形成的imu预计分量
  397.         const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
  398.         gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
  399.         graphFactors.add(imu_factor);
  400.         //add imu bias between factor
  401.         graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
  402.                          gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
  403.         // add pose factor
  404.         gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
  405.         gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
  406.         graphFactors.add(pose_factor);
  407.         // insert predicted values
  408.         gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
  409.         graphValues.insert(X(key), propState_.pose());
  410.         graphValues.insert(V(key), propState_.v());
  411.         graphValues.insert(B(key), prevBias_);
  412.         // optimize
  413.         optimizer.update(graphFactors, graphValues);
  414.         optimizer.update();
  415.         graphFactors.resize(0);
  416.         graphValues.clear();
  417.         // Overwrite the beginning of the preintegration for the next step.
  418.         //更新位姿、速度、偏置等优化结果
  419.         gtsam::Values result = optimizer.calculateEstimate();
  420.         prevPose_  = result.at<gtsam::Pose3>(X(key));
  421.         prevVel_   = result.at<gtsam::Vector3>(V(key));
  422.         prevState_ = gtsam::NavState(prevPose_, prevVel_);
  423.         prevBias_  = result.at<gtsam::imuBias::ConstantBias>(B(key));
  424.         // Reset the optimization preintegration object.
  425.         imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
  426.         // check optimization
  427.         if (failureDetection(prevVel_, prevBias_))
  428.         {
  429.             resetParams();
  430.             return;
  431.         }
  432.  
  433.  
  434.         // 2. after optiization, re-propagate imu odometry preintegration
  435.         //这两个变量在imu回调函数里用到了，用来预测当前帧的位姿
  436.         prevStateOdom = prevState_;
  437.         prevBiasOdom  = prevBias_;
  438.         // first pop imu message older than current correction data
  439.         double lastImuQT = -1;
  440.         //引用zzk大佬的解释
  441.         //注意，这里是要“删除”当前帧“之前”的imu数据，是想根据当前帧“之后”的累积递推。
  442.         //而前面imuIntegratorOpt_做的事情是，“提取”当前帧“之前”的imu数据，用两帧之间的imu数据进行积分。处理过的就弹出来。
  443.         //因此，新到一帧激光帧里程计数据时，imuQueOpt队列变化如下：
  444.         //当前帧之前的数据被提出来做积分，用一个删一个（这样下一帧到达后，队列中就不会有现在这帧之前的数据了）
  445.         //那么在更新完以后，imuQueOpt队列不再变化，剩下的原始imu数据用作下一次优化时的数据。
  446.         //而imuQueImu队列则是把当前帧之前的imu数据都给直接剔除掉，仅保留当前帧之后的imu数据，
  447.         //用作两帧lidar里程计到达时刻之间发布的imu增量式里程计的预测。
  448.         //imuQueImu和imuQueOpt的区别要明确,imuIntegratorImu_和imuIntegratorOpt_的区别也要明确
  449.         while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
  450.         {
  451.             lastImuQT = ROS_TIME(&imuQueImu.front());
  452.             imuQueImu.pop_front();
  453.         }
  454.         // repropogate
  455.         if (!imuQueImu.empty())
  456.         {
  457.             // reset bias use the newly optimized bias
  458.             imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
  459.             // integrate imu message from the beginning of this optimization
  460.             for (int i = 0; i < (int)imuQueImu.size(); ++i)
  461.             {
  462.                 sensor_msgs::Imu *thisImu = &imuQueImu[i];
  463.                 double imuTime = ROS_TIME(thisImu);
  464.                 double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);
  465.  
  466.                 imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
  467.                                                         gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
  468.                 lastImuQT = imuTime;
  469.             }
  470.         }
  471.  
  472.         ++key;
  473.         doneFirstOpt = true;
  474.     }
  475.  
  476.     bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
  477.     {
  478.         Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
  479.         if (vel.norm() > 30)
  480.         {
  481.             ROS_WARN("Large velocity, reset IMU-preintegration!");
  482.             return true;
  483.         }
  484.  
  485.         Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
  486.         Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
  487.         if (ba.norm() > 1.0 || bg.norm() > 1.0)
  488.         {
  489.             ROS_WARN("Large bias, reset IMU-preintegration!");
  490.             return true;
  491.         }
  492.  
  493.         return false;
  494.     }
  495.  
  496.     //imu回调函数
  497.     void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
  498.     {
  499.         std::lock_guard<std::mutex> lock(mtx);
  500.         //imuConverter函数定义在utility.h文件里，把imu的数据旋转到前左上坐标系下
  501.         sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
  502.  
  503.         imuQueOpt.push_back(thisImu);
  504.         imuQueImu.push_back(thisImu);
  505.  
  506.         //odometryHandler结束标志位
  507.         if (doneFirstOpt == false)
  508.             return;
  509.  
  510.         double imuTime = ROS_TIME(&thisImu);
  511.         double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
  512.         lastImuT_imu = imuTime;
  513.  
  514.         // gtsam中的imu预积分器，将thisImu里的数据和dt存进去
  515.         imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
  516.                                                 gtsam::Vector3(thisImu.angular_velocity.x,    thisImu.angular_velocity.y,    thisImu.angular_velocity.z), dt);
  517.  
  518.         // predict odometry
  519.         gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
  520.  
  521.         //发布imu里程计
  522.         nav_msgs::Odometry odometry;
  523.         odometry.header.stamp = thisImu.header.stamp;
  524.         odometry.header.frame_id = odometryFrame;
  525.         odometry.child_frame_id = "odom_imu";
  526.  
  527.         // transform imu pose to ldiar
  528.         gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
  529.         gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
  530.  
  531.         odometry.pose.pose.position.x = lidarPose.translation().x();
  532.         odometry.pose.pose.position.y = lidarPose.translation().y();
  533.         odometry.pose.pose.position.z = lidarPose.translation().z();
  534.         odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
  535.         odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
  536.         odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
  537.         odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
  538.         
  539.         odometry.twist.twist.linear.x = currentState.velocity().x();
  540.         odometry.twist.twist.linear.y = currentState.velocity().y();
  541.         odometry.twist.twist.linear.z = currentState.velocity().z();
  542.         odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
  543.         odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
  544.         odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
  545.         pubImuOdometry.publish(odometry);
  546.     }
  547. };
  548.  
  549.  
  550. int main(int argc, char** argv)
  551. {
  552.     ros::init(argc, argv, "roboat_loam");
  553.     
  554.     IMUPreintegration ImuP;
  555.  
  556.     TransformFusion TF;
  557.  
  558.     ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
  559.     
  560.     ros::MultiThreadedSpinner spinner(4);
  561.     spinner.spin();
  562.     
  563.     return 0;
  564. }

1.4后记

        LIO-SAM中的imu紧耦合激光雷达SLAM的方式是作者Tixiao Shan做这一套系统的核心，其他部分与LeGO-LOAM并无太大区别，作者后续又在这套系统的基础上加入了视觉系统即LVI-SAM，也做的十分优秀，值得学习。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小蓝xlanll/article/detail/116688
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号