赞
踩
这个cpp文件主要有两个类,一个叫IMUPreintegration类,一个叫TransformFusion类。
现在我们分开讲,先说IMUPreintegration类。
关于IMU原始数据,送入imuhandle中:
imu原始数据,会先被坐标系转换,通过调用头文件里的imuConverter函数,转换到一个“雷达中间系”中,其实和真正的雷达系差了一个平移。
转换后,会存入两个队列,一个imuQueOpt队列,一个imuQueImu队列。这两队列有什么区别和联系呢?这个主要在另一个回调函数odometryHandler会被处理,在那个地方我会讲。这里我们可以先理解为,imuQueImu是真正我们要用的数据,imuQueOpt是一个中间缓存的数据结构,用来优化imu的bias之类的东西。
在标志位doneFirstOpt为True的时候(注意这个标志位,这是一个很重要的变量,之后会再提到),每到一个imu数据,就用imuIntegratorImu_这个预积分器,把这一帧imu数据累积进去,然后预测当前时刻的状态:currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 其中prevStateOdom就是之前系统所保持的状态。
把currentState,通过imu2Lidar,从“中间系”给平移到真正的雷达系,然后发布出去。发布的话题就叫odometry/imu_incremental,这也就是imageProjection.cpp的总结部分的第2点部分提到的“imu”里程计。
这部分订阅的是/mapping/odometry_incremental,这个话题是由mapOptmization.cpp发布的,可以把它理解为激光里程计。同理,也不要被incremental误导,觉得好像是两帧激光之间的变换,可不是这样的啊。它和imu里程计性质类似,就是相对世界坐标系的位姿。
从imuQueOpt队列里,删掉当前这帧激光里程计时间上之前的数据,然后把雷达的pose变换到“中间系”,保存为prevPose。图优化部分,加入乱七八糟各种东西,priorVel,priorBias,把两个预积分器imuIntegratorImu_,imuIntegratorOpt_给reset一下。(之后简称imu积分器和opt积分器)
这两个预积分器opt积分器和imu积分器有什么区别呢?马上就要讲,在1.2.3部分。
100帧激光里程计数据了,就把优化器给reset一下(用前面的先验协方差给reset一下)。注意,1.2.1和1.2.2的主要区别在于,1.2.1中的乱七八糟协方差数据,是用构造函数中写入的一大堆(我认为是经验值),而1.2.2这里的协方差,用的是optimizer.marginalCovariance这个轮子算出来的先验协方差。
假设数据如下分布:
之前imu数据 ——————第一帧开始——————第二帧开始————之后imu数据
把“第一帧开始”——“第二帧开始”这个之间的imu数据拿出来,送入opt积分器。这样得到二者之间的预积分,构建imu因子。
然后把Xkey-1 到Xkey之间,加入这个imu因子以及 激光里程计提供的pose因子,整体做一个优化。优化的结果就是bias,以及“第二帧开始”这个时刻的系统位姿。
把优化的结果(主要是bias),重置opt积分器和imu积分器。 然后把当前帧(上图的“第二帧开始”)之前的数据给删掉,用imu积分器,从“第二帧开始”这里开始往后积分。(我们需要明确一点,在这个处理过程中,imu队列也在持续的进数据,(即1.1的imuhandle中)),这里处理完,那么就置donefirst=True,这样1.1.3部分,就可以无缝衔接接着在这里的基础上对imu积分器继续积分。(可以看出,这点处理,Tixiaoshan还是做的比较牛的)
回顾:在1.1.3部分,发布imu里程计,在这里我们可以的出结果,它并非是纯粹的imu里程计,因为时不时是要加入激光里程计的信息做因子来优化得到imu的bias等信息的。
这部分监听的是/mapping/odometry,(也就是激光雷达里程计)这个回调函数比较特殊,它并没有把雷达里程计的东西再像别的回调函数一样,时刻存到什么队列里去。而是就保存当前的雷达里程计的数据到lidarOdomAffine里面,把时间戳存到lidarOdomTime里面去。
注意,这里/mapping/odometry和/mapping/odometry_incremental有什么区别呢?我认为本质上区别不大,前者是激光里程计部分直接优化得到的激光位姿,后者相当于激光的位姿经过旋转约束和z轴约束的限制以后,又和原始imu信息里面的角度做了一个加权以后的位姿。
这部分监听的是/imu/odometry_incremental(也就是上面我一直在说的imu里程计),把imu里程计放到imuodomQueue里面保存,然后把lidarOdomTime之前的数据扔掉,用imu里程计的两时刻差异,再加上lidarOdomAffine的基础进行发布。
实际上,/imu/odometry_incremental本身就是雷达里程计基础上imu数据上的发布,而在现在这里,也是雷达里程计在“imu里程计”上的一个“再次精修”。最后会发布两个内容,一个是odometry/imu,但是这个实际上是无人监听的,我觉得作者主要是发布tf变换,顺手给它发布了。当然我觉得用户可以监听这个数据,我觉得这个应该是频率上最高的一个里程计数据了。
另外还会发布一个path,用来rviz显示,名字叫lio-sam/imu/path。
- #include "utility.h"
-
- #include <gtsam/geometry/Rot3.h>
- #include <gtsam/geometry/Pose3.h>
- #include <gtsam/slam/PriorFactor.h>
- #include <gtsam/slam/BetweenFactor.h>
- #include <gtsam/navigation/GPSFactor.h>
- #include <gtsam/navigation/ImuFactor.h>
- #include <gtsam/navigation/CombinedImuFactor.h>
- #include <gtsam/nonlinear/NonlinearFactorGraph.h>
- #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
- #include <gtsam/nonlinear/Marginals.h>
- #include <gtsam/nonlinear/Values.h>
- #include <gtsam/inference/Symbol.h>
-
- #include <gtsam/nonlinear/ISAM2.h>
- #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
-
- using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
- using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot)
- using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
-
- class TransformFusion : public ParamServer
- {
- public:
- std::mutex mtx;
-
- ros::Subscriber subImuOdometry;
- ros::Subscriber subLaserOdometry;
-
- ros::Publisher pubImuOdometry;
- ros::Publisher pubImuPath;
-
- Eigen::Affine3f lidarOdomAffine;
- Eigen::Affine3f imuOdomAffineFront;
- Eigen::Affine3f imuOdomAffineBack;
-
- tf::TransformListener tfListener;
- tf::StampedTransform lidar2Baselink;
-
- double lidarOdomTime = -1;
- deque<nav_msgs::Odometry> imuOdomQueue;
-
- /**
- * 构造函数
- */
- TransformFusion()
- {
- // 如果lidar系与baselink系不同(激光系和载体系),需要外部提供二者之间的变换关系
- if(lidarFrame != baselinkFrame)
- {
- try
- {
- // 等待3s
- tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
- // lidar系到baselink系的变换
- tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
- }
- catch (tf::TransformException ex)
- {
- ROS_ERROR("%s",ex.what());
- }
- }
-
- // 订阅激光里程计,来自mapOptimization
- subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
- // 订阅imu里程计,来自IMUPreintegration
- subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
-
- // 发布imu里程计,用于rviz展示
- pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
- // 发布imu里程计轨迹
- pubImuPath = nh.advertise<nav_msgs::Path> ("lio_sam/imu/path", 1);
- }
-
- /**
- * 里程计对应变换矩阵
- */
- Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
- {
- double x, y, z, roll, pitch, yaw;
- x = odom.pose.pose.position.x;
- y = odom.pose.pose.position.y;
- z = odom.pose.pose.position.z;
- tf::Quaternion orientation;
- tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
- tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
- return pcl::getTransformation(x, y, z, roll, pitch, yaw);
- }
-
- /**
- * 订阅激光里程计,来自mapOptimization
- */
- void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
- {
- std::lock_guard<std::mutex> lock(mtx);
- // 激光里程计对应变换矩阵
- lidarOdomAffine = odom2affine(*odomMsg);
- // 激光里程计时间戳
- lidarOdomTime = odomMsg->header.stamp.toSec();
- }
-
- /**
- * 订阅imu里程计,来自IMUPreintegration
- * 1、以最近一帧激光里程计位姿为基础,计算该时刻与当前时刻间imu里程计增量位姿变换,相乘得到当前时刻imu里程计位姿
- * 2、发布当前时刻里程计位姿,用于rviz展示;发布imu里程计路径,注:只是最近一帧激光里程计时刻与当前时刻之间的一段
- */
- void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
- {
- // 发布tf,map与odom系设为同一个系
- static tf::TransformBroadcaster tfMap2Odom;
- static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
- tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
-
- std::lock_guard<std::mutex> lock(mtx);
-
- // 添加imu里程计到队列
- imuOdomQueue.push_back(*odomMsg);
-
- // 从imu里程计队列中删除当前(最近的一帧)激光里程计时刻之前的数据
- if (lidarOdomTime == -1)
- return;
- while (!imuOdomQueue.empty())
- {
- if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
- imuOdomQueue.pop_front();
- else
- break;
- }
- // 最近的一帧激光里程计时刻对应imu里程计位姿
- Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
- // 当前时刻imu里程计位姿
- Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
- // imu里程计增量位姿变换
- Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
- // 最近的一帧激光里程计位姿 * imu里程计增量位姿变换 = 当前时刻imu里程计位姿
- Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
- 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);
- // 发布tf,当前时刻odom与baselink系变换关系
- static tf::TransformBroadcaster tfOdom2BaseLink;
- tf::Transform tCur;
- tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
- if(lidarFrame != baselinkFrame)
- tCur = tCur * lidar2Baselink;
- tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
- tfOdom2BaseLink.sendTransform(odom_2_baselink);
-
- // 发布imu里程计路径,注:只是最近一帧激光里程计时刻与当前时刻之间的一段
- static nav_msgs::Path imuPath;
- static double last_path_time = -1;
- double imuTime = imuOdomQueue.back().header.stamp.toSec();
- // 每隔0.1s添加一个
- 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);
- // 删除最近一帧激光里程计时刻之前的imu里程计
- while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
- imuPath.poses.erase(imuPath.poses.begin());
- if (pubImuPath.getNumSubscribers() != 0)
- {
- imuPath.header.stamp = imuOdomQueue.back().header.stamp;
- imuPath.header.frame_id = odometryFrame;
- pubImuPath.publish(imuPath);
- }
- }
- }
- };
-
- class IMUPreintegration : public ParamServer
- {
- public:
-
- std::mutex mtx;
-
- // 订阅与发布
- ros::Subscriber subImu;
- ros::Subscriber subOdometry;
- ros::Publisher pubImuOdometry;
-
- bool systemInitialized = false;
-
- // 噪声协方差
- gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
- gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
- gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
- gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
- gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;
- gtsam::Vector noiseModelBetweenBias;
-
- // imu预积分器
- gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
- gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
-
- // imu数据队列
- std::deque<sensor_msgs::Imu> imuQueOpt;
- std::deque<sensor_msgs::Imu> imuQueImu;
-
- // imu因子图优化过程中的状态变量
- gtsam::Pose3 prevPose_;
- gtsam::Vector3 prevVel_;
- gtsam::NavState prevState_;
- gtsam::imuBias::ConstantBias prevBias_;
-
- // imu状态
- gtsam::NavState prevStateOdom;
- gtsam::imuBias::ConstantBias prevBiasOdom;
-
- bool doneFirstOpt = false;
- double lastImuT_imu = -1;
- double lastImuT_opt = -1;
-
- // ISAM2优化器
- gtsam::ISAM2 optimizer;
- gtsam::NonlinearFactorGraph graphFactors;
- gtsam::Values graphValues;
-
- const double delta_t = 0;
-
- int key = 1;
-
- // imu-lidar位姿变换
- gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
- gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
-
- /**
- * 构造函数
- */
- IMUPreintegration()
- {
- // 订阅imu原始数据,用下面因子图优化的结果,施加两帧之间的imu预计分量,预测每一时刻(imu频率)的imu里程计
- subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
- // 订阅激光里程计,来自mapOptimization,用两帧之间的imu预计分量构建因子图,优化当前帧位姿(这个位姿仅用于更新每时刻的imu里程计,以及下一次因子图优化)
- subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
-
- // 发布imu里程计
- pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);
-
- // imu预积分的噪声协方差
- boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
- p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
- p->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
- p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
- gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
-
- // 噪声先验
- 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
- priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
- priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
- // 激光里程计scan-to-map优化过程中发生退化,则选择一个较大的协方差
- 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
- correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
- noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
-
- // imu预积分器,用于预测每一时刻(imu频率)的imu里程计(转到lidar系了,与激光里程计同一个系)
- imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
- // imu预积分器,用于因子图优化
- imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
- }
-
- /**
- * 重置ISAM2优化器
- */
- void resetOptimization()
- {
- gtsam::ISAM2Params optParameters;
- optParameters.relinearizeThreshold = 0.1;
- optParameters.relinearizeSkip = 1;
- optimizer = gtsam::ISAM2(optParameters);
-
- gtsam::NonlinearFactorGraph newGraphFactors;
- graphFactors = newGraphFactors;
-
- gtsam::Values NewGraphValues;
- graphValues = NewGraphValues;
- }
-
- /**
- * 重置参数
- */
- void resetParams()
- {
- lastImuT_imu = -1;
- doneFirstOpt = false;
- systemInitialized = false;
- }
-
- /**
- * 订阅激光里程计,来自mapOptimization
- * 1、每隔100帧激光里程计,重置ISAM2优化器,添加里程计、速度、偏置先验因子,执行优化
- * 2、计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量,用前一帧状态施加预积分量得到当前帧初始状态估计,添加来自mapOptimization的当前帧位姿,进行因子图优化,更新当前帧状态
- * 3、优化之后,执行重传播;优化更新了imu的偏置,用最新的偏置重新计算当前激光里程计时刻之后的imu预积分,这个预积分用于计算每时刻位姿
- */
- void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
- {
- std::lock_guard<std::mutex> lock(mtx);
- // 当前帧激光里程计时间戳
- double currentCorrectionTime = ROS_TIME(odomMsg);
-
- // 确保imu优化队列中有imu数据进行预积分
- if (imuQueOpt.empty())
- return;
-
- // 当前帧激光位姿,来自scan-to-map匹配、因子图优化后的位姿
- 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;
- gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
-
-
- // 0. 系统初始化,第一帧
- if (systemInitialized == false)
- {
- // 重置ISAM2优化器
- resetOptimization();
-
- // 从imu优化队列中删除当前帧激光里程计时刻之前的imu数据
- while (!imuQueOpt.empty())
- {
- if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
- {
- lastImuT_opt = ROS_TIME(&imuQueOpt.front());
- imuQueOpt.pop_front();
- }
- else
- break;
- }
- // 添加里程计位姿先验因子
- prevPose_ = lidarPose.compose(lidar2Imu);
- gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
- graphFactors.add(priorPose);
- // 添加速度先验因子
- prevVel_ = gtsam::Vector3(0, 0, 0);
- gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
- graphFactors.add(priorVel);
- // 添加imu偏置先验因子
- prevBias_ = gtsam::imuBias::ConstantBias();
- gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
- graphFactors.add(priorBias);
- // 变量节点赋初值
- graphValues.insert(X(0), prevPose_);
- graphValues.insert(V(0), prevVel_);
- graphValues.insert(B(0), prevBias_);
- // 优化一次
- optimizer.update(graphFactors, graphValues);
- graphFactors.resize(0);
- graphValues.clear();
-
- // 重置优化之后的偏置
- imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
- imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
-
- key = 1;
- systemInitialized = true;
- return;
- }
-
-
- // 每隔100帧激光里程计,重置ISAM2优化器,保证优化效率
- if (key == 100)
- {
- // 前一帧的位姿、速度、偏置噪声模型
- gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
- gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
- gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
- // 重置ISAM2优化器
- resetOptimization();
- // 添加位姿先验因子,用前一帧的值初始化
- gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
- graphFactors.add(priorPose);
- // 添加速度先验因子,用前一帧的值初始化
- gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
- graphFactors.add(priorVel);
- // 添加偏置先验因子,用前一帧的值初始化
- gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
- graphFactors.add(priorBias);
- // 变量节点赋初值,用前一帧的值初始化
- graphValues.insert(X(0), prevPose_);
- graphValues.insert(V(0), prevVel_);
- graphValues.insert(B(0), prevBias_);
- // 优化一次
- optimizer.update(graphFactors, graphValues);
- graphFactors.resize(0);
- graphValues.clear();
-
- key = 1;
- }
-
-
- // 1. 计算前一帧与当前帧之间的imu预积分量,用前一帧状态施加预积分量得到当前帧初始状态估计,添加来自mapOptimization的当前帧位姿,进行因子图优化,更新当前帧状态
- while (!imuQueOpt.empty())
- {
- // 提取前一帧与当前帧之间的imu数据,计算预积分
- sensor_msgs::Imu *thisImu = &imuQueOpt.front();
- double imuTime = ROS_TIME(thisImu);
- if (imuTime < currentCorrectionTime - delta_t)
- {
- // 两帧imu数据时间间隔
- double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
- // imu预积分数据输入:加速度、角速度、dt
- imuIntegratorOpt_->integrateMeasurement(
- gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
- gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
-
- lastImuT_opt = imuTime;
- // 从队列中删除已经处理的imu数据
- imuQueOpt.pop_front();
- }
- else
- break;
- }
- // 添加imu预积分因子
- const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
- // 参数:前一帧位姿,前一帧速度,当前帧位姿,当前帧速度,前一帧偏置,预计分量
- gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
- graphFactors.add(imu_factor);
- // 添加imu偏置因子,前一帧偏置,当前帧偏置,观测值,噪声协方差;deltaTij()是积分段的时间
- graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
- gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
- // 添加位姿因子
- gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
- gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
- graphFactors.add(pose_factor);
- // 用前一帧的状态、偏置,施加imu预计分量,得到当前帧的状态
- gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
- // 变量节点赋初值
- graphValues.insert(X(key), propState_.pose());
- graphValues.insert(V(key), propState_.v());
- graphValues.insert(B(key), prevBias_);
- // 优化
- optimizer.update(graphFactors, graphValues);
- optimizer.update();
- graphFactors.resize(0);
- graphValues.clear();
- // 优化结果
- gtsam::Values result = optimizer.calculateEstimate();
- // 更新当前帧位姿、速度
- prevPose_ = result.at<gtsam::Pose3>(X(key));
- prevVel_ = result.at<gtsam::Vector3>(V(key));
- // 更新当前帧状态
- prevState_ = gtsam::NavState(prevPose_, prevVel_);
- // 更新当前帧imu偏置
- prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
- // 重置预积分器,设置新的偏置,这样下一帧激光里程计进来的时候,预积分量就是两帧之间的增量
- imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
-
- // imu因子图优化结果,速度或者偏置过大,认为失败
- if (failureDetection(prevVel_, prevBias_))
- {
- // 重置参数
- resetParams();
- return;
- }
-
-
- // 2. 优化之后,执行重传播;优化更新了imu的偏置,用最新的偏置重新计算当前激光里程计时刻之后的imu预积分,这个预积分用于计算每时刻位姿
- prevStateOdom = prevState_;
- prevBiasOdom = prevBias_;
- // 从imu队列中删除当前激光里程计时刻之前的imu数据
- double lastImuQT = -1;
- while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
- {
- lastImuQT = ROS_TIME(&imuQueImu.front());
- imuQueImu.pop_front();
- }
- // 对剩余的imu数据计算预积分
- if (!imuQueImu.empty())
- {
- // 重置预积分器和最新的偏置
- imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
- // 计算预积分
- for (int i = 0; i < (int)imuQueImu.size(); ++i)
- {
- sensor_msgs::Imu *thisImu = &imuQueImu[i];
- double imuTime = ROS_TIME(thisImu);
- double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);
-
- imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
- gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
- lastImuQT = imuTime;
- }
- }
-
- ++key;
- doneFirstOpt = true;
- }
-
- /**
- * imu因子图优化结果,速度或者偏置过大,认为失败
- */
- bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
- {
- Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
- if (vel.norm() > 30)
- {
- ROS_WARN("Large velocity, reset IMU-preintegration!");
- return true;
- }
-
- Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
- Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
- if (ba.norm() > 1.0 || bg.norm() > 1.0)
- {
- ROS_WARN("Large bias, reset IMU-preintegration!");
- return true;
- }
-
- return false;
- }
-
- /**
- * 订阅imu原始数据
- * 1、用上一帧激光里程计时刻对应的状态、偏置,施加从该时刻开始到当前时刻的imu预计分量,得到当前时刻的状态,也就是imu里程计
- * 2、imu里程计位姿转到lidar系,发布里程计
- */
- void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
- {
- std::lock_guard<std::mutex> lock(mtx);
- // imu原始测量数据转换到lidar系,加速度、角速度、RPY
- sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
-
- // 添加当前帧imu数据到队列
- imuQueOpt.push_back(thisImu);
- imuQueImu.push_back(thisImu);
-
- // 要求上一次imu因子图优化执行成功,确保更新了上一帧(激光里程计帧)的状态、偏置,预积分重新计算了
- if (doneFirstOpt == false)
- return;
-
- double imuTime = ROS_TIME(&thisImu);
- double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
- lastImuT_imu = imuTime;
-
- // imu预积分器添加一帧imu数据,注:这个预积分器的起始时刻是上一帧激光里程计时刻
- imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
- gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);
-
- // 用上一帧激光里程计时刻对应的状态、偏置,施加从该时刻开始到当前时刻的imu预计分量,得到当前时刻的状态
- gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
-
- // 发布imu里程计(转到lidar系,与激光里程计同一个系)
- nav_msgs::Odometry odometry;
- odometry.header.stamp = thisImu.header.stamp;
- odometry.header.frame_id = odometryFrame;
- odometry.child_frame_id = "odom_imu";
-
- // 变换到lidar系
- gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
- gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
-
- odometry.pose.pose.position.x = lidarPose.translation().x();
- odometry.pose.pose.position.y = lidarPose.translation().y();
- odometry.pose.pose.position.z = lidarPose.translation().z();
- odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
- odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
- odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
- odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
-
- odometry.twist.twist.linear.x = currentState.velocity().x();
- odometry.twist.twist.linear.y = currentState.velocity().y();
- odometry.twist.twist.linear.z = currentState.velocity().z();
- odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
- odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
- odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
- pubImuOdometry.publish(odometry);
- }
- };
-
-
- int main(int argc, char** argv)
- {
- ros::init(argc, argv, "roboat_loam");
-
- IMUPreintegration ImuP;
-
- TransformFusion TF;
-
- ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
-
- ros::MultiThreadedSpinner spinner(4);
- spinner.spin();
-
- return 0;
- }
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。