【LIO-SAM】激光SLAM 之激光里程计实现细节
1. 激光里程计实现流程
* 订阅激光里程计，来自mapOptimization
* 1、每隔100帧激光里程计，重置ISAM2优化器，添加里程计、速度、偏置先验因子，执行优化
* 2、计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
* 3、优化之后，执行重传播；优化更新了imu的偏置，用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿
-
系统初始化（第0帧）：
- 初始化
ISAM2优化器，并设置初始条件。 - 添加里程计位姿先验因子、速度先验因子、IMU偏置先验因子，这是因子图优化的基础。
- 优化一次，重置IMU预积分器。
- 初始化
-
每隔100帧重置优化器：
- 重置优化器以提高效率，但保留上一帧的状态（位姿、速度、偏置），继续进行后续的优化。
-
IMU预积分：
- 提取IMU数据，计算激光帧之间的IMU预积分。
- 根据前一帧的状态（位姿、速度、偏置），施加IMU预积分，预测出当前帧的状态。
- 将这些状态加入因子图中，并结合激光里程计的观测进行优化。
-
因子图优化：
- 优化使用
ISAM2算法，逐步更新状态估计。 - 优化之后，通过重传播更新IMU偏置，并继续预积分下一帧IMU数据。
- 优化使用
-
重传播：
- 更新IMU偏置后，重新计算当前帧之后的IMU预积分，保证精度。
2. 激光里程计详细数学推导
这段代码通过因子图优化和IMU预积分实现了激光里程计的状态估计，并结合IMU数据进行运动畸变校正。代码使用了GTSAM库来进行位姿、速度和偏置的联合优化。
-
IMU预积分：
IMU预积分使用IMU的加速度和角速度数据来估计设备在帧间的增量运动。通过IMU测量的线性加速度 a \mathbf{a} a 和角速度 ω \mathbf{\omega} ω，可以计算两个时间点间的位姿增量：
Δ R i j = R i ⋅ exp ( ∫ t i t j ω ( t ) d t ) \Delta R_{ij} = R_i \cdot \exp\left(\int_{t_i}^{t_j} \omega(t) dt \right) ΔRij=Ri⋅exp(∫titjω(t)dt)
其中， Δ R i j \Delta R_{ij} ΔRij是从时刻 t i t_i ti到 t j t_j tj 的旋转变化； R i R_i Ri是时刻 t i t_i ti的旋转矩阵。对于位移预积分，采用以下公式：
Δ p i j = ∫ t i t j v ( t ) d t + 1 2 ∫ t i t j a ( t ) d t 2 \Delta p_{ij} = \int_{t_i}^{t_j} \mathbf{v}(t) dt + \frac{1}{2} \int_{t_i}^{t_j} \mathbf{a}(t) dt^2 Δpij=∫titjv(t)dt+21∫titja(t)dt2
其中， v ( t ) \mathbf{v}(t) v(t) 是速度， a ( t ) \mathbf{a}(t) a(t)是加速度。IMU预积分计算不仅考虑位姿增量，还会根据IMU偏置更新其预积分结果。公式中加入了偏置修正：
a ′ ( t ) = a ( t ) − b a , ω ′ ( t ) = ω ( t ) − b g \mathbf{a}'(t) = \mathbf{a}(t) - \mathbf{b}_a, \quad \mathbf{\omega}'(t) = \mathbf{\omega}(t) - \mathbf{b}_g a′(t)=a(t)−ba,ω′(t)=ω(t)−bg
其中， b a \mathbf{b}_a ba 和 b g \mathbf{b}_g bg 分别是加速度计和陀螺仪的偏置。 -
因子图优化：
在每一帧激光里程计数据到来后，结合IMU预积分量更新状态估计。通过因子图优化，构建并求解以下问题：
目标：最小化测量误差
min x ∑ i ( ∥ z i − h i ( x ) ∥ 2 + ∥ u i − g i ( x ) ∥ 2 ) \min_{\mathbf{x}} \sum_i \left( \| \mathbf{z}_i - h_i(\mathbf{x}) \|^2 + \| \mathbf{u}_i - g_i(\mathbf{x}) \|^2 \right) xmini∑(∥zi−hi(x)∥2+∥ui−gi(x)∥2)-
x
\mathbf{x}
x表示状态变量（位姿、速度、偏置）
- z i \mathbf{z}_i zi 和 u i \mathbf{u}_i ui 分别表示激光里程计和IMU的观测量
- h i ( x ) h_i(\mathbf{x}) hi(x) 和 g i ( x ) g_i(\mathbf{x}) gi(x) 分别表示激光里程计和IMU的测量模型
GTSAM 通过非线性优化（如
iSAM2），不断调整状态变量，使得观测误差最小。 -
x
\mathbf{x}
x表示状态变量（位姿、速度、偏置）
-
状态更新：
通过优化后的结果，更新当前帧的状态：
x k = optimize ( x k − 1 , Δ x , z k ) \mathbf{x}_k = \text{optimize}(\mathbf{x}_{k-1}, \Delta \mathbf{x}, \mathbf{z}_k) xk=optimize(xk−1,Δx,zk)
其中， Δ x \Delta \mathbf{x} Δx是IMU预积分的预测增量， z k \mathbf{z}_k zk 是当前激光帧的位姿观测。 -
重传播：
当偏置更新后，需要重新计算后续IMU数据的预积分，以确保在后续帧中使用最新的偏置进行校正。通过重传播，可以更精确地估计后续帧的状态。
3. 因子图处理规模分析
每帧的处理规模主要由以下几部分决定：
- 因子图规模：包括位姿因子、速度因子、偏置因子、IMU预积分因子，因子图的规模随帧数线性增加，每隔100帧重置一次以控制计算复杂度。
- IMU预积分计算量：IMU预积分器需要对每个时间段的加速度、角速度进行积分操作，计算复杂度与IMU数据的频率成正比。
- 优化步骤：
iSAM2优化是增量式的，每帧更新因子图，优化规模与帧数线性相关，通常在每100帧重置以控制计算开销。
总体来说，代码中的因子图优化和IMU预积分都是高效的，适合实时系统使用。
4. 激光里程计实现代码
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;
}
扫一扫
531
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
