建设银行网站怎么登录密码忘了怎么办,东莞化妆品网站建设,服务器上 网站,怎么做qq网站前言#xff1a; LIO-SAM是一个多传感器融合的紧耦合SLAM框架#xff0c;融合的传感器类型有雷达、IMU和GPS#xff0c;其中雷达和IMU在LIO-SAM框架中必须使用的。LIO-SAM的优化策略采用了GTSAM库#xff0c;GTSAM库采用了因子图的优化方法#xff0c;其提供了一些列C的外…前言 LIO-SAM是一个多传感器融合的紧耦合SLAM框架融合的传感器类型有雷达、IMU和GPS其中雷达和IMU在LIO-SAM框架中必须使用的。LIO-SAM的优化策略采用了GTSAM库GTSAM库采用了因子图的优化方法其提供了一些列C的外部接口以便用户方便传入参数等进行优化。特别的是GTSAM库专门单独设计关于IMU计算与优化的接口。 IMU预积分模块在LIO-SAM源码中写在了imuPreintegration.cpp文件中其中预积分模块的功能使用class IMUPreintegration来实现IMUPreintegration类中构造函数中最主要的两个部分分别是imu的回调函数imuHandler和odom的回调函数odometryHandler。
1.IMU回调函数imuHandler 2.odom回调函数odometryHandler
在odometryHandler回调函数中主要进行了imu数据和lidar的里程计数据联合进行因子图优化的操作。
2.1 odometryHandler中进行的主要操作
Step 0. 系统初始化第一帧Step 1. 计算前一帧与当前帧之间的imu预积分量用前一帧状态施加预积分量得到当前帧初始状态估计添加来自mapOptimization的当前帧位姿进行因子图优化更新当前帧状态Step 2. 优化之后执行重传播优化更新了imu的偏置用最新的偏置重新计算当前激光里程计时刻之后的imu预积分这个预积分用于计算每时刻位姿。Step 3. 每隔100帧激光里程计重置ISAM2优化器保证优化效率
2.2 因子图优化的步骤
1.添加imu预积分因子
// 上面imu预积分的结果
const gtsam::PreintegratedImuMeasurements preint_imu dynamic_castconst 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);
2.添加imu偏置因子前一帧偏置当前帧偏置观测值噪声协方差deltaTij()是积分段的时间
graphFactors.add(gtsam::BetweenFactorgtsam::imuBias::ConstantBias(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_-deltaTij()) * noiseModelBetweenBias)));3.添加位姿因子
gtsam::Pose3 curPose lidarPose.compose(lidar2Imu);
gtsam::PriorFactorgtsam::Pose3 pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);
// 用前一帧的状态、偏置施加imu预计分量得到当前帧的状态 // note 前一帧的状态是经过上一次优化后的结果
gtsam::NavState propState_ imuIntegratorOpt_-predict(prevState_, prevBias_);
4.变量节点赋初值
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);
5.优化
optimizer.update(graphFactors, graphValues);
optimizer.update();
graphFactors.resize(0);
graphValues.clear();
// 优化结果
gtsam::Values result optimizer.calculateEstimate();
注意 每优化完成一次后就会清空因子图和变量优化器是每100帧重置一次。因此每次向优化器内添加的因子图和变量是一一对应的。
6.利用优化结果更新状态量
// 更新当前帧位姿、速度
prevPose_ result.atgtsam::Pose3(X(key));
prevVel_ result.atgtsam::Vector3(V(key));
// 更新当前帧状态
prevState_ gtsam::NavState(prevPose_, prevVel_);
// 更新当前帧imu偏置
prevBias_ result.atgtsam::imuBias::ConstantBias(B(key));
7.重置预积分器设置新的偏置这样下一帧激光里程计进来的时候预积分量就是两帧之间的增量
imuIntegratorOpt_-resetIntegrationAndSetBias(prevBias_);
2.3 因子图优化之后的重传播步骤 这里用一张示意图来表达这一部操作最主要的原因是imu接受数据的频率大于odom里程计的数据因此每来一个odom数据队列中已经有多个imu数据而因子图优化的频率是按照odom里程计的频率来进行的因此如果想要得到每一个imu数据时刻的位姿估计就要以最近的odom时刻的位姿为初始值通过每个imu数据时刻的预积分进行位姿的传播。 效果展示此处一小段粉红色的轨迹就是通过经过因子图优化后的重传播IMU预积分预测出的轨迹前面蓝色的轨迹是因子图优化得到的轨迹。
