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

LIO_SAM程序实现原理学习笔记（三）--imuPreintergration.cpp_gtsam::vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).

作者：从前慢现在也慢 | 2024-02-18 00:47:47

赞

踩

gtsam::vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()

        在文章开始先介绍一下预积分环节使用的GTSAM库，GTSAM是基于因子图的c++库，可以用于slam优化环节，因子图由因子和变量组成，变量表示待估计的变量，因子表示变量之间的约束。在slam问题中，变量一般表示位姿，因子表示位姿之间的约束，可能是帧间约束，闭环约束。

 

        对于它的使用，首先要构建因子图（包括构建因子图、添加先验位姿、里程计测量噪音、相邻位姿添加里程计因子、回环检测噪声、添加回环约束因子）、设置变量初值（变量初始化）、进行优化。

        在进入程序之前，先看一下文件内函数的相互关系，虽然vs code可以跳转，但是跳来跳去非常容易把结构搞乱，在这里贴上一张大体的结构图：

        在贴上程序注释前，把需要请提前注意的几个点放在前面，以便于读到这里时可以更有针对性的阅读。

        1、 首先是IMU与Lidar坐标系之间的变换问题，在头文件中定义了一个imuConverter函数，这个函数内只包含了旋转变化，将imu坐标系变换到了lidar坐标系。在本文件中，还包含了imu2lidar和lidar2imu两个变换（这里把to记作2哈哈哈学到了），乍一看这两个变换似乎是两个坐标系之间的转换矩阵。但事实是这两个变换只包含平移。流程是这样的，作者将imu通过旋转转至雷达坐标系下，但是两个坐标系之间缺一个平移。通过lidar2imu变换将两个坐标系对齐。最后通过imu2lidar将两个坐标系最终转至lidar系下。

        2、在储存imu数据的时候，有imuQueImu和imuQueOpt两个队列。这两个队列的作用是不同的，从结果来说imuQueImu是后面程序需要用到的数据，而imuQueOpt储存的是用于优化的缓存数据。从过程来说，每当lidar里程计新到一帧时，imuQueImu会删除此帧前的数据，只保留之后的数据用于两帧lidar里程计间发布imu增量式里程计；而imuQueOpt将此帧前的数据提取出来做积分，边用边删，数据用于后续优化过程。

        建议从main函数按程序运行顺序来看下面的注释：

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

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

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

  

闽ICP备14008679号