imuPreintegration

总体叙述
1. ParamServer
- 1.1 参数定义
- 1.2 ParamServer 构造
- 1.3 other 函数
2. TransformFusion
- 2.1 成员变量
- 2.2 TransformFusion 构造
- 2.3 other函数
- - 1. 里程位姿的转换
  - 2. Imu里程计callback
  - 3. 激光里程计callback
3. IMUPreintegration
- 3.1 成员变量
- 3.1 构造
- 3.3 other
- - imuHandler callback
  - odometryHandler
  - failureDetection

总体叙述

位姿前端的预积分，数据间加了多帧帧间优化，优化变量pose，vel，bias

订阅激光里程计（来自MapOptimization）和IMU里程计，根据前一时刻激光里程计，
和该时刻到当前时刻的IMU里程计变换增量，计算当前时刻IMU里程计；rviz展示IMU里程计轨迹（局部）
订阅激光里程计，来自mapOptimization
1、每隔100帧激光里程计，重置ISAM2优化器，添加里程计、速度、偏置先验因子，执行优化
2、计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，
添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
3、优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿
订阅imu原始数据
1、用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态，也就是imu里程计
2、imu里程计位姿转到lidar系，发布里程计

1. ParamServer

1.1 参数定义

1.1 other

    ros::NodeHandle nh;std::string robot_id;

1.2 topic

    string pointCloudTopic;string imuTopic;string odomTopic;string gpsTopic;

1.3 Frame

    string lidarFrame;string baselinkFrame;string odometryFrame;string mapFrame;

1.4 GPS setting

    bool useImuHeadingInitialization;bool useGpsElevation;   //海拔float gpsCovThreshold;float poseCovThreshold;  位姿协方差阈值

1.5 save pcd

    bool savePCD;string savePCDDirectory;

1.6 Lidar Sensor Configuration

    SensorType sensor;  // velodyneint N_SCAN;  // 线数 16int Horizon_SCAN;  // 水平扫描点int downsampleRate;  float lidarMinRange;float lidarMaxRange;

1.7 IMU

    float imuAccNoise;float imuGyrNoise;float imuAccBiasN;float imuGyrBiasN;float imuGravity;float imuRPYWeight;vector<double> extRotV;vector<double> extRPYV;vector<double> extTransV;Eigen::Matrix3d extRot;Eigen::Matrix3d extRPY;Eigen::Vector3d extTrans;Eigen::Quaterniond extQRPY;

1.8 LOAM

    float edgeThreshold;  // 0.1float surfThreshold;  // 0.1int edgeFeatureMinValidNum;  //边特征的最小有效个数 10int surfFeatureMinValidNum;  // 100

1.9 voxel filter paprams

    float odometrySurfLeafSize;  // 0.2float mappingCornerLeafSize;  // 0.2float mappingSurfLeafSize ;  // 0.2float z_tollerance;  //FLT_MAXfloat rotation_tollerance;  //FLT_MAX

1.10 CPU Params

    int numberOfCores;  // 内核数  2double mappingProcessInterval;  // 建图间隔  0.15

1.11 Surrounding map

    float surroundingkeyframeAddingDistThreshold;  // 1.0float surroundingkeyframeAddingAngleThreshold;  // 0.float surroundingKeyframeDensity;  // 密度 1.0 float surroundingKeyframeSearchRadius;  // 50.

1.12 Loop closure

    bool  loopClosureEnableFlag;float loopClosureFrequency;  // 1.0int   surroundingKeyframeSize; // 50float historyKeyframeSearchRadius;  // 10.0float historyKeyframeSearchTimeDiff;  // 30.0int   historyKeyframeSearchNum;  //25float historyKeyframeFitnessScore;  //0.3

1.13 lobal map visualization radius

    float globalMapVisualizationSearchRadius;  // 1e3float globalMapVisualizationPoseDensity;  // 10.0float globalMapVisualizationLeafSize;  // 1.0

1.2 ParamServer 构造

param ros read

 nh.param<int>("lio_sam/N_SCAN", N_SCAN, 16);nh.param<float>("lio_sam/lidarMinRange", lidarMinRange, 1.0);nh.param<bool>("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false);usleep(100);

1.3 other 函数

IMU数据转换
imuConverter
发布点云函数
publishCloud
IMU ros 角度，加速度
imuAngular2rosAngular
点距离一个点+二个点
pointDistance

2. TransformFusion

优化变量

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)

2.1 成员变量

互斥锁
std::mutex mtx;

订阅话题
subImuOdometry;
subLaserOdometry;

发布话题
发布Imu里程计 pubImuOdometry
发布Imu 路径 pubImuPath

lidarOdomAffine;
imuOdomAffineFront;
imuOdomAffineBack;

tf 接听
tf::TransformListener tfListener;
tf::StampedTransform lidar2Baselink;

