准备工作
- 引用头文件
/************************************************* TransformFusion类 功能简介： 主要功能是订阅激光里程计（来自MapOptimization）和IMU里程计，根据前一时刻激光里程计，和该时刻到当前时刻的IMU里程计变换增量，计算当前时刻IMU里程计；rviz展示IMU里程计轨迹（局部）。 订阅： 1、订阅激光里程计，来自MapOptimization； 2、订阅imu里程计，来自ImuPreintegration。 发布： 1、发布IMU里程计，用于rviz展示； 2、发布IMU里程计轨迹，仅展示最近一帧激光里程计时刻到当前时刻之间的轨迹。 -------------------------------------------------- IMUPreintegration类 功能简介： 1、用激光里程计，两帧激光里程计之间的IMU预积分量构建因子图，优化当前帧的状态（包括位姿、速度、偏置）; 2、以优化后的状态为基础，施加IMU预积分量，得到每一时刻的IMU里程计。 订阅： 1、订阅IMU原始数据，以因子图优化后的激光里程计为基础，施加两帧之间的IMU预积分量，预测每一时刻（IMU频率）的IMU里程计； 2、订阅激光里程计（来自MapOptimization），用两帧之间的IMU预积分量构建因子图，优化当前帧位姿（这个位姿仅用于更新每时刻的IMU里程计，以及下一次因子图优化）。 发布： 1、发布imu里程计； **************************************************/ // 引用自定义的函数 #include "utility.h" // gtsam相关 #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) 加速度计与陀螺仪的零偏
此文件中主要包括TransformFusion与IMUPreintegration两个类，分别进行位姿坐标转换与预积分操作。
首先，我们先来看一下IMUPreintegration类中的相关操作：
IMUPreintegration类
准备工作
- IMUPreintegration类变量声明
/** * @brief IMU预积分类 * 1、接收IMU原始数据，接收mapoptimization节点发送的lidar里程计incremental消息 * 2、分别设置两个预积分器，一个在imuhandler函数中不断添加IMU原始数据，以最新的优化后的lidar位姿为基础，同IMU原始数据相同频率发布 * 3、另外一个预积分器odometryHandler中进行处理，添加两帧LiDAR里程计优化后位姿之间的预积分量，噪声、零偏约束等因子，进行优化 * 4、对两个预积分器重新设置优化后的零偏，状态（pose, bias），预计分量。 */ class IMUPreintegration : public ParamServer { public: std::mutex mtx; // 接听IMU消息数据，里程计数据 ros::Subscriber subImu; ros::Subscriber subOdometry; // 增量式的lidar里程计 // 发布IMU里程计 "odometry/imu_incremental" 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预积分器，都存储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_; // pose + vel gtsam::imuBias::ConstantBias prevBias_; // imu的状态 gtsam::NavState prevStateOdom; gtsam::imuBias::ConstantBias prevBiasOdom; // 是否完成了第一次优化 bool doneFirstOpt = false; // 上一帧IMU、上一帧IMU优化的时间 double lastImuT_imu = -1; double lastImuT_opt = -1; // 定义ISAM2优化器 需要设置ISAM2Params参数 gtsam::ISAM2 optimizer; // 定义一个factors以及一个values进行优化 gtsam::NonlinearFactorGraph graphFactors; gtsam::Values graphValues; // IMU数据与激光里程计数据之间的时间差，设置为0 const double delta_t = 0; int key = 1; // lidar与imu坐标系之间的相互转换 外参变换（只有平移没有旋转，参数文件中平移量设置的也为 [0.0, 0.0, 0.0]） // 所以两个变换都没有作用 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消息（imuTopic， /imu/data） subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay()); // 订阅"lio_sam/mapping/odometry_incremental"消息（后端mapOptmization模块发出） subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay()); // 发布imu里程计， 预积分节点发布的增量位姿（"odometry/imu_incremental"） pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000); // 定义预积分相关参数 噪声协方差 定义重力方向ENU 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 // 假设初始零偏为0 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优化过程中发生退化，则选择一个较大的协方差correctionNoise2 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预积分器 // imuIntegratorImu_用于预测每一时刻（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 }
重设优化器、重设参数
/** * @brief 重设ISAM2优化器相关参数 * 1、主要是optimizer优化器重设optParameters * 2、重设graphFactors与graphValues */ void resetOptimization() { // gtsam初始化 gtsam::ISAM2Params optParameters; optParameters.relinearizeThreshold = 0.1; optParameters.relinearizeSkip = 1; optimizer = gtsam::ISAM2(optParameters); gtsam::NonlinearFactorGraph newGraphFactors; graphFactors = newGraphFactors; gtsam::Values NewGraphValues; graphValues = NewGraphValues; } /** * @brief 重设参数 * 包括上一帧接收的IMU时间 是否已经进行过激光里程计的优化 系统是否已经初始化 */ void resetParams() { lastImuT_imu = -1; doneFirstOpt = false; systemInitialized = false; }
订阅激光里程计数据，并处理
/** * @brief 订阅激光里程计，来自mapOptimization节点 * * 1、每隔100帧激光里程计，重置ISAM2优化器，添加里程计、速度、偏置先验因子，执行优化 * 2、计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态 * 3、优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿 * * @param odomMsg */ void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg) { std::lock_guard<std::mutex> lock(mtx); // 当前接收的激光里程计数据时间戳 double currentCorrectionTime = ROS_TIME(odomMsg); // make sure we have imu data to integrate // 确保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的格式（gtsam::Pose3） 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. initialize system // 是否已经初始化系统？ 首先初始化系统（第一帧数据） if (systemInitialized == false) { // 重置ISAM2优化器 resetOptimization(); // pop old IMU message // 将这个里程计消息之前的imu信息全部扔掉 while (!imuQueOpt.empty()) { // delta_t = 0，弹出在当前激光里程计currentCorrectionTime时刻之前的IMU数据 if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t) { // 取出imu队列中的第一个数据的时间 lastImuT_opt = ROS_TIME(&imuQueOpt.front()); imuQueOpt.pop_front(); } else break; } // initial pose（添加里程计位姿先验因子）PriorFactor Pose3 // 将接收的激光里程计位姿转移到imu坐标系下 prevPose_ = lidarPose.compose(lidar2Imu); // 设置其初始位姿和置信度 gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise); // 约束加入到因子中 添加里程计的位姿先验因子 graphFactors.add(priorPose); // initial velocity（添加速度先验因子） PriorFactor Vector3 // 初始化速度，这里就直接赋0了 prevVel_ = gtsam::Vector3(0, 0, 0); gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise); // 将对速度的约束也加入到因子图中 添加速度先验因子 graphFactors.add(priorVel); // initial bias（添加imu零偏先验因子） PriorFactor imuBias::ConstantBias // 初始化零偏 prevBias_ = gtsam::imuBias::ConstantBias(); // 初始为（0,0,0,0,0,0） gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise); // 零偏加入到因子图中 graphFactors.add(priorBias); // 以上把约束加入完毕，下面开始添加状态量 // add values // 将各个状态量赋成初始值 graphValues.insert(X(0), prevPose_); graphValues.insert(V(0), prevVel_); graphValues.insert(B(0), prevBias_); // optimize once // 约束和状态量更新进isam优化器（优化一次） optimizer.update(graphFactors, graphValues); // 进优化器之后保存约束和状态量的变量就清零 graphFactors.resize(0); graphValues.clear(); // 预积分的接口，使用初始零偏进行初始化（对两个预积分器分别重置零偏） imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_); imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); key = 1; // 索引 systemInitialized = true; // 完成系统初始化 return; } // reset graph for speed // 当isam优化器中加入了较多的约束后，为了避免运算时间变长，就直接清空 // 每收到100个来自后端Lidar odometry数据重设一次 if (key == 100) { // get updated noise before reset // 取出最新时刻位姿 速度 零偏的协方差矩阵 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))); // reset graph // 复位整个优化问题 resetOptimization(); /************类似于系统初始化，添加三种不同类型的因子****************/ // add pose（添加最新的位姿因子） // 将最新的位姿，速度，零偏以及对应的协方差矩阵加入到因子图中 gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise); graphFactors.add(priorPose); // add velocity（添加最新的速度因子） gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise); graphFactors.add(priorVel); // add bias（添加零偏因子） gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise); graphFactors.add(priorBias); // add values（添加最新的状态） graphValues.insert(X(0), prevPose_); graphValues.insert(V(0), prevVel_); graphValues.insert(B(0), prevBias_); // optimize once（优化一次） optimizer.update(graphFactors, graphValues); graphFactors.resize(0); graphValues.clear(); // 重新计数 key = 1; } // 计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计 // 添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态 // 1. integrate imu data and optimize // 添加到上一帧激光里程计到当前帧时间戳之间的IMU数据 while (!imuQueOpt.empty()) { // pop and integrate imu data that is between two optimizations // 将imu消息取出来 sensor_msgs::Imu *thisImu = &imuQueOpt.front(); double imuTime = ROS_TIME(thisImu); // 时间上小于当前lidar位姿的都取出来 if (imuTime < currentCorrectionTime - delta_t) { // 计算两个imu数据帧之间的时间差 double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt); // 调用预积分接口将imu数据送进去处理 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); // 记录当前imu时间 lastImuT_opt = imuTime; imuQueOpt.pop_front(); } else break; } // add imu factor to graph 添加IMU预积分因子 ImuFactor // 两帧间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); // add imu bias between factor BetweenFactor // 零偏的约束，两帧间零偏相差不会太大，因此使用常量约束 // 添加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))); // add pose factor 添加位姿因子（后端LiDAR里程计传入的LiDAR位姿，转换到IMU坐标系下）PriorFactor gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu); // lidar位姿转换到imu坐标系下，同时根据是否退化选择不同的置信度，作为这一帧的先验估计，如果degenerate为真，则调大噪声 gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise); // 加入因子图中去 graphFactors.add(pose_factor); // 用前一帧的状态、偏置，施加imu预计分量，得到当前帧的状态 // insert predicted values // 根据上一时刻的状态，结合预积分结果，对当前状态进行预测 gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_); // 预测量作为初始值插入因子图中 graphValues.insert(X(key), propState_.pose()); graphValues.insert(V(key), propState_.v()); graphValues.insert(B(key), prevBias_); // optimize // 执行优化 optimizer.update(graphFactors, graphValues); optimizer.update(); graphFactors.resize(0); graphValues.clear(); // Overwrite the beginning of the preintegration for the next step. // 优化结果 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)); // Reset the optimization preintegration object. // 当前约束任务已经完成，预积分约束复位，同时需要设置一下零偏作为下一次积分的先决条件（更新零偏） // reset零偏之后，之前的IMU预积分量就清零了 imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_); // check optimization // 一个简单的失败检测 // imu因子图优化结果，速度或者偏置过大，认为失败。进行状态复位 if (failureDetection(prevVel_, prevBias_)) { // 状态异常就直接复位了 resetParams(); return; } // 优化之后，执行重传播； // 优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿 // 2. after optiization, re-propagate imu odometry preintegration prevStateOdom = prevState_; prevBiasOdom = prevBias_; // first pop imu message older than current correction data double lastImuQT = -1; // 首先把lidar帧之前的imu状态全部弹出去 while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t) { lastImuQT = ROS_TIME(&imuQueImu.front()); imuQueImu.pop_front(); } // repropogate // 如果有新于lidar状态时刻的imu if (!imuQueImu.empty()) { // reset bias use the newly optimized bias // 这个预积分变量复位 （reset零偏之后，之前的IMU预积分量就清零了） imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom); // integrate imu message from the beginning of this optimization // 然后把剩下的imu状态重新积分 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数据，并处理
接听原始的IMU数据，包括加速度计的三轴线性加速度与陀螺仪的三轴旋转速度。添加这些原始的IMU测量值到imuQueOpt与imuQueImu中。以当前最新的IMU预积分器零偏值进行预测IMU里程计数据。
/** * @brief 订阅IMU原始数据/imu/data * 处理IMU原始数据 * 1、用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态，也就是imu里程计 * 2、imu里程计位姿转到lidar系，发布里程计 * @param imu_raw */ void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw) { std::lock_guard<std::mutex> lock(mtx); // 首先把imu的状态做一个简单的转换（utilty.h文件中的ParamServer类） // shan ti xiao 使用的IMU加速度计、陀螺仪，与磁力计的坐标系朝向不同，进行一个转换 sensor_msgs::Imu thisImu = imuConverter(*imu_raw); // 注意这里有两个imu的队列，作用不相同，一个用来执行预积分和位姿的优化，一个用来更新最新imu状态 imuQueOpt.push_back(thisImu); imuQueImu.push_back(thisImu); // 如果没有发生过优化就return // 要求上一次imu因子图优化执行成功，确保更新了上一帧（激光里程计帧）的状态、偏置，预积分重新计算了 if (doneFirstOpt == false) return; // 更新IMU时间，相邻帧之间的时间间隔 double imuTime = ROS_TIME(&thisImu); // 如果上一帧IMU时间小于0（-1，更新激光帧之后的第一帧IMU数据）则dt=1/500(500hz) // 反之若上一帧IMU时间大于0，则dt=当前帧IMU数据-上一帧IMU数据 double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu); lastImuT_imu = imuTime; // integrate this single imu message // 每来一个imu值就加入预积分状态中 // 注：这个预积分器的起始时刻是上一帧激光里程计时刻，而不是0时刻 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); // predict // 用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态 gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); // publish odometry nav_msgs::Odometry odometry; odometry.header.stamp = thisImu.header.stamp; odometry.header.frame_id = odometryFrame; odometry.child_frame_id = "odom_imu"; // transform imu pose to ldiar // 将这个状态转到lidar坐标系下去发送出去 // 增量位姿（"odometry/imu_incremental"） 从上一帧激光里程计到当前的IMU数据时间 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); }
错误检测
检测优化的结果是否在合理范围内：
/** * @brief 检测优化后的结果是否正确 * 检查优化后的速度prevVel_ 与 零偏 prevBias_是否在合理范围内 * @param velCur * @param biasCur * @return true * @return false */ bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur) { // 速度 Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z()); // 如果当前速度大于30m/s，108km/h就认为是异常状态， 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; }
