7.3直接法激光雷达里程计
之前的点到点ICP、点到线ICP、点到面ICP、NDT是配准两个点云的。在3D SLAM中，点云之间可以很容易地进行合并，也可以将过去一段时间内的点云组成一个局部地图，然后将当前帧和这个局部地图进行配准。这种不需要提取特征的激光雷达里程计被称为激光雷达里程计。根据使用的配准方法不同，可以写成两种实现激光雷达里程计的思路。
也可不必把过去的点云拼在一起形成局部地图，而是把配准好的点云更新到NDT的每个体素内部，更新它们的高斯分布情况，再做下一步的配准。这种思路被称为增量式NDT。
7.3.1使用NDT构建激光雷达里程计
这个里程计维护一个由多个Scan组成的局部地图，然后把他们拼在一起。调用NDT配准来求解当前帧的位姿。核心目标是通过维护有限数量关键帧组成的局部地图，利用NDT（正态分布变换）配准逐帧估计激光帧的世界坐标。
第一步为添加当前帧 ，若当前帧为第一帧是就初始化局部地图，将位姿设为原点；若不就可进行NDT配准，估计当前帧位姿，若为关键帧（仅当当前帧与上一关键帧的相对运动超过阈值时，才将当前帧标记为关键帧，平移阈值：相对平移距离 > 设定值（如 0.5 米），旋转阈值：相对旋转角度（转弧度后）> 设定值（如 30 度））更新关键帧队列，将关键帧点云转换到世界坐标系后，存入双端队列，：若缓存的关键帧数量超过设定上限（如 30 个），每次新增关键帧后，拼接队列中所有关键帧的世界坐标系点云，将重建后的局部地图设为 NDT 配准的 "目标点云"，为后续帧配准做准备。
第二步为在NDT配准中 首先进行目标点云配置 ，当前帧原始激光点云为源点云（传感器坐标系），局部地图（关键帧拼接的世界坐标系点云）为目标点云，重建局部地图，更新NDT目标点云。之后进行初始位姿猜测 ，前 2 帧：无运动历史，以单位矩阵（世界坐标系原点）作为初始猜测；2 帧后：基于前两帧的位姿趋势推断（当前猜测 = 上一帧位姿 × 上两帧的相对运动），减少 NDT 迭代次数，提升鲁棒性。**之后进行配准执行，**传入初始猜测，执行配准，提取最终变换矩阵转换为 SE3 位姿。
第三步为坐标系转换（传感器系→世界系） 每帧配准得到位姿后，通过pcl::transformPointCloud将当前帧点云从传感器坐标系转换到世界坐标系；所有关键帧均以世界坐标系存储，保证局部地图的配准基准统一
代码：
cpp
、/**
* @brief 基于直接NDT（正态分布变换）递推的激光里程计(Lidar Odometry)
* @note 核心逻辑：维护由历史关键帧组成的局部地图，将当前帧与局部地图做NDT配准以估计位姿；
* 关键帧根据运动距离/角度阈值判定，局部地图仅保留最近N个关键帧
*/
class DirectNDTLO {
public:
/**
* @brief 激光里程计配置参数结构体
*/
struct Options {
Options() {}
double kf_distance_ = 0.5; // 关键帧判定的平移阈值（米）
double kf_angle_deg_ = 30; // 关键帧判定的旋转角度阈值（度）
int num_kfs_in_local_map_ = 30; // 局部地图保留的最大关键帧数量
bool use_pcl_ndt_ = true; // NDT实现选择：true=PCL库NDT，false=自定义Ndt3d
bool display_realtime_cloud_ = true; // 是否启用实时点云可视化
Ndt3d::Options ndt3d_options_; // 自定义Ndt3d的配置参数
};
/**
* @brief 构造函数
* @param options 配置参数（默认值见Options结构体）
*/
DirectNDTLO(Options options = Options()) : options_(options) {
// 初始化实时可视化器（如果启用）
if (options_.display_realtime_cloud_) {
viewer_ = std::make_shared<PCLMapViewer>(0.5); // 0.5为可视化更新频率
}
// 初始化自定义Ndt3d
ndt_ = Ndt3d(options_.ndt3d_options_);
// 初始化PCL NDT参数（固定值，也可放入Options）
ndt_pcl_.setResolution(1.0); // NDT体素网格分辨率（米）
ndt_pcl_.setStepSize(0.1); // 优化步长
ndt_pcl_.setTransformationEpsilon(0.01); // 收敛阈值（两次变换矩阵的差值小于该值则收敛）
}
/**
* @brief 向里程计添加一帧激光点云，核心入口函数
* @param scan 当前帧原始点云（传感器坐标系）
* @param pose 输出参数：当前帧的世界坐标系位姿（由NDT配准估计）
*/
void AddCloud(CloudPtr scan, SE3& pose);
/// 存储地图（viewer中的点云）到文件（声明未实现）
void SaveMap(const std::string& map_path);
private:
/**
* @brief 将当前帧与局部地图做NDT配准，估计当前帧位姿
* @param scan 当前帧原始点云
* @return 当前帧的世界坐标系位姿估计值
*/
SE3 AlignWithLocalMap(CloudPtr scan);
/**
* @brief 判断当前帧是否为关键帧
* @param current_pose 当前帧的世界坐标系位姿
* @return true=是关键帧，false=非关键帧
*/
bool IsKeyframe(const SE3& current_pose);
private:
Options options_; // 配置参数
CloudPtr local_map_ = nullptr; // 局部地图点云（由关键帧拼接而成）
std::deque<CloudPtr> scans_in_local_map_; // 局部地图中的关键帧点云（世界坐标系）
std::vector<SE3> estimated_poses_; // 所有帧的位姿估计结果（用于轨迹记录和位姿猜测）
SE3 last_kf_pose_; // 上一个关键帧的位姿（用于关键帧判定）
// PCL库的NDT配准器
pcl::NormalDistributionsTransform<PointType, PointType> ndt_pcl_;
// 自定义的3D NDT配准器
Ndt3d ndt_;
// 实时点云可视化器
std::shared_ptr<PCLMapViewer> viewer_ = nullptr;
};
/**
* @brief 实现AddCloud：处理当前帧点云，完成配准、关键帧判定、局部地图更新
*/
void DirectNDTLO::AddCloud(CloudPtr scan, SE3& pose) {
// 处理第一帧点云：无局部地图，直接作为初始局部地图
if (local_map_ == nullptr) {
local_map_.reset(new PointCloudType);
*local_map_ += *scan; // 拼接点云（第一帧即为局部地图）
pose = SE3(); // 第一帧位姿设为世界坐标系原点
last_kf_pose_ = pose; // 第一帧作为第一个关键帧
// 设置NDT的目标点云（局部地图）
if (options_.use_pcl_ndt_) {
ndt_pcl_.setInputTarget(local_map_);
} else {
ndt_.SetTarget(local_map_);
}
return;
}
// 非第一帧：与局部地图做NDT配准，估计当前帧位姿
pose = AlignWithLocalMap(scan);
// 将当前帧点云从传感器坐标系转换到世界坐标系
CloudPtr scan_world(new PointCloudType);
pcl::transformPointCloud(*scan, *scan_world, pose.matrix().cast<float>());
// 判断是否为关键帧：若是则更新局部地图
if (IsKeyframe(pose)) {
last_kf_pose_ = pose; // 更新上一关键帧位姿
// 将当前关键帧（世界坐标系）加入局部地图缓存
scans_in_local_map_.emplace_back(scan_world);
// 若关键帧数量超过阈值，移除最早的关键帧（滑动窗口）
if (scans_in_local_map_.size() > options_.num_kfs_in_local_map_) {
scans_in_local_map_.pop_front();
}
// 重建局部地图：拼接所有缓存的关键帧点云
local_map_.reset(new PointCloudType);
for (auto& kf_scan : scans_in_local_map_) {
*local_map_ += *kf_scan;
}
// 更新NDT的目标点云（新的局部地图）
if (options_.use_pcl_ndt_) {
ndt_pcl_.setInputTarget(local_map_);
} else {
ndt_.SetTarget(local_map_);
}
}
// 实时可视化：更新位姿和点云
if (viewer_ != nullptr) {
viewer_->SetPoseAndCloud(pose, scan_world);
}
}
/**
* @brief 实现IsKeyframe：基于与上一关键帧的相对运动判定关键帧
* @note 平移距离超过kf_distance_ 或 旋转角度超过kf_angle_deg_ 则判定为关键帧
*/
bool DirectNDTLO::IsKeyframe(const SE3& current_pose) {
// 计算当前帧与上一关键帧的相对位姿
SE3 delta = last_kf_pose_.inverse() * current_pose;
// 平移阈值判定：平移向量的模长
bool trans_condition = delta.translation().norm() > options_.kf_distance_;
// 旋转阈值判定：旋转向量（so3对数映射）的模长（转弧度）
bool rot_condition = delta.so3().log().norm() > options_.kf_angle_deg_ * math::kDEG2RAD;
return trans_condition || rot_condition;
}
/**
* @brief 实现AlignWithLocalMap：NDT配准核心逻辑，估计当前帧位姿
* @note 位姿猜测策略：
* 1. 前两帧未完成时：用单位矩阵作为初始猜测
* 2. 前两帧完成后：用前两帧的运动趋势推断当前帧初始猜测（运动模型）
*/
SE3 DirectNDTLO::AlignWithLocalMap(CloudPtr scan) {
// 设置NDT的源点云（当前帧）
if (options_.use_pcl_ndt_) {
ndt_pcl_.setInputSource(scan);
} else {
ndt_.SetSource(scan);
}
CloudPtr output(new PointCloudType()); // PCL NDT输出配准后的点云（未使用）
SE3 guess; // NDT配准的初始位姿猜测
bool align_success = true; // 自定义NDT的配准成功标志
// 分情况设置初始猜测并执行NDT配准
if (estimated_poses_.size() < 2) {
// 前两帧：无运动趋势，初始猜测为单位矩阵
if (options_.use_pcl_ndt_) {
// PCL NDT配准：输入初始猜测（单位矩阵），输出配准后点云
ndt_pcl_.align(*output, guess.matrix().cast<float>());
// 提取PCL NDT的最终变换矩阵，转换为SE3
guess = Mat4ToSE3(ndt_pcl_.getFinalTransformation().cast<double>().eval());
} else {
// 自定义NDT配准：传入初始猜测，输出配准后的位姿
align_success = ndt_.AlignNdt(guess);
}
} else {
// 前两帧已完成：用运动趋势推断初始猜测（简单运动模型：假设运动趋势延续）
SE3 T1 = estimated_poses_[estimated_poses_.size() - 1]; // 上一帧位姿
SE3 T2 = estimated_poses_[estimated_poses_.size() - 2]; // 上上帧位姿
guess = T1 * (T2.inverse() * T1); // 推断当前帧初始猜测：T1 * (T2到T1的相对运动)
if (options_.use_pcl_ndt_) {
ndt_pcl_.align(*output, guess.matrix().cast<float>());
guess = Mat4ToSE3(ndt_pcl_.getFinalTransformation().cast<double>().eval());
} else {
align_success = ndt_.AlignNdt(guess);
}
}
// 打印配准结果：平移向量 + 四元数（旋转）
LOG(INFO) << "pose: " << guess.translation().transpose() << ", "
<< guess.so3().unit_quaternion().coeffs().transpose();
// 打印PCL NDT的变换概率（配准可信度）
if (options_.use_pcl_ndt_) {
LOG(INFO) << "trans prob: " << ndt_pcl_.getTransformationProbability();
}
// 记录当前帧位姿估计结果（用于后续运动趋势推断）
estimated_poses_.emplace_back(guess);
return guess;
}
/**
* @brief 存储地图到文件（示例实现，可根据需求修改格式）
* @param map_path 地图保存路径（如.pcd文件）
*/
void DirectNDTLO::SaveMap(const std::string& map_path) {
if (local_map_ != nullptr && !local_map_->empty()) {
pcl::io::savePCDFileBinary(map_path, *local_map_);
LOG(INFO) << "Local map saved to: " << map_path;
} else {
LOG(WARNING) << "Local map is empty, save failed!";
}
}该激光里程计输出的局部地图 是滑动窗口式的、由最近 N 个关键帧拼接而成的世界坐标系点云集合，所有点云都在世界坐标系下。
代码核心思路：
1.整体
测试代码：
cpp
#include "rosbag_io/rosbag_io.h" // 假设包含RosbagIO类，用于读取rosbag
#include "direct_ndt_lo.h" // DirectNDTLO激光里程计类头文件
#include "common/timer.h" // 计时工具类
#include "common/cloud_utils.h" // 点云处理工具（如格式转换、下采样）
#include <gflags/gflags.h> // 命令行参数解析
#include <glog/logging.h> // 日志工具
/// 命令行参数定义（gflags）
/// 本程序以ULHK数据集为例，测试基于NDT的激光里程计（Lidar Odometry）
/// 若使用PCL NDT，会重新构建NDT的体素树
// rosbag文件路径（默认：ULHK数据集test2.bag）
DEFINE_string(bag_path, "./dataset/sad/ulhk/test2.bag", "path to rosbag");
// 数据集类型（支持NCLT/ULHK/KITTI/WXB_3D）
DEFINE_string(dataset_type, "ULHK", "NCLT/ULHK/KITTI/WXB_3D");
// 是否使用PCL库的NDT实现（false则用自定义Ndt3d）
DEFINE_bool(use_pcl_ndt, false, "use pcl ndt to align?");
// NDT邻域搜索类型：是否使用6邻域（false则用中心体素）
DEFINE_bool(use_ndt_nearby_6, false, "use ndt nearby 6?");
// 是否实时显示点云地图
DEFINE_bool(display_map, true, "display map?");
int main(int argc, char** argv) {
// 1. 初始化日志工具（GLOG）
google::InitGoogleLogging(argv[0]);
FLAGS_stderrthreshold = google::INFO; // 输出INFO及以上级别日志到stderr
FLAGS_colorlogtostderr = true; // 日志带颜色输出
// 解析命令行参数（gflags）
google::ParseCommandLineFlags(&argc, &argv, true);
// 2. 初始化Rosbag读取器：关联bag文件和数据集类型
// Str2DatasetType：字符串转数据集类型枚举，适配不同数据集的点云格式/话题
sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path, sad::Str2DatasetType(FLAGS_dataset_type));
// 3. 配置DirectNDTLO激光里程计参数
sad::DirectNDTLO::Options options;
options.use_pcl_ndt_ = fLB::FLAGS_use_pcl_ndt; // 设置是否使用PCL NDT
// 设置NDT邻域搜索类型：6邻域/NDT中心体素
options.ndt3d_options_.nearby_type_ =
FLAGS_use_ndt_nearby_6 ? sad::Ndt3d::NearbyType::NEARBY6 : sad::Ndt3d::NearbyType::CENTER;
options.display_realtime_cloud_ = FLAGS_display_map; // 设置是否实时显示地图
// 初始化激光里程计实例
sad::DirectNDTLO ndt_lo(options);
// 4. 注册点云回调函数，逐帧处理rosbag中的点云
// AddAutoPointCloudHandle：自动匹配数据集对应的点云话题，注册处理函数
rosbag_io
.AddAutoPointCloudHandle([&ndt_lo](sensor_msgs::PointCloud2::Ptr msg) -> bool {
// 计时：统计单帧NDT配准耗时
sad::common::Timer::Evaluate(
[&]() {
// 步骤1：ROS点云格式（PointCloud2）转PCL点云格式，同时下采样（VoxelCloud）
// VoxelCloud：体素下采样，减少点云数量，提升配准效率
auto scan = sad::VoxelCloud(sad::PointCloud2ToCloudPtr(msg));
SE3 pose; // 存储当前帧估计的位姿
// 步骤2：将点云传入激光里程计，执行NDT配准，估计位姿
ndt_lo.AddCloud(scan, pose);
},
"NDT registration"); // 计时标签：NDT配准
return true; // 返回true表示继续处理下一针
})
.Go(); // 启动rosbag读取，逐帧执行回调函数
// 5. 若启用可视化，保存最终的局部地图到PCD文件
if (FLAGS_display_map) {
ndt_lo.SaveMap("./data/ch7/map.pcd");
}
// 6. 打印所有计时结果（如单帧NDT配准耗时、总耗时等）
sad::common::Timer::PrintAll();
LOG(INFO) << "done."; // 程序结束日志
return 0;
}该代码的关键在于NDT配准将处理后的点云传入DirectNDTLO，执行AddCloud完成：
- 与局部地图的 NDT 配准，估计当前帧位姿；
- 关键帧判定与局部地图更新；
- 实时可视化（启用）
之后保存地图。
7.3.2增量式NDT里程计
实现增量式NDT里程计会带来两个问题：一个是如何维护体素；二是每个体素内的高斯参数应该如何改变。
首先考虑高斯参数更新问题 ，设每个体素之前有m个历史点云，他们构成的高斯分布为，
。现在这里又增加了n个点，这n个点的均值和方差为
。设合并之后的估计为
，如何来表示他们之间的关系呢？
之后是体素的增量维护，我们需要维护一个近期使用的缓存机制。设置一个队列模型，把最近需要更新的体素放在最前面。当队列超出预期容量时，就删除最旧的那部分体素。
体素的增量维护代码实现：
cpp
/**
* @class IncNdt3d
* @brief 增量版本的3D NDT（正态分布变换）配准类
* @details 核心特性：
* 1. 内部维护增量式的体素（voxels），自动清理较旧的体素以节省内存
* 2. 向体素添加点云时，动态更新体素内点集的均值和协方差估计
* 3. 基于高斯牛顿法实现NDT配准，支持构建优化用的边（Edge）
*/
class IncNdt3d {
public:
/**
* @enum NearbyType
* @brief 配准时考虑的邻域体素类型
*/
enum class NearbyType {
CENTER, // 仅考虑目标点所在的中心体素
NEARBY6, // 考虑中心体素+6个邻接体素（上下/左右/前后）
};
/**
* @struct Options
* @brief NDT配准的参数配置结构体
*/
struct Options {
int max_iteration_ = 4; ///< 配准最大迭代次数（高斯牛顿）
double voxel_size_ = 1.0; ///< 体素大小（单位：米）
double inv_voxel_size_ = 1.0; ///< 体素大小的倒数（预计算以加速索引计算）
int min_effective_pts_ = 10; ///< 有效配准的最近邻点数阈值（低于此值则配准无效）
int min_pts_in_voxel_ = 5; ///< 每个体素计算NDT的最小点数（低于此值不估计均值/协方差）
int max_pts_in_voxel_ = 50; ///< 每个体素存储的最大点数（超过则不再添加，平衡精度与效率）
double eps_ = 1e-3; ///< 收敛判定阈值（位姿更新量小于此值则停止迭代）
double res_outlier_th_ = 5.0; ///< 残差异常值拒绝阈值（超过此值的残差不计入优化）
size_t capacity_ = 100000; ///< 缓存的体素最大数量（超过则删除最旧的体素）
NearbyType nearby_type_ = NearbyType::NEARBY6; ///< 邻域体素类型选择
};
using KeyType = Eigen::Matrix<int, 3, 1>; ///< 体素索引类型（3维整数向量，对应xyz方向的体素坐标）
/**
* @struct VoxelData
* @brief 单个体素的核心数据结构
* @details 存储体素内的点集、均值、协方差等信息，支持增量更新
*/
struct VoxelData {
/// @brief 默认构造函数（初始化空体素）
VoxelData() {}
/**
* @brief 带初始点的构造函数
* @param pt 初始加入体素的点（世界坐标系）
*/
VoxelData(const Vec3d& pt) {
pts_.emplace_back(pt); // 添加初始点
num_pts_ = 1; // 初始化点数为1
}
/**
* @brief 向体素添加单个点
* @param pt 待添加的点（世界坐标系）
* @details 仅当体素尚未估计NDT（均值/协方差）时，才更新点数；已估计则仅缓存点
*/
void AddPoint(const Vec3d& pt) {
pts_.emplace_back(pt);
if (!ndt_estimated_) { // 未估计NDT时才更新点数
num_pts_++;
}
}
std::vector<Vec3d> pts_; ///< 体素内存储的点集（世界坐标系）
Vec3d mu_ = Vec3d::Zero(); ///< 点集的均值（重心），初始化为零向量
Mat3d sigma_ = Mat3d::Zero(); ///< 点集的协方差矩阵，初始化为零矩阵
Mat3d info_ = Mat3d::Zero(); ///< 协方差矩阵的逆（信息矩阵），预计算以加速配准
bool ndt_estimated_ = false; ///< 标记：是否已估计该体素的均值和协方差
int num_pts_ = 0; ///< 体素内的总点数（用于判断是否满足NDT估计条件）
};
/**
* @brief 默认构造函数
* @details 使用默认参数配置，初始化体素大小倒数和邻域体素列表
*/
IncNdt3d() {
options_.inv_voxel_size_ = 1.0 / options_.voxel_size_;
GenerateNearbyGrids();
}
/**
* @brief 带参数的构造函数
* @param options 自定义的NDT配置参数
* @details 初始化参数配置、体素大小倒数和邻域体素列表
*/
IncNdt3d(Options options) : options_(options) {
options_.inv_voxel_size_ = 1.0 / options_.voxel_size_;
GenerateNearbyGrids();
}
/// @brief 获取当前维护的体素数量（统计信息）
/// @return 体素总数
int NumGrids() const { return grids_.size(); }
/**
* @brief 向增量体素中添加点云（世界坐标系）
* @param cloud_world 待添加的点云指针（世界坐标系）
* @details 核心逻辑：
* 1. 将点云分配到对应体素中
* 2. 自动清理超过容量的旧体素
* 3. 触发体素的均值/协方差更新（满足点数条件时）
*/
void AddCloud(CloudPtr cloud_world);
/**
* @brief 设置待配准的源点云（需要配准到目标体素地图的点云）
* @param source 源点云指针（局部坐标系）
*/
void SetSource(CloudPtr source) { source_ = source; }
/**
* @brief 使用高斯牛顿法执行NDT配准
* @param init_pose 配准初始位姿（输入）；配准结果位姿（输出）
* @return 配准是否成功（true=成功，false=失败，如迭代不收敛/有效点数不足）
*/
bool AlignNdt(SE3& init_pose);
/**
* @brief 计算给定位姿下的残差和雅可比矩阵（符合IEKF公式）
* @param pose 当前估计的位姿（SE3）
* @param HTVH 输出：雅可比加权乘积矩阵（18x18，对应6DoF位姿的二阶信息）
* @param HTVr 输出：雅可比加权残差向量（18x1，对应6DoF位姿的一阶信息）
* @details 公式对应IEKF（迭代扩展卡尔曼滤波）的8.17和8.19式，用于高斯牛顿迭代更新
*/
void ComputeResidualAndJacobians(const SE3& pose, Mat18d& HTVH, Vec18d& HTVr);
/**
* @brief 基于当前估计的NDT体素构建优化用的边（Edge）
* @param v 位姿顶点（优化变量）
* @param edges 输出：构建的NDT边列表（需外部管理内存）
* @details 用于图优化框架（如g2o/Ceres），将NDT约束转化为优化边
*/
void BuildNDTEdges(VertexPose* v, std::vector<EdgeNDT*>& edges);
private:
/**
* @brief 根据邻域类型生成需要考虑的邻域体素索引偏移
* @details 例如NearbyType::NEARBY6会生成6个方向的偏移（如(1,0,0), (-1,0,0)等），
* 结果存储在nearby_grids_中，供配准时快速查询邻域体素
*/
void GenerateNearbyGrids();
/**
* @brief 更新体素内部的均值和协方差
* @param v 待更新的体素数据
* @details 核心逻辑：
* 1. 检查体素内点数是否满足最小阈值（min_pts_in_voxel_）
* 2. 计算点集的均值（mu_）
* 3. 计算点集的协方差矩阵（sigma_）
* 4. 计算协方差的逆（info_），并处理奇异值（防止矩阵不可逆）
* 5. 标记ndt_estimated_为true，避免重复计算
*/
void UpdateVoxel(VoxelData& v);
CloudPtr source_ = nullptr; ///< 待配准的源点云（局部坐标系）
Options options_; ///< NDT配准参数配置
using KeyAndData = std::pair<KeyType, VoxelData>; ///< 体素索引-数据对（方便list存储）
std::list<KeyAndData> data_; ///< 体素数据链表（按添加顺序存储，方便删除旧体素）
/// 体素索引映射表（哈希表）：体素索引 -> 链表迭代器（快速查找体素）
std::unordered_map<KeyType, std::list<KeyAndData>::iterator, hash_vec<3>> grids_;
std::vector<KeyType> nearby_grids_; ///< 邻域体素的索引偏移列表（如(0,0,0), (1,0,0)等）
bool flag_first_scan_ = true; ///< 首帧点云标记：首帧点云直接作为初始体素地图，不执行配准
};在 IncNdt3d 类中,把实际的体素数据放在双向链表中,然后在哈希表中维护它们的迭代器作为查找索引。当用户在里程计中添加新的点云时，再去维护体素数据、更新它们内部的高斯分布并且维护缓存队列。其中，体素内部的高斯分布的更新是可以并发的，通过并发函数来调用它。
cpp
void IncNdt3d::AddCloud(CloudPtr cloud_world) {
// 记录本次更新中被修改的体素（用set去重，less_vec<3>是3维向量的比较器）
std::set<KeyType, less_vec<3>> active_voxels;
// 遍历点云的每个点，分配到对应体素
for (const auto& p : cloud_world->points) {
// 将点云的点转换为Eigen向量（适配内部数据结构）
auto pt = ToVec3d(p);
// 计算点对应的体素索引：点坐标 * 体素大小倒数 → 取整得到整数索引
auto key = CastToInt(Vec3d(pt * options_.inv_voxel_size_));
// 查找该体素是否已存在
auto iter = grids_.find(key);
if (iter == grids_.end()) {
// 场景1：体素不存在 → 创建新体素
// 1. 向链表头部添加新体素（新体素标记为最新），初始点为当前点
data_.push_front({key, {pt}});
// 2. 向哈希表中添加索引：体素key → 链表迭代器（快速查找）
grids_.insert({key, data_.begin()});
// 3. 检查体素总数是否超过容量 → 超过则删除最旧的体素（LRU策略）
if (data_.size() >= options_.capacity_) {
// 从哈希表中删除最旧体素的索引
grids_.erase(data_.back().first);
// 从链表尾部删除最旧的体素（链表尾部是最久未使用的）
data_.pop_back();
}
} else {
// 场景2：体素已存在 → 向体素添加点，并更新LRU顺序
// 1. 向已有体素中添加当前点
iter->second->second.AddPoint(pt);
// 2. 将该体素移到链表头部（标记为最新使用，避免被优先删除）
data_.splice(data_.begin(), data_, iter->second);
// 3. 更新哈希表中的迭代器（指向链表新位置）
iter->second = data_.begin();
}
// 记录当前体素为活跃体素（后续需要更新其均值/协方差）
active_voxels.emplace(key);
}
// 并行更新所有活跃体素的均值、协方差和信息矩阵（par_unseq：并行+无序执行，提升效率）
std::for_each(std::execution::par_unseq, active_voxels.begin(), active_voxels.end(),
[this](const auto& key) {
// 从哈希表找到体素迭代器，更新体素数据
UpdateVoxel(grids_[key]->second);
});
// 首帧点云处理完成 → 后续点云需要执行配准，而非直接添加
flag_first_scan_ = false;
}
/**
* @brief 更新单个体素的均值、协方差和信息矩阵（核心：区分首帧/非首帧，增量更新）
* @param v 待更新的体素数据（引用传递，直接修改）
* @details 核心逻辑：
* 1. 首帧处理：简化计算，保证初始体素地图可用；
* 2. 非首帧：
* - 已估计且点数超最大 → 跳过（避免体素点数过多导致计算量激增）；
* - 未估计且点数达标 → 首次计算均值/协方差；
* - 已估计且新点达标 → 增量更新均值/协方差，SVD处理防止矩阵奇异。
* 关键技巧：
* - 正则项（1e-3）：避免协方差矩阵不可逆；
* - SVD分解：修正奇异值，保证信息矩阵（协方差逆）有效；
* - 清空pts_：节省内存（均值/协方差已计算，无需保留原始点）。
*/
void IncNdt3d::UpdateVoxel(VoxelData& v) {
// ========== 场景1：首帧点云的体素更新（简化逻辑） ==========
if (flag_first_scan_) {
if (v.pts_.size() > 1) {
// 体素内点数>1 → 计算均值和协方差（transform为恒等函数，直接用原始点）
math::ComputeMeanAndCov(v.pts_, v.mu_, v.sigma_, [this](const Vec3d& p) { return p; });
// 计算信息矩阵（协方差逆）：加1e-3*单位矩阵（正则项），避免协方差奇异导致逆不存在
v.info_ = (v.sigma_ + Mat3d::Identity() * 1e-3).inverse();
} else {
// 体素内仅1个点 → 均值为该点，信息矩阵设为1e2*单位矩阵（简化处理）
v.mu_ = v.pts_[0];
v.info_ = Mat3d::Identity() * 1e2;
}
// 标记该体素已完成NDT估计（避免重复计算）
v.ndt_estimated_ = true;
// 清空原始点集（节省内存，均值/协方差已存储）
v.pts_.clear();
return;
}
// ========== 场景2：非首帧点云的体素更新 ==========
// 跳过条件：体素已估计且总点数超过最大值 → 不再更新（平衡精度与效率）
if (v.ndt_estimated_ && v.num_pts_ > options_.max_pts_in_voxel_) {
return;
}
// 子场景1：体素未估计NDT，且当前点集数量达标 → 首次计算均值/协方差
if (!v.ndt_estimated_ && v.pts_.size() > options_.min_pts_in_voxel_) {
// 计算点集的均值和协方差
math::ComputeMeanAndCov(v.pts_, v.mu_, v.sigma_, [this](const Vec3d& p) { return p; });
// 计算信息矩阵（加正则项避免奇异）
v.info_ = (v.sigma_ + Mat3d::Identity() * 1e-3).inverse();
// 标记为已估计
v.ndt_estimated_ = true;
// 清空原始点集
v.pts_.clear();
}
// 子场景2：体素已估计NDT，且新增点集数量达标 → 增量更新分布
else if (v.ndt_estimated_ && v.pts_.size() > options_.min_pts_in_voxel_) {
// 临时变量：存储新增点集的均值/协方差、合并后的均值/协方差
Vec3d cur_mu, new_mu;
Mat3d cur_var, new_var;
// 1. 计算本次新增点集的均值和协方差
math::ComputeMeanAndCov(v.pts_, cur_mu, cur_var, [this](const Vec3d& p) { return p; });
// 2. 增量合并：将旧体素的分布与新增点集的分布合并（更新均值和协方差）
math::UpdateMeanAndCov(v.num_pts_, v.pts_.size(), v.mu_, v.sigma_, cur_mu, cur_var, new_mu, new_var);
// 3. 更新体素的均值和协方差
v.mu_ = new_mu;
v.sigma_ = new_var;
// 4. 更新体素总点数（旧点数 + 新增点数）
v.num_pts_ += v.pts_.size();
// 5. 清空新增点集（节省内存）
v.pts_.clear();
// 6. 修正协方差矩阵的奇异值，保证信息矩阵有效
// SVD分解：将协方差矩阵分解为 U * Σ * V^T（Σ为奇异值对角矩阵）
Eigen::JacobiSVD svd(v.sigma_, Eigen::ComputeFullU | Eigen::ComputeFullV);
Vec3d lambda = svd.singularValues(); // 获取奇异值（Σ的对角线元素）
// 修正小奇异值：避免奇异值过小导致逆矩阵爆炸（nan/inf）
if (lambda[1] < lambda[0] * 1e-3) {
lambda[1] = lambda[0] * 1e-3;
}
if (lambda[2] < lambda[0] * 1e-3) {
lambda[2] = lambda[0] * 1e-3;
}
// 7. 重新计算信息矩阵（协方差逆）：利用SVD分解结果重构逆矩阵
Mat3d inv_lambda = Vec3d(1.0 / lambda[0], 1.0 / lambda[1], 1.0 / lambda[2]).asDiagonal();
v.info_ = svd.matrixV() * inv_lambda * svd.matrixU().transpose();
}
}每个体素会带有一个标记，标志它内部的高斯参数是否已经被估计。为每个体素设计一个缓冲区，当缓冲区中的点数大于一定数量时，才估计它的高斯参数。同时,新加人的点也会放入这个缓冲区，达到一定数量后会重新估计。此外，如果一个栅格里的累积点数已经有很多，就不再对其进行更新，这也可以节省一定的计算量。
高斯更新的实现代码：
由此可得在实现局部地图构建时，只需去一些关键帧，不断地往NDT内部添加新关键帧，然后效用函数即可，不用去拼接关键帧点云。
cpp
// 前置声明（保证代码完整性）
namespace sad { // 对应原代码的namespace
using PointCloudType = pcl::PointCloud<pcl::PointXYZIRT>; // 示例：激光点云类型（可根据实际调整）
using CloudPtr = PointCloudType::Ptr; // 点云智能指针类型
struct PCLMapViewer; // 点云可视化器前置声明
namespace math {
const double kDEG2RAD = M_PI / 180.0; // 角度转弧度常量
} // namespace math
/**
* @class IncrementalNDTLO
* @brief 基于直接NDT方法的递推式激光里程计（Lidar Odometry, LO）
* @details 核心特性：
* 1. 以"关键帧"为单位构建局部地图（Local Map），而非使用全部历史帧，平衡精度与效率；
* 2. 利用增量式NDT（IncNdt3d）完成当前帧到局部地图的配准，估计位姿；
* 3. 支持实时可视化点云和轨迹，支持地图保存；
* 4. 自动判定关键帧（基于平移/旋转阈值、帧数阈值），更新局部地图。
*/
class IncrementalNDTLO {
public:
/**
* @struct Options
* @brief 激光里程计的配置参数结构体
*/
struct Options {
/// @brief 默认构造函数（初始化默认参数）
Options() {}
double kf_distance_ = 0.5; ///< 关键帧判定的平移阈值（米）：当前帧与上一关键帧平移超过此值则设为关键帧
double kf_angle_deg_ = 30; ///< 关键帧判定的旋转角度阈值（度）：当前帧与上一关键帧旋转超过此值则设为关键帧
bool display_realtime_cloud_ = true; ///< 是否启用实时点云可视化（依赖PCLMapViewer）
IncNdt3d::Options ndt3d_options_; ///< 增量NDT的配置参数（体素大小、迭代次数等）
};
/**
* @brief 构造函数（带默认配置）
* @param options 激光里程计配置参数（默认使用结构体默认值）
* @details 初始化逻辑：
* 1. 保存配置参数；
* 2. 如果启用实时可视化，创建PCL点云可视化器；
* 3. 初始化增量NDT对象（传入NDT配置）。
*/
IncrementalNDTLO(Options options = Options()) : options_(options) {
if (options_.display_realtime_cloud_) {
// 创建PCL地图可视化器，参数0.5为可视化窗口的点云显示分辨率（示例）
viewer_ = std::make_shared<PCLMapViewer>(0.5);
}
// 初始化增量NDT对象（核心配准器）
ndt_ = IncNdt3d(options_.ndt3d_options_);
}
/**
* @brief 向激光里程计中添加一帧激光点云，估计该帧的位姿
* @param scan 当前帧激光点云（局部坐标系，即激光雷达坐标系）
* @param pose 输出：该帧的估计位姿（世界坐标系，输入时可传猜测值）
* @param use_guess 是否使用外部传入的位姿猜测值（默认false）
* @details 核心逻辑：
* 1. 首帧处理：位姿设为单位矩阵，直接加入局部地图；
* 2. 非首帧：
* - 设置NDT的源点云（当前帧）；
* - 生成位姿猜测值（历史两帧递推 / 外部传入）；
* - 调用NDT配准估计位姿；
* - 将当前帧点云变换到世界坐标系；
* - 判定是否为关键帧，若是则加入局部地图并更新关键帧位姿；
* - 实时可视化（若启用）；
* - 累计帧数。
*/
void AddCloud(CloudPtr scan, SE3& pose, bool use_guess = false);
/**
* @brief 保存当前的点云地图（依赖可视化器）
* @param map_path 地图保存路径（如"./map.pcd"）
*/
void SaveMap(const std::string& map_path);
private:
/**
* @brief 判定当前帧是否为关键帧
* @param current_pose 当前帧的估计位姿（世界坐标系）
* @return true=是关键帧，false=非关键帧
* @details 关键帧判定条件（满足任一即成立）：
* 1. 距离上一关键帧的帧数超过10帧（强制关键帧，避免长时间不更新局部地图）；
* 2. 与上一关键帧的平移距离超过kf_distance_；
* 3. 与上一关键帧的旋转角度超过kf_angle_deg_（转弧度后）。
*/
bool IsKeyframe(const SE3& current_pose);
private:
Options options_; ///< 激光里程计配置参数
bool first_frame_ = true; ///< 首帧标记：首帧位姿设为原点，直接加入局部地图
std::vector<SE3> estimated_poses_; ///< 存储所有帧的估计位姿（用于轨迹记录、位姿猜测）
SE3 last_kf_pose_; ///< 上一关键帧的位姿（用于关键帧判定）
int cnt_frame_ = 0; ///< 距离上一关键帧的累计帧数
IncNdt3d ndt_; ///< 增量NDT配准器（核心：维护局部地图，执行配准）
std::shared_ptr<PCLMapViewer> viewer_ = nullptr; ///< 点云可视化器（实时显示点云/轨迹）
};
/**
* @brief 核心函数：添加一帧点云并估计位姿
* @param scan 当前帧点云（激光雷达坐标系）
* @param pose 输出位姿（世界坐标系）；若use_guess=true，输入为猜测位姿
* @param use_guess 是否使用外部猜测位姿（默认false：用历史位姿递推）
*/
void IncrementalNDTLO::AddCloud(CloudPtr scan, SE3& pose, bool use_guess) {
// ========== 场景1：首帧处理（初始化） ==========
if (first_frame_) {
pose = SE3(); // 首帧位姿设为单位矩阵（世界坐标系原点）
last_kf_pose_ = pose; // 首帧作为第一个关键帧
ndt_.AddCloud(scan); // 首帧点云直接加入NDT的局部地图
first_frame_ = false; // 标记首帧处理完成
return;
}
// ========== 场景2：非首帧处理（位姿估计） ==========
SE3 guess; // 位姿猜测值（用于NDT配准的初始值，提升收敛速度）
ndt_.SetSource(scan); // 将当前帧设为NDT的源点云（待配准点云）
// 生成位姿猜测值：
if (estimated_poses_.size() < 2) {
// 历史帧不足2帧 → 无递推依据，猜测值设为单位矩阵
ndt_.AlignNdt(guess);
} else {
if (!use_guess) {
// 不使用外部猜测 → 从最近两帧位姿递推（匀速运动假设）
SE3 T1 = estimated_poses_[estimated_poses_.size() - 1]; // 上一帧位姿
SE3 T2 = estimated_poses_[estimated_poses_.size() - 2]; // 上上帧位姿
// 递推公式：guess = T1 * (T2⁻¹ * T1) → 假设运动趋势与上两帧一致
guess = T1 * (T2.inverse() * T1);
} else {
// 使用外部传入的猜测位姿（如IMU预积分结果）
guess = pose;
}
// 调用增量NDT配准：以guess为初始值，估计当前帧位姿
ndt_.AlignNdt(guess);
}
// 更新当前帧的估计位姿
pose = guess;
// 记录当前帧位姿（用于后续递推、轨迹可视化）
estimated_poses_.emplace_back(pose);
// 将当前帧点云从激光坐标系变换到世界坐标系（用于关键帧添加/可视化）
CloudPtr scan_world(new PointCloudType);
pcl::transformPointCloud(*scan, *scan_world, guess.matrix().cast<float>());
// 判定当前帧是否为关键帧 → 若是则更新局部地图
if (IsKeyframe(pose)) {
last_kf_pose_ = pose; // 更新上一关键帧位姿
cnt_frame_ = 0; // 重置关键帧帧数计数器
ndt_.AddCloud(scan_world); // 将关键帧点云加入NDT的局部地图
}
// 实时可视化（若启用）：显示当前帧位姿和世界坐标系下的点云
if (viewer_ != nullptr) {
viewer_->SetPoseAndCloud(pose, scan_world);
}
// 累计距离上一关键帧的帧数
cnt_frame_++;
}
/**
* @brief 关键帧判定函数：满足以下任一条件则为关键帧
* 1. 距离上一关键帧超过10帧；
* 2. 平移距离超过阈值；
* 3. 旋转角度超过阈值（转弧度后）。
*/
bool IncrementalNDTLO::IsKeyframe(const SE3& current_pose) {
// 条件1：强制关键帧（避免局部地图长时间不更新）
if (cnt_frame_ > 10) {
return true;
}
// 计算当前帧与上一关键帧的相对位姿
SE3 delta = last_kf_pose_.inverse() * current_pose;
// 条件2：平移距离阈值 | 条件3：旋转角度阈值（角度转弧度）
return delta.translation().norm() > options_.kf_distance_ ||
delta.so3().log().norm() > options_.kf_angle_deg_ * math::kDEG2RAD;
}
/**
* @brief 保存地图：委托可视化器完成（仅当可视化器存在时有效）
* @param map_path 地图保存路径（PCD格式）
*/
void IncrementalNDTLO::SaveMap(const std::string& map_path) {
if (viewer_) {
viewer_->SaveMap(map_path);
}
}
} // namespace sad构建结果：
ps俺不中嘞，实习半周啥都没学，难受