【AutowareAuto】ndt

目的/用例

该软件包包含允许使用正态分布变换匹配通过 3D 激光雷达扫描进行定位的结构和功能。 有关实现的更多详细信息，请参阅本文。

设计

地图

算法设计

NDTMap 类似于 voxel_grid，基于 std::unordered_map 并允许快速查找给定点。

NDT 体素图有 2 种主要类型：DynamicNDTMap 和 StaticNDTMap。

DynamicNDTMap 通过将点传递到相应的体素，将原始点云消息转换为 NDT 地图表示。每个体素的质心和协方差都是相对于落入其中的点计算的。协方差和质心计算是根据 Welford 算法在线完成的。

一旦 DynamicNDTMap 将密集点云转换为体素，每个体素的质心和协方差就可以序列化为 PointCloud2 消息，其中生成的点云是稀疏的，并且只是转换后的地图的中间表示。然后可以通过 StaticNDTMap 将该点云转换回 NDT 地图表示，其中不计算质心/协方差，但仅将点插入到其相应的体素中进行查找。输入点云由 point_cloud_msg_wrapper::PointCloudMsgWrapper<> 验证，其中每个点都表示为 PointWithCovariances 类。

输入/输出/API

输入：

• 点云

输出：

• 给定点的一组体素。

Scan

NDT 扫描是一种表示激光雷达扫描的数据结构。 实现取决于优化问题。 所有的实现都必须提供基本的迭代能力，这是所有 ndt 优化问题所必需的。

P2DNDTScan

算法设计

P2DNDTScan 是点向量（Eigen::Vector3d）的包装器。 该类允许通过公开其向量的迭代器来迭代内部容器。

优化问题

P2D优化问题

算法设计

该算法在论文“The Three-Dimensional Normal-Distributions Transform”[Magnusson 2009] 中有详细定义。 实际实现基于 PCL 和 autoware.ai 实现。

CachedExpression 用于表示优化问题。 因此，分数、雅可比和海瑟矩阵可以选择一起计算，以利用源自计算中共享项的协同作用。

输入/输出/API

输入：

• Scan
• 地图输出：
• 分数
• 雅可比
• 海瑟矩阵

NDT 定位器

NDTLocalizerBase 实现了由 LocalizerConstraint 强制执行的接口。 它以扫描、地图和优化问题类型为模板，允许使用不同的 ndt 方法注册测量值。 NDTLocalizerBase 的实现可以选择覆盖消息和猜测验证以及协方差计算步骤。

P2DNDTLocalizer 是 P2D NDT 目标的 NDTLocalizerBase 实现。

输入/输出/API

输入：

• Scan
• 地图
• 初步估计
• 优化器输出
• 带协方差的位姿。

相关问题

• #137: NDT Map format validation
• #138: Implement NDTMapRepresentation
• #136: Implement NDT Map Publisher
• #166: Implement P2D NDT scan