里程计队列
double lidarOdomTime = -1;
deque<nav_msgs::Odometry> imuOdomQueue;

2.2 TransformFusion 构造

1、接听激光外参

// 如果lidar系与baselink系不同（激光系和载体系），需要外部提供二者之间的变换关系
if(lidarFrame != baselinkFrame)
{try{// 等待3s  + lidar系到baselink系的变换tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);}catch (tf::TransformException ex){ROS_ERROR("%s",ex.what());}
}

2、订阅发布地图

 // 订阅激光里程计，来自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);

2.3 other函数

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);}

Affine3f中有rotation和translation分量，但Matrix4f中没有。

//1、Matrix4f到Affine3f：
Matrix4f m4f_transform;
Eigen::Transform<float, 3, Eigen::Affine> a3f_transform (m4f_transform);
//2、Affine3f到Matrix4f：
Eigen::Transform<float, 3, Eigen::Affine> a3f_transform ;
Matrix4f m4f_transform=a3f_transform.matrix();

2. Imu里程计callback

订阅imu里程计，来自IMUPreintegration

1、以最近一帧激光里程计位姿为基础，计算该时刻与当前时刻间imu里程计增量位姿变换，相乘得到当前时刻imu里程计位姿
2、发布当前时刻里程计位姿，用于rviz展示；发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段

void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{// static tf // 发布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里程计队列中删除当前（最近的一帧）激光里程计时刻之前的数据// get latest odometry (at current IMU stamp)if (lidarOdomTime == -1)return;while (!imuOdomQueue.empty()){if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)imuOdomQueue.pop_front();elsebreak;}// 最近的一帧激光里程计时刻对应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);// publish latest odometry // 发布当前时刻里程计位姿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);// publish tf         // 发布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);// publish IMU path // 发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段static nav_msgs::Path imuPath;static double last_path_time = -1;double imuTime = imuOdomQueue.back().header.stamp.toSec();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);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);}}
}

3. 激光里程计callback

将激光里程计数据赋值

3. IMUPreintegration

用激光里程计，两帧激光里程计之间的IMU预计分量构建因子图，优化当前帧的状态（包括位姿、速度、偏置）;
以优化后的状态为基础，施加IMU预计分量，得到每一时刻的IMU里程计。

3.1 成员变量

// 公共锁
std::mutex mtx;# 订阅imu，odom，发布imu_odom
ros::Subscriber subImu;
ros::Subscriber subOdometry;
ros::Publisher pubImuOdometry;bool systemInitialized = false;// 噪声协方差，gtsam因子图中
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因子图优化过程中的状态变量 pose\vel\status\bias
gtsam::Pose3 prevPose_;
gtsam::Vector3 prevVel_;
gtsam::NavState prevState_;
gtsam::imuBias::ConstantBias prevBias_;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;//  imu2lidar  lidar2imu
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()));

3.1 构造

初始化订阅发布，设定参数，构造imuIntegratorImu_+imuIntegratorOpt_

 // 订阅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

3.3 other

imuHandler callback

 * 订阅imu原始数据* 1、用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态，也就是imu里程计* 2、imu里程计位姿转到lidar系，发布里程计

1、将imu数据放入两队列中 imuQueOpt 、imuQueImu

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);

2、计算dt，并赋值 lastImuT_imu，这儿时间回跳也没关系

double imuTime = ROS_TIME(&thisImu);
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
lastImuT_imu = imuTime;

3、imu预积分器添加一帧imu数据，注：这个预积分器的起始时刻是上一帧激光里程计时刻

// integrate this single imu message
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);

4、预测odometry

// predict odometry
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";

5、转换 imu_pose到激光frame

// transform imu pose to ldiar
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);

odometryHandler

/*** 订阅激光里程计，来自mapOptimization* 1、每隔100帧激光里程计，重置ISAM2优化器，添加里程计、速度、偏置先验因子，执行优化* 2、计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，*     添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态* 3、优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿
*/

利用优化估计了bias和角度
1、当前帧激光位姿，来自scan-to-map匹配、因子图优化后的位姿

double currentCorrectionTime = ROS_TIME(odomMsg);// make sure we have imu data to integrate
if (imuQueOpt.empty())return;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));

2、初始化第一帧


// 0. initialize system
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();}elsebreak;}// initial pose 添加里程计位姿先验因子prevPose_ = lidarPose.compose(lidar2Imu);gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);graphFactors.add(priorPose);// initial velocity  添加速度先验因子prevVel_ = gtsam::Vector3(0, 0, 0);gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);graphFactors.add(priorVel);// initial bias 添加imu偏置先验因子prevBias_ = gtsam::imuBias::ConstantBias();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 优化一次optimizer.update(graphFactors, graphValues);graphFactors.resize(0);graphValues.clear();// 重置优化之后的偏置imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);key = 1;systemInitialized = true;return;
}

