FAST-LIO2是一个基于紧耦合的激光雷达-惯性里程计(LIO)系统，采用Error-State Iterated Kalman Filter (ESIKF)进行多传感器融合，实现高精度实时建图和定位。本文档详细解释了整个建图流程、ESIKF的作用机制以及多传感器融合策略。
- LIOBuilder: 核心建图模块，负责ESIKF状态估计和地图构建
- IMUProcessor: IMU数据处理器，负责IMU初始化和点云去畸变
- ESIKF: 误差状态迭代卡尔曼滤波器，实现多传感器融合
- ikd-tree: 增量式KD树，用于高效点云存储和搜索
- LoopClosureThread: 回环检测线程，基于GTSAM进行后端优化
IMU数据 ──┐
├─→ MeasureGroup ─→ LIOBuilder ─→ ESIKF ─→ 状态估计
LiDAR数据 ─┘ ↓
地图更新 ─→ ikd-tree
↓
回环检测 ─→ GTSAM优化
系统状态采用流形表示，包含以下24维状态：
state_ikfom {
vect3 pos; // 位置 (3维)
SO3 rot; // 旋转 (3维流形)
SO3 offset_R_L_I; // 激光雷达到IMU旋转外参 (3维流形)
vect3 offset_T_L_I; // 激光雷达到IMU平移外参 (3维)
vect3 vel; // 速度 (3维)
vect3 bg; // 陀螺仪偏置 (3维)
vect3 ba; // 加速度计偏置 (3维)
S2 grav; // 重力向量 (2维球面流形)
}input_ikfom {
vect3 acc; // 加速度测量
vect3 gyro; // 角速度测量
}process_noise_ikfom {
vect3 ng; // 陀螺仪噪声
vect3 na; // 加速度计噪声
vect3 nbg; // 陀螺仪偏置噪声
vect3 nba; // 加速度计偏置噪声
}bool MeasureGroup::syncPackage(ImuData &imu_data, LivoxData &livox_data)- 时间对齐: 确保IMU数据覆盖整个激光雷达扫描周期
- 数据缓存: 使用双端队列缓存传感器数据
- 时间戳检查: 防止数据回环和时序错误
void LivoxData::livox2pcl(const livox_ros_driver2::CustomMsg::ConstPtr &msg,
fastlio::PointCloudXYZI::Ptr &out)- 数据转换: Livox格式转PCL格式
- 滤波处理: 按比例降采样，去除近距离点
- 时间戳提取: 提取每个点的相对时间戳
bool IMUProcessor::init(const MeasureGroup &meas)初始化过程:
- 静止检测: 检查IMU是否处于静止状态
- 重力估计: 通过加速度计测量估计重力方向
- 偏置估计: 计算陀螺仪和加速度计的初始偏置
- 协方差初始化: 设置初始状态协方差
重力对齐:
if (align_gravity_) {
// 将估计的重力方向与标准重力对齐
Eigen::Vector3d gravity_estimated = mean_acc_.normalized() * G_m_s2;
// 计算对齐旋转矩阵
}void IMUProcessor::undistortPointcloud(const MeasureGroup &meas,
PointCloudXYZI::Ptr &out)去畸变流程:
-
IMU状态积分:
for (auto &imu : meas.imus) { input_ikfom inp; inp.acc = imu.acc; inp.gyro = imu.gyro; double dt = imu.timestamp - last_time; kf_->predict(dt, Q_, inp); // ESIKF预测步骤 // 记录IMU位姿 imu_poses_.emplace_back(imu.timestamp, ...); }
-
点云时间插值:
for (auto &point : cloud->points) { double point_time = point.curvature / 1000.0; // 相对时间 // 在IMU位姿序列中插值 Pose interpolated_pose = interpolate(imu_poses_, point_time); // 运动补偿 point_compensated = apply_motion_compensation(point, interpolated_pose); }
-
坐标变换链:
点云(t) → IMU(t) → IMU(end) → 激光雷达(end)
void esekf::predict(double dt, const cov &Q, const input_ikfom &i_in)状态传播:
// 计算状态转移函数
Eigen::Matrix<double, 24, 1> f = get_f(x_, i_in);
// 状态更新
x_.oplus(f, dt);状态转移方程:
Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in) {
// 位置导数 = 速度
res.segment<3>(0) = s.vel;
// 旋转导数 = 去偏置角速度
vect3 omega;
in.gyro.boxminus(omega, s.bg);
res.segment<3>(3) = omega;
// 速度导数 = 旋转后的加速度 + 重力
vect3 a_inertial = s.rot * (in.acc - s.ba);
res.segment<3>(12) = a_inertial + s.grav;
// 偏置导数 = 0 (随机游走模型)
// 重力导数 = 0 (常量)
return res;
}协方差传播:
// 计算雅可比矩阵
Matrix f_x = df_dx(x_, i_in); // 状态雅可比
Matrix f_w = df_dw(x_, i_in); // 噪声雅可比
// 协方差更新
P_ = f_x * P_ * f_x.transpose() + f_w * Q * f_w.transpose();void esekf::update_iterated_dyn_share_modified(double R, double &solve_time)观测模型 - 点到平面距离:
-
平面拟合:
bool esti_plane(Eigen::Vector4d &plane_coeff, const PointVector &points, const double &thresh) { // 最小二乘平面拟合 // 平面方程: ax + by + cz + d = 0 }
-
残差计算:
// 点到平面距离作为观测残差 double residual = plane_coeff(0) * point.x + plane_coeff(1) * point.y + plane_coeff(2) * point.z + plane_coeff(3);
-
雅可比矩阵:
void LIOBuilder::sharedUpdateFunc(state_ikfom &state, esekfom::dyn_share_datastruct<double> &share_data) { // 位置雅可比 share_data.h_x.block<1, 3>(i, 0) = norm_vec.transpose(); // 旋转雅可比 Matrix3d temp_mat = SKEW_SYM_MATRX(temp_vec); Matrix<double, 1, 3> B = -norm_vec.transpose() * state.rot.toRotationMatrix() * temp_mat; share_data.h_x.block<1, 3>(i, 3) = B; // 外参雅可比 (如果估计外参) if (extrinsic_est_en_) { // 外参旋转雅可比 share_data.h_x.block<1, 3>(i, 6) = C; // 外参平移雅可比 share_data.h_x.block<1, 3>(i, 9) = D; } }
-
迭代优化:
// 迭代卡尔曼滤波更新 for (int i = 0; i < max_iterations; i++) { // 计算卡尔曼增益 K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); // 状态更新 dx = K * residual; x.oplus(dx); // 收敛检查 if (dx.norm() < convergence_threshold) break; }
void LIOBuilder::trimMap()动态地图边界:
- 根据当前激光雷达位置动态调整局部地图范围
- 当接近地图边缘时，移动地图中心
- 删除超出范围的历史点云
void LIOBuilder::increaseMap()智能点云添加策略:
-
体素网格降采样:
// 计算点所在体素中心 PointType mid_point; mid_point.x = floor(point.x / resolution) * resolution + 0.5 * resolution; mid_point.y = floor(point.y / resolution) * resolution + 0.5 * resolution; mid_point.z = floor(point.z / resolution) * resolution + 0.5 * resolution;
-
距离比较策略:
// 比较新点与现有点到体素中心的距离 float dist_new = sq_dist(new_point, mid_point); for (auto &existing_point : nearby_points) { if (sq_dist(existing_point, mid_point) < dist_new) { need_add = false; // 现有点更接近中心，不添加新点 break; } }
-
ikd-tree更新:
ikdtree_->Add_Points(points_to_add, true); // 需要降采样 ikdtree_->Add_Points(points_no_downsample, false); // 直接添加
时间同步:
- IMU高频率(通常200-1000Hz)提供连续运动信息
- 激光雷达低频率(通常10-20Hz)提供空间约束
- 通过时间插值实现精确同步
状态共享:
- IMU和激光雷达共享同一状态向量
- 实时估计传感器外参
- 统一的不确定性传播
| 传感器 | 优势 | 劣势 | 在系统中的作用 |
|---|---|---|---|
| IMU | 高频率、短期精度高 | 长期漂移 | 运动预测、点云去畸变 |
| 激光雷达 | 绝对位置精度高 | 低频率、计算量大 | 位置校正、地图构建 |
在线外参估计:
if (extrinsic_est_en_) {
// 将外参作为状态变量进行估计
// 旋转外参: offset_R_L_I
// 平移外参: offset_T_L_I
}外参雅可比:
- 外参旋转对观测的影响
- 外参平移对观测的影响
- 通过点到平面约束同时优化外参
void MapBuilderROS::addKeyPose() {
// 基于运动阈值选择关键帧
if (diff_pose.norm() > dist_thresh ||
abs(rpy(0)) > rad_thresh ||
abs(rpy(1)) > rad_thresh ||
abs(rpy(2)) > rad_thresh) {
// 添加新的关键帧
}
}候选回环搜索:
- 基于欧几里得距离的粗搜索
- 时间间隔约束
- ICP精配准验证
GTSAM后端优化详解:
GTSAM (Georgia Tech Smoothing and Mapping) 后端优化是FAST-LIO2系统中的关键组件，负责全局一致性优化和回环检测。以下是详细的技术实现：
LoopClosureThread类:
class LoopClosureThread {
private:
std::shared_ptr<gtsam::ISAM2> isam2_; // ISAM2增量优化器
gtsam::NonlinearFactorGraph gtsam_graph_; // 因子图
gtsam::Values initialized_estimate_; // 初始估计值
gtsam::Values optimized_estimate_; // 优化后的估计值
pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_history_poses_; // 历史位姿KD树
pcl::IterativeClosestPoint<...>::Ptr icp_; // ICP配准器
};关键数据结构定义:
// 6DOF位姿结构
struct Pose6D {
int index; // 关键帧索引
double time; // 时间戳
Eigen::Matrix3d local_rot; // 局部坐标系旋转
Eigen::Vector3d local_pos; // 局部坐标系位置
Eigen::Matrix3d global_rot; // 全局坐标系旋转
Eigen::Vector3d global_pos; // 全局坐标系位置
};
// 回环约束对
struct LoopPair {
int pre_idx, cur_idx; // 回环关键帧索引
Eigen::Matrix3d diff_rot; // 相对旋转
Eigen::Vector3d diff_pos; // 相对位移
double score; // ICP匹配得分
};
// 共享数据结构
struct SharedData {
std::vector<Pose6D> key_poses; // 关键帧位姿
std::vector<LoopPair> loop_pairs; // 回环约束
std::vector<std::pair<int, int>> loop_history; // 回环历史
std::vector<fastlio::PointCloudXYZI::Ptr> cloud_history; // 点云历史
Eigen::Matrix3d offset_rot; // 全局偏移旋转
Eigen::Vector3d offset_pos; // 全局偏移位移
};void LoopClosureThread::init() {
// ISAM2参数配置
gtsam::ISAM2Params isam2_params;
isam2_params.relinearizeThreshold = 0.01; // 重线性化阈值
isam2_params.relinearizeSkip = 1; // 重线性化跳过步数
isam2_ = std::make_shared<gtsam::ISAM2>(isam2_params);
// ICP配准器配置
icp_->setMaxCorrespondenceDistance(100); // 最大对应距离
icp_->setMaximumIterations(50); // 最大迭代次数
icp_->setTransformationEpsilon(1e-6); // 变换收敛阈值
icp_->setEuclideanFitnessEpsilon(1e-6); // 欧几里得适应度阈值
}候选回环搜索:
void LoopClosureThread::loopCheck() {
// 1. 构建历史位姿点云
for (Pose6D &p : temp_poses_) {
pcl::PointXYZ point;
point.x = p.global_pos(0);
point.y = p.global_pos(1);
point.z = p.global_pos(2);
cloud_history_poses_->push_back(point);
}
// 2. KD树半径搜索
kdtree_history_poses_->setInputCloud(cloud_history_poses_);
std::vector<int> ids;
std::vector<float> sqdists;
kdtree_history_poses_->radiusSearch(
cloud_history_poses_->back(),
loop_params_.loop_pose_search_radius,
ids, sqdists, 0);
// 3. 时间间隔约束
for (int i = 0; i < ids.size(); i++) {
int id = ids[i];
if (std::abs(temp_poses_[id].time - temp_poses_.back().time) >
loop_params_.time_thresh) {
pre_index = id;
break;
}
}
}子地图构建与ICP配准:
// 构建当前帧和历史帧的子地图
fastlio::PointCloudXYZI::Ptr cur_cloud =
getSubMaps(temp_poses_, shared_data_->cloud_history, cur_index, 0);
fastlio::PointCloudXYZI::Ptr sub_maps =
getSubMaps(temp_poses_, shared_data_->cloud_history, pre_index,
loop_params_.submap_search_num);
// ICP精配准
icp_->setInputSource(cur_cloud);
icp_->setInputTarget(sub_maps);
fastlio::PointCloudXYZI::Ptr aligned(new fastlio::PointCloudXYZI);
icp_->align(*aligned, Eigen::Matrix4f::Identity());
// 验证回环质量
float score = icp_->getFitnessScore();
if (!icp_->hasConverged() || score > loop_params_.loop_icp_thresh)
return; // 回环检测失败
// 计算相对变换
Eigen::Matrix4d T_pre_cur = icp_->getFinalTransformation().cast<double>();
Eigen::Matrix3d R12 = temp_poses_[pre_index].global_rot.transpose() *
T_pre_cur.block<3, 3>(0, 0) *
temp_poses_[cur_index].global_rot;
Eigen::Vector3d t12 = temp_poses_[pre_index].global_rot.transpose() *
(T_pre_cur.block<3, 3>(0, 0) * temp_poses_[cur_index].global_pos +
T_pre_cur.block<3, 1>(0, 3) - temp_poses_[pre_index].global_pos);里程计因子添加:
void LoopClosureThread::addOdomFactor() {
for (int i = previous_index_; i < lastest_index_; i++) {
Pose6D &p1 = temp_poses_[i];
Pose6D &p2 = temp_poses_[i + 1];
// 添加先验因子（仅第一帧）
if (i == 0) {
initialized_estimate_.insert(i, gtsam::Pose3(
gtsam::Rot3(p1.local_rot), gtsam::Point3(p1.local_pos)));
gtsam::noiseModel::Diagonal::shared_ptr noise =
gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6::Ones() * 1e-12);
gtsam_graph_.add(gtsam::PriorFactor<gtsam::Pose3>(
i, gtsam::Pose3(gtsam::Rot3(p1.local_rot), gtsam::Point3(p1.local_pos)), noise));
}
// 添加相邻帧之间的里程计因子
initialized_estimate_.insert(i + 1, gtsam::Pose3(
gtsam::Rot3(p2.local_rot), gtsam::Point3(p2.local_pos)));
// 计算相对变换
Eigen::Matrix3d R12 = p1.local_rot.transpose() * p2.local_rot;
Eigen::Vector3d t12 = p1.local_rot.transpose() * (p2.local_pos - p1.local_pos);
// 添加BetweenFactor
gtsam::noiseModel::Diagonal::shared_ptr noise =
gtsam::noiseModel::Diagonal::Variances(
(gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-6).finished());
gtsam_graph_.add(gtsam::BetweenFactor<gtsam::Pose3>(
i, i + 1, gtsam::Pose3(gtsam::Rot3(R12), gtsam::Point3(t12)), noise));
}
}回环因子添加:
void LoopClosureThread::addLoopFactor() {
if (!loop_found_ || shared_data_->loop_pairs.empty())
return;
for (LoopPair &lp : shared_data_->loop_pairs) {
// 构建回环约束
gtsam::Pose3 pose_between(gtsam::Rot3(lp.diff_rot), gtsam::Point3(lp.diff_pos));
// 基于ICP得分的自适应噪声模型
gtsam::noiseModel::Diagonal::shared_ptr loop_noise =
gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6::Ones() * lp.score);
// 添加回环因子
gtsam_graph_.add(gtsam::BetweenFactor<gtsam::Pose3>(
lp.pre_idx, lp.cur_idx, pose_between, loop_noise));
}
shared_data_->loop_pairs.clear();
}Z轴先验因子（可选）:
class ZaxisPriorFactor : public gtsam::NoiseModelFactor1<gtsam::Pose3> {
public:
ZaxisPriorFactor(gtsam::Key key, const gtsam::SharedNoiseModel &noise, double z)
: gtsam::NoiseModelFactor1<gtsam::Pose3>(noise, key), z_(z) {}
virtual gtsam::Vector evaluateError(const gtsam::Pose3 &p,
boost::optional<gtsam::Matrix &> H = boost::none) const {
auto z = p.translation()(2);
if (H) {
gtsam::Matrix Jac = gtsam::Matrix::Zero(1, 6);
Jac << 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; // 仅对Z轴位移求导
*H = Jac;
}
return (gtsam::Vector(1) << z - z_).finished();
}
};void LoopClosureThread::smoothAndUpdate() {
// 第一次更新：添加新因子和变量
isam2_->update(gtsam_graph_, initialized_estimate_);
isam2_->update(); // 执行一次优化迭代
// 回环检测后的多次迭代优化
if (loop_found_) {
for (int i = 0; i < 5; i++) {
isam2_->update(); // 多次迭代确保收敛
}
loop_found_ = false;
}
// 清空临时数据
gtsam_graph_.resize(0);
initialized_estimate_.clear();
// 获取优化结果
optimized_estimate_ = isam2_->calculateBestEstimate();
// 更新最新关键帧的全局位姿
gtsam::Pose3 latest_estimate = optimized_estimate_.at<gtsam::Pose3>(lastest_index_);
temp_poses_[lastest_index_].global_rot = latest_estimate.rotation().matrix().cast<double>();
temp_poses_[lastest_index_].global_pos = latest_estimate.translation().matrix().cast<double>();
// 计算全局偏移
Eigen::Matrix3d offset_rot;
Eigen::Vector3d offset_pos;
temp_poses_[lastest_index_].getOffset(offset_rot, offset_pos);
// 更新共享数据
shared_data_->mutex.lock();
shared_data_->offset_rot = offset_rot;
shared_data_->offset_pos = offset_pos;
// 更新所有历史关键帧的全局位姿
for (int i = 0; i < lastest_index_; i++) {
gtsam::Pose3 temp_pose = optimized_estimate_.at<gtsam::Pose3>(i);
shared_data_->key_poses[i].global_rot = temp_pose.rotation().matrix().cast<double>();
shared_data_->key_poses[i].global_pos = temp_pose.translation().matrix().cast<double>();
}
// 应用偏移到未优化的关键帧
int current_size = shared_data_->key_poses.size();
for (int i = lastest_index_; i < current_size; i++) {
shared_data_->key_poses[i].addOffset(offset_rot, offset_pos);
}
shared_data_->mutex.unlock();
}struct LoopParams {
double rad_thresh = 0.4; // 旋转阈值（弧度）
double dist_thresh = 2.5; // 距离阈值（米）
double time_thresh = 30.0; // 时间间隔阈值（秒）
double loop_pose_search_radius = 10.0; // 回环搜索半径（米）
int loop_pose_index_thresh = 5; // 最小回环间隔（关键帧数）
double submap_resolution = 0.2; // 子地图分辨率（米）
int submap_search_num = 20; // 子地图搜索范围
double loop_icp_thresh = 0.3; // ICP匹配阈值
bool activate = true; // 回环检测开关
};- 增量式优化: ISAM2支持增量式贝叶斯网络优化，避免重复计算
- 自适应噪声: 基于ICP匹配得分动态调整回环约束的噪声模型
- 多层验证: 距离搜索→时间约束→ICP配准→得分验证的多层回环检测
- 异步处理: 后端优化在独立线程中运行，不影响前端实时性
- 全局一致性: 通过因子图优化保证全局轨迹和地图的一致性
ESIKF是FAST-LIO2中用于状态估计的核心算法，它是一种基于误差状态的迭代卡尔曼滤波器，用于融合来自IMU和LiDAR等多种传感器的数据，实现高精度的定位和建图。
传统状态 vs 误差状态:
- 传统状态: 直接估计系统的绝对状态（位置、姿态等）
- 误差状态: 估计真实状态与名义状态之间的误差
优势:
- 避免了直接对非线性状态（如旋转）进行滤波
- 误差状态通常较小，线性化误差更小
- 更好的数值稳定性
MTK工具包的使用:
// 状态定义使用流形表示
typedef MTK::vect<3, double> vect3; // 3维向量
typedef MTK::SO3<double> SO3; // 旋转群SO(3)
typedef MTK::S2<double, 98090, 10000, 1> S2; // 球面S2
// 完整状态向量（24维DOF，23维流形）
MTK_BUILD_MANIFOLD(state_ikfom,
((vect3, pos)) // 位置 (3维)
((SO3, rot)) // 旋转 (3维流形)
((SO3, offset_R_L_I)) // 激光雷达到IMU旋转外参 (3维流形)
((vect3, offset_T_L_I)) // 激光雷达到IMU平移外参 (3维)
((vect3, vel)) // 速度 (3维)
((vect3, bg)) // 陀螺仪偏置 (3维)
((vect3, ba)) // 加速度计偏置 (3维)
((S2, grav))); // 重力向量 (2维球面流形)状态转移函数:
Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in) {
Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
vect3 omega;
in.gyro.boxminus(omega, s.bg); // 角速度 - 陀螺仪偏置
vect3 a_inertial = s.rot * (in.acc - s.ba); // 惯性系加速度
for (int i = 0; i < 3; i++) {
res(i) = s.vel[i]; // 位置导数 = 速度
res(i + 3) = omega[i]; // 旋转导数 = 角速度
res(i + 12) = a_inertial[i] + s.grav[i]; // 速度导数 = 加速度 + 重力
}
return res;
}状态传播:
void esekf::predict(double dt, const cov &Q, const input_ikfom &i_in) {
// 1. 计算状态转移函数
flatted_state f_ = f(x_, i_in);
// 2. 计算雅可比矩阵
cov_ f_x_ = f_x(x_, i_in); // 对状态的雅可比
Matrix<scalar_type, m, process_noise_dof> f_w_ = f_w(x_, i_in); // 对噪声的雅可比
// 3. 状态更新（流形上的加法）
x_.oplus(f_, dt);
// 4. 协方差传播
P_ = f_x_final * P_ * f_x_final.transpose() + f_w_final * Q * f_w_final.transpose();
}观测模型 - 点到平面距离:
void LIOBuilder::sharedUpdateFunc(state_ikfom &state,
esekfom::dyn_share_datastruct<double> &share_data) {
// 1. 点云坐标变换
for (int i = 0; i < size; i++) {
PointType &point_body = cloud_down_lidar_->points[i];
Eigen::Vector3d point_body_vec(point_body.x, point_body.y, point_body.z);
// 激光雷达坐标系 -> IMU坐标系 -> 世界坐标系
Eigen::Vector3d point_world_vec = state.rot.toRotationMatrix() *
(state.offset_R_L_I.toRotationMatrix() * point_body_vec + state.offset_T_L_I) + state.pos;
}
// 2. 最近邻搜索和平面拟合
for (int i = 0; i < effect_feat_num; i++) {
// 平面拟合得到法向量和点面距离
if (esti_plane(pabcd, points_near, 0.1)) {
double pd2 = pabcd(0) * point_world_vec(0) + pabcd(1) * point_world_vec(1) +
pabcd(2) * point_world_vec(2) + pabcd(3);
}
}
// 3. 计算观测雅可比矩阵
for (int i = 0; i < effect_feat_num; i++) {
Eigen::Vector3d temp_vec = state.offset_R_L_I.toRotationMatrix() * laser_p_vec + state.offset_T_L_I;
Eigen::Matrix3d temp_mat = SKEW_SYM_MATRX(temp_vec);
// 对位置的雅可比
share_data.h_x.block<1, 3>(i, 0) = norm_vec.transpose();
// 对旋转的雅可比
Eigen::Matrix<double, 1, 3> B = -norm_vec.transpose() * state.rot.toRotationMatrix() * temp_mat;
share_data.h_x.block<1, 3>(i, 3) = B;
// 对外参的雅可比（如果估计外参）
if (extrinsic_est_en_) {
// 外参旋转雅可比
temp_mat = SKEW_SYM_MATRX(laser_p_vec);
Eigen::Matrix<double, 1, 3> C = -norm_vec.transpose() * state.rot.toRotationMatrix() *
state.offset_R_L_I.toRotationMatrix() * temp_mat;
share_data.h_x.block<1, 3>(i, 6) = C;
// 外参平移雅可比
Eigen::Matrix<double, 1, 3> D = norm_vec.transpose() * state.rot.toRotationMatrix();
share_data.h_x.block<1, 3>(i, 9) = D;
}
// 观测残差
share_data.h(i) = -norm_p.intensity; // 点面距离
}
}迭代卡尔曼滤波:
void esekf::update_iterated_dyn_share_modified(double R, double &solve_time) {
state x_propagated = x_;
cov P_propagated = P_;
// 迭代优化
for(int i = 0; i < maximum_iter; i++) {
// 1. 调用观测函数，计算雅可比和残差
h_dyn_share(x_, dyn_share);
if(!dyn_share.valid) continue;
// 2. 计算卡尔曼增益
Matrix<scalar_type, n, Eigen::Dynamic> K =
P_ * dyn_share.h_x.transpose() *
(dyn_share.h_x * P_ * dyn_share.h_x.transpose() + dyn_share.R).inverse();
// 3. 状态更新
vectorized_state dx = K * (dyn_share.z - dyn_share.h);
x_.boxplus(dx);
// 4. 协方差更新
cov I_KH = cov::Identity() - K * dyn_share.h_x;
P_ = I_KH * P_ * I_KH.transpose() + K * dyn_share.R * K.transpose();
// 5. 收敛性检查
if(dx.norm() < convergence_threshold) break;
}
}旋转群SO(3)操作:
- boxplus: 流形上的"加法"操作，用于状态更新
- boxminus: 流形上的"减法"操作，用于计算误差
- 正确处理旋转的非线性性质
球面S2处理:
- 重力向量约束在单位球面上
- 减少参数化维度（3维→2维）
- 避免奇异性问题
动态观测噪声:
// 基于点面距离和点的距离的自适应权重
double s = 1 - 0.9 * std::fabs(pd2) / std::sqrt(point_body_vec.norm());
if (s > 0.9) {
point_selected_flag_[i] = true; // 选择高质量观测
}IMU-LiDAR紧耦合:
- IMU提供高频运动信息
- LiDAR提供几何约束
- 外参在线估计
- 时间同步处理
- 高精度: 迭代优化处理非线性，流形表示保持几何约束
- 实时性: 高效的矩阵运算和稀疏化处理
- 鲁棒性: 自适应噪声和异常检测
- 可扩展性: 模块化设计支持不同传感器配置
- 高效数据结构: ikd-tree支持O(log n)的插入、删除和搜索
- 流形状态表示: 正确处理旋转群SO(3)和球面S2的几何约束
- 迭代优化: 处理非线性观测模型的迭代卡尔曼滤波
- 自适应地图: 动态局部地图管理，保持计算效率
- 误差状态滤波: ESIKF避免直接对非线性状态进行滤波
- 紧耦合融合: IMU和LiDAR数据在统一框架下融合
- 实时性: 前端ESIKF实现实时状态估计，后端异步优化
- 高精度: 紧耦合融合和迭代优化提供高精度位姿估计
- 鲁棒性: 多层次的异常检测、自适应噪声处理机制
- 可扩展性: 模块化设计支持不同传感器配置
- 全局一致性: GTSAM后端优化保证全局轨迹一致性
- 移动机器人导航: 室内外环境的实时建图和定位
- 自动驾驶车辆: 高精度地图构建和车辆定位
- 无人机SLAM: 空中平台的三维环境感知
- 增强现实(AR): 实时环境理解和虚拟物体叠加
- 工业检测: 三维重建和质量检测
FAST-LIO2通过ESIKF前端和GTSAM后端的结合，实现了IMU和激光雷达的高效紧耦合融合，具有以下核心优势：
- 误差状态表示: 避免非线性状态的直接滤波，提高数值稳定性
- 流形几何处理: 正确处理旋转和重力约束的几何性质
- 迭代优化: 通过迭代减少线性化误差，提高估计精度
- 自适应策略: 动态调整观测权重，增强系统鲁棒性
- 因子图优化: 全局优化保证轨迹和地图的一致性
- 增量式处理: ISAM2实现高效的增量优化
- 回环检测: 自动检测和校正累积误差
- 异步处理: 不影响前端实时性能
- 统一状态表示: 所有传感器信息融合到统一的状态空间
- 连续运动建模: IMU提供高频运动信息，实现精确的运动补偿
- 几何约束优化: 激光雷达提供几何约束，校正IMU累积误差
- 实时性能: 前端实时处理，后端异步优化
- 自适应管理: 动态地图管理和智能点云更新策略
整个系统形成了一个完整的SLAM解决方案，在保证实时性的同时提供了高精度的建图和定位能力，是当前激光雷达-惯性里程计领域的先进技术代表。