3、每隔100帧激光里程计，重置ISAM2优化器，保证优化效率

// reset graph for speed
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)));// reset graph 重置ISAM2优化器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;
}

4、计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态

while (!imuQueOpt.empty())
{// pop and integrate imu data that is between two optimizationssensor_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);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();}elsebreak;
}

5、添加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);

6、添加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)));

7、添加位姿因子

gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);

8、用前一帧的状态、偏置，施加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_);

9、优化

optimizer.update(graphFactors, graphValues);
optimizer.update();
graphFactors.resize(0);
graphValues.clear();

10、为下次操作更新数据

gtsam::Values result = optimizer.calculateEstimate();
prevPose_  = result.at<gtsam::Pose3>(X(key));
prevVel_   = result.at<gtsam::Vector3>(V(key));
prevState_ = gtsam::NavState(prevPose_, prevVel_);
prevBias_  = result.at<gtsam::imuBias::ConstantBias>(B(key));
// Reset the optimization preintegration object.
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

11、检测优化，若失败则restParam
imu因子图优化结果，速度或者偏置过大，认为失败

// check optimization
if (failureDetection(prevVel_, prevBias_))
{resetParams();return;
}

12、优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿

prevStateOdom = prevState_;
prevBiasOdom  = prevBias_;
// first pop imu message older than current correction data
double lastImuQT = -1;
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{lastImuQT = ROS_TIME(&imuQueImu.front());imuQueImu.pop_front();
}
// repropogate  对剩余的imu数据计算预积分
if (!imuQueImu.empty())
{// reset bias use the newly optimized biasimuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);// integrate imu message from the beginning of this optimizationfor (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;

failureDetection

imu因子图优化结果，速度或者偏置过大，认为失败
速度模大于30
bias 加速度和角速度模大于1

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;

LIO-SAM imuPreintegration相关推荐

SLAM相关学习资料：综述/激光/视觉/数据集/常用库
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨菠萝包包包@知乎来源丨https://zhuanlan.zhihu.com/p/4348743 ...
八种常用激光雷达和视觉SLAM算法的评估与比较
文章:Evaluation and comparison of eight popular Lidar and Visual SLAM algorithms 作者:Bharath Garigipati ...
国内外LiDAR SLAM实验室总结
知乎上有一个关于视觉的SLAM实验室以及20年前的视觉SLAM开源算法的汇总SLAM 领域国内外优秀实验室汇总,非常详细,但是偏视觉一点,本文主要关注LiDAR SLAM. 这篇文章一开始是非常简短的 ...
激光SLAM论文简单导读--LOAM、VLOAM、LeGO-LOAM、LIO-SAM、LVI-SAM、LIMO、LIC-FUSION、TVL-SLAM、R2LIVE、R3LIVE
激光SLAM论文简单导读--LOAM.VLOAM.LeGO-LOAM.LIO-SAM.LVI-SAM.LIMO.LIC-FUSION.TVL-SLAM.R2LIVE.R3LIVE 时间线开篇巨作LO ...
iSCSI target介绍及LIO实操入门
前文介绍了iSCSI的基本架构及启动器的基本操作,也就是在客户端的操作.今天我们介绍一下目标器的相关概念.开源实现和基本操作.Linux操作系统下面有很多目标器的开源实现,比如LIO.SCST和TGT ...
lio linux工具,Linux中三种SCSI target的介绍之LIO
1. 简介 Linux-IO Target在Linux内核中(linux 2.6.38后),用软件实现各种SCSI Target,其支持的SAN技术中所有流行的存储协议包括Fibre Channel( ...
2019.03.01 bzoj2555: SubString（sam+lct）
传送门题意简述: 要求在线支持两个操作 (1):在当前字符串的后面插入一个字符串 (2):询问字符串s在当前字符串中出现了几次?(作为连续子串) 思路: 考虑用lctlctlct来动态维护samsa ...
4566: [Haoi2016]找相同字符 SAM
折腾了好久.不过收获还是很多的.第一次自己去画SAM所建出来fail树.深入体会了这棵树的神奇性质. 当然,我最终靠着自己A掉了.(这是我第一次推SAM的性质(以前都是抄别人的,感觉自己好可耻),不过 ...
BZOJ1396:识别子串(SAM)
Description Input 一行,一个由小写字母组成的字符串S,长度不超过10^5 Output L行,每行一个整数,第i行的数据表示关于S的第i个元素的最短识别子串有多长. Sample I ...
【MIT Sam Hopkins教授】如何读论文？How to Read a Paper
来源:专知本文多图,建议阅读5分钟MIT Sam Hopkins助理教授为您讲解提供有效阅读论文的工具.

LIO-SAM imuPreintegration