7.4特征法激光雷达里程计
7.4.1特征提取
7.3中直接将点云进行配准的方法（利用关键帧或者是体素进行配准）称为直接里程计。与之相对的，自动驾驶中也经常使用先提取特征，再做配准的激光雷达里程计，这种称为特征法激光雷达里程计，方法为先对点云提取一些简单的特征，之后对特征点进行配准，同时也要根据特征点本身的不同性质，采取不同的配准方法，使之更加精准。那我们要探讨第一步什么样的特征是对实时配准是有意义的？实时SLAM对特征有四个要求：1.特征应该能反应点云的特点。2.在提取特征之后，应该很容易对这些特征点进行几何的配准。3.特征提取不应占用太多CPU或者GPU资源，也不应使用特殊的硬件。4.在系统设计上，我们希望能够整个激光雷达里程计或者SLAM系统使用同样的计算结构，而不是一部分在CPU上，另一部分在GPU上，造成不必要的数据传输和资源占用。
另外，在进行特征提取时，大部分系统会使用线束信息进行特征提取。
7.4.2基于激光雷达线束的特征提取
多线激光雷达天生带有线束这一信息。我们从激光雷达中得到点云，除了这样的位置，我们也可以得到其他的信息，例如：1.某个激光点来自哪一条扫描线2.同一条线上的激光点的先后顺序。3.有些驱动也会输出各点的极坐标角度、扫描时间等精准信息。同时依据这些信息可以为我们省去一大部分工作，比如知道哪些点处于同一条扫描线，以及它们的时间顺序，最明显的是可以省去寻找同一条扫描线上的最近邻工作。其次，根据这些点的先后顺序，可以计算曲率，然后根据曲率判断这些点的类型。曲率本身的计算可以定义为同一根线束上的某个点与其他近邻点之间的差值。
关于曲率，在LOAM系统的工作中，研究人员给出了一种非常简单的判别方法：沿着同一根线束，如果曲率较大，就认为是角点；如果曲率较小，就认为是平面点。为了取到最明显的角点与平面点，可以根据当前点云的曲率情况，取最大和最小的几个采样点作为特征点。在多线激光雷达的扫描中，角点一般位于垂直物体的表面，或者两个平面的交界面上，在垂直方向适合使用点到线 ICP。此外，平面点在不同线束上采样，就可以使用点到面ICP进行配准。比如相邻线束的角点可以构成物体的直线特征，相邻线束的平面点可以构成平面。
一定限度地提取特征，有助于进一步优化配准结果，然而，这种基于线束的方法实际上利用了很多先验信息，如点云由若干条扫描线组成，激光雷达是水平放置的，等等。它们的实现往往非常工程化，依赖于每种激光雷达的扫描角度、线束信息。这种假设在固态激光雷达，或者RGBD相机点云中无法正常工作，也限制了此类里程计的使用范围。
7.4.3特征提取部分的实现
基于线束的特征提取需要知道点云本身的线束信息（可以从驱动程序中获取，也可以根据每个点的坐标计算该点的俯仰角度，然后通过雷达自身每条线的俯仰角度信息，推断该点属于哪个线束）。在高老师书中提供了自带线束信息的点云，只需关注如何计算角点和平面点即可。
特征提取的算法主要包括以下几个部分：1.计算每个点的线束并归类。由于点云中已携带了这部分信息，因此这步可以跳过。2.依次计算线束中每个点的曲率。3.把点云按一周分成若干个区间（现实中取6个区间）。选择其中曲率最大的若干个点作为角点。剩余的点作为平面点。
**大多数特征提取方法并没有明确的理论依据，更多是通过工程实践摸索。（此处应关注调整哪些参数可以满足工程需求）。**代码实现
cpp
/**
* @brief 激光雷达点云特征提取核心逻辑
* 步骤：1. 按激光线（ring）拆分点云 2. 计算每个点的曲率 3. 分6个扇区提取边缘/平面点
*/
void FeatureExtraction::Extract(FullCloudPtr pc_in, CloudPtr pc_out_edge, CloudPtr pc_out_surf) {
int num_scans = 16; // 激光雷达线数（16线激光雷达）
// 存储每条激光线的点云（索引对应ring值，0~15）
std::vector<CloudPtr> scans_in_each_line;
for (int i = 0; i < num_scans; i++) {
scans_in_each_line.emplace_back(new pcl::PointCloud<PointType>);
}
// 1. 按激光线（ring）拆分输入点云
for (auto &pt : pc_in->points) {
// 断言：确保ring值在合法范围内（防止数组越界）
assert(pt.ring >= 0 && pt.ring < num_scans);
PointType p;
// 复制点的核心属性（坐标+强度）
p.x = pt.x;
p.y = pt.y;
p.z = pt.z;
p.intensity = pt.intensity;
// 将点放入对应激光线的点云容器中
scans_in_each_line[pt.ring]->points.emplace_back(p);
}
// 2. 逐激光线处理：计算曲率 + 分扇区提取特征点
for (int i = 0; i < num_scans; i++) {
// 跳过点数过少的激光线（少于131个点时，无法计算5邻域曲率）
if (scans_in_each_line[i]->points.size() < 131) {
continue;
}
// 存储当前激光线每个点的「索引-曲率」对
std::vector<IdAndValue> cloud_curvature;
// 有效点数：排除前后各5个点（因为曲率计算需要5邻域）
int total_points = scans_in_each_line[i]->points.size() - 10;
// 计算每个有效点的曲率（基于邻域10个点的偏差）
// 遍历范围：5 ~ (总点数-6)，确保j-5和j+5都不越界
for (int j = 5; j < (int)scans_in_each_line[i]->points.size() - 5; j++) {
// 曲率计算逻辑：当前点与前后各5个点的坐标偏差平方和
// 公式：diff = (Σ(邻域点坐标) - 10*当前点坐标)，曲率=diffX²+diffY²+diffZ²
// 物理意义：值越大，点越偏离邻域拟合的平面，越可能是边缘点
double diffX = scans_in_each_line[i]->points[j - 5].x + scans_in_each_line[i]->points[j - 4].x +
scans_in_each_line[i]->points[j - 3].x + scans_in_each_line[i]->points[j - 2].x +
scans_in_each_line[i]->points[j - 1].x - 10 * scans_in_each_line[i]->points[j].x +
scans_in_each_line[i]->points[j + 1].x + scans_in_each_line[i]->points[j + 2].x +
scans_in_each_line[i]->points[j + 3].x + scans_in_each_line[i]->points[j + 4].x +
scans_in_each_line[i]->points[j + 5].x;
double diffY = scans_in_each_line[i]->points[j - 5].y + scans_in_each_line[i]->points[j - 4].y +
scans_in_each_line[i]->points[j - 3].y + scans_in_each_line[i]->points[j - 2].y +
scans_in_each_line[i]->points[j - 1].y - 10 * scans_in_each_line[i]->points[j].y +
scans_in_each_line[i]->points[j + 1].y + scans_in_each_line[i]->points[j + 2].y +
scans_in_each_line[i]->points[j + 3].y + scans_in_each_line[i]->points[j + 4].y +
scans_in_each_line[i]->points[j + 5].y;
double diffZ = scans_in_each_line[i]->points[j - 5].z + scans_in_each_line[i]->points[j - 4].z +
scans_in_each_line[i]->points[j - 3].z + scans_in_each_line[i]->points[j - 2].z +
scans_in_each_line[i]->points[j - 1].z - 10 * scans_in_each_line[i]->points[j].z +
scans_in_each_line[i]->points[j + 1].z + scans_in_each_line[i]->points[j + 2].z +
scans_in_each_line[i]->points[j + 3].z + scans_in_each_line[i]->points[j + 4].z +
scans_in_each_line[i]->points[j + 5].z;
// 计算曲率值（偏差平方和），并存储点索引和曲率
IdAndValue distance(j, diffX * diffX + diffY * diffY + diffZ * diffZ);
cloud_curvature.push_back(distance);
}
// 3. 将当前激光线的点云按360°角度分为6个扇区（每个扇区60°），逐扇区提取特征
for (int j = 0; j < 6; j++) {
int sector_length = (int)(total_points / 6); // 每个扇区的点数
int sector_start = sector_length * j; // 扇区起始索引
int sector_end = sector_length * (j + 1) - 1; // 扇区结束索引
// 最后一个扇区：修正结束索引（避免点数整除不均导致的遗漏）
if (j == 5) {
sector_end = total_points - 1;
}
// 截取当前扇区的「索引-曲率」数据
std::vector<IdAndValue> sub_cloud_curvature(cloud_curvature.begin() + sector_start,
cloud_curvature.begin() + sector_end);
// 对当前扇区提取边缘点和平面点
ExtractFromSector(scans_in_each_line[i], sub_cloud_curvature, pc_out_edge, pc_out_surf);
}
}
}
/**
* @brief 单扇区特征点提取逻辑
* 规则：1. 曲率从大到小选边缘点（最多20个），并排除边缘点邻域 2. 剩余低曲率点选为平面点
*/
void FeatureExtraction::ExtractFromSector(const CloudPtr &pc_in, std::vector<IdAndValue> &cloud_curvature,
CloudPtr &pc_out_edge, CloudPtr &pc_out_surf) {
// 按曲率升序排序（cloud_curvature[0]曲率最小，最后一个曲率最大）
std::sort(cloud_curvature.begin(), cloud_curvature.end(),
[](const IdAndValue &a, const IdAndValue &b) { return a.value_ < b.value_; });
int largest_picked_num = 0; // 已选取的边缘点数量
int point_info_count = 0; // 临时计数（未实际使用，可考虑删除）
// 标记已选中的边缘点（及其邻域），避免重复选取
std::vector<int> picked_points;
/// 第一步：提取边缘点（曲率最大的点，对应物体边缘/角点）
// 从曲率最大的点开始遍历（倒序遍历排序后的数组）
for (int i = cloud_curvature.size() - 1; i >= 0; i--) {
int ind = cloud_curvature[i].id_; // 当前点在激光线中的索引
// 跳过已被标记的点（边缘点或其邻域）
if (std::find(picked_points.begin(), picked_points.end(), ind) == picked_points.end()) {
// 曲率阈值过滤：小于0.1的点不视为边缘点
if (cloud_curvature[i].value_ <= 0.1) {
break;
}
largest_picked_num++; // 边缘点计数+1
picked_points.push_back(ind); // 标记为已选
// 每个扇区最多选20个边缘点
if (largest_picked_num <= 20) {
pc_out_edge->push_back(pc_in->points[ind]); // 加入边缘点云
point_info_count++;
} else {
break;
}
// 标记当前边缘点的后5个邻域点（避免重复选取）
// 邻域判断规则：相邻点距离平方>0.05时，视为不同特征区域，停止标记
for (int k = 1; k <= 5; k++) {
double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x;
double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y;
double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
picked_points.push_back(ind + k);
}
// 标记当前边缘点的前5个邻域点（规则同上）
for (int k = -1; k >= -5; k--) {
double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x;
double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y;
double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
break;
}
picked_points.push_back(ind + k);
}
}
}
/// 第二步：提取平面点（未被标记的低曲率点，对应平面/地面）
// 遍历曲率升序的点（从最小开始），选取未被标记的点作为平面点
for (int i = 0; i <= (int)cloud_curvature.size() - 1; i++) {
int ind = cloud_curvature[i].id_;
// 未被标记为边缘点/邻域点 → 视为平面点
if (std::find(picked_points.begin(), picked_points.end(), ind) == picked_points.end()) {
pc_out_surf->push_back(pc_in->points[ind]);
}
}
}在室内结构环境中，角点和平面点可以被准确地提取。角点通常分布于建筑物的棱上，平面点则位于建筑的面上。对于室外开开阔场景，平面点通常效果较好，角点稍微差一些。另外地面上的点通常不呈直线分布，而是呈圆周分布。（激光雷达多采用旋转式扫描方案，相当于在空间中形成无数个同轴的圆锥面 。当这些圆锥面与平坦的地面相交时，交线就是一个个同心圆，对应到点云上就是呈圆周分布的地面点。）
7.4.4特征法激光雷达里程计的实现
整个里程计的计算流程和之前基于NDT的类似，为角点和平面点分别构建两个局部地图，两者的ICP会放入同一个优化问题中。
cpp
class LoamLikeOdom {
public:
/**
* @struct Options
* @brief LoamLikeOdom的配置参数结构体
* @details 包含特征提取、ICP配准、关键帧、可视化等所有可配置参数，默认值为经验值
*/
struct Options {
/**
* @brief 构造函数，初始化默认参数
*/
Options() {}
int min_edge_pts_ = 20; ///< 有效配准所需的最小边缘点数量（低于此值则跳过边缘点ICP）
int min_surf_pts_ = 20; ///< 有效配准所需的最小平面点数量（低于此值则跳过平面点ICP）
double kf_distance_ = 1.0; ///< 关键帧判定的距离阈值（米）：与上一关键帧位移超过此值则生成新关键帧
double kf_angle_deg_ = 15; ///< 关键帧判定的角度阈值（度）：与上一关键帧旋转超过此值则生成新关键帧
int num_kfs_in_local_map_ = 30; ///< 局部地图中保留的最大关键帧数量（超出则剔除最旧的）
bool display_realtime_cloud_ = true; ///< 是否启用实时点云可视化（依赖PCL可视化器）
// ICP 核心参数
int max_iteration_ = 5; ///< ICP迭代的最大次数（LOAM风格的少量迭代保证效率）
double max_plane_distance_ = 0.05; ///< 平面点最近邻查找的距离阈值（米）：超出则视为无效匹配
double max_line_distance_ = 0.5; ///< 边缘点最近邻查找的距离阈值（米）：超出则视为无效匹配
int min_effective_pts_ = 10; ///< ICP配准的最小有效匹配点数（低于此值则配准失败）
double eps_ = 1e-3; ///< ICP收敛判定阈值：位姿更新量小于此值则停止迭代
bool use_edge_points_ = true; ///< 是否启用边缘点参与ICP配准（关闭则仅用平面点）
bool use_surf_points_ = true; ///< 是否启用平面点参与ICP配准（关闭则仅用边缘点）
};
/**
* @brief 构造函数
* @param options 配置参数（默认使用Options结构体的默认值）
*/
explicit LoamLikeOdom(Options options = Options());
/**
* @brief 处理单帧原始点云，完成特征提取、配准、位姿估计全流程
* @param full_cloud 输入的原始完整点云（未做特征提取的点云）
* @details 内部流程：
* 1. 调用特征提取模块分离边缘点和平面点
* 2. 用提取的特征点与局部地图进行ICP配准
* 3. 判定是否生成新关键帧，更新局部地图/全局地图
* 4. （可选）实时可视化点云和轨迹
*/
void ProcessPointCloud(FullCloudPtr full_cloud);
/**
* @brief 将全局地图保存到指定路径
* @param path 保存路径（支持.pcd等PCL支持的点云格式）
* @details 保存的全局地图包含所有关键帧的边缘点和平面点，可用于后续离线分析或可视化
*/
void SaveMap(const std::string& path);
private:
/**
* @brief 用提取的特征点与局部地图进行ICP配准，估计当前帧位姿
* @param edge 当前帧提取的边缘点云
* @param surf 当前帧提取的平面点云
* @return SE3 当前帧相对于局部地图的位姿（欧式变换）
* @details 核心配准逻辑：
* - 边缘点：匹配局部地图中的边缘线，用点到线ICP
* - 平面点：匹配局部地图中的平面，用点到面ICP
* - 融合两类特征的ICP结果，输出最终位姿
*/
SE3 AlignWithLocalMap(CloudPtr edge, CloudPtr surf);
/**
* @brief 判断当前帧是否需要作为关键帧
* @param current_pose 当前帧的估计位姿
* @return bool true=是关键帧，false=不是关键帧
* @details 判定规则：满足以下任一条件则为关键帧
* 1. 与上一关键帧的平移距离 > kf_distance_
* 2. 与上一关键帧的旋转角度 > kf_angle_deg_
*/
bool IsKeyframe(const SE3& current_pose);
Options options_; ///< 里程计配置参数（全局生效）
int cnt_frame_ = 0; ///< 已处理的总帧数计数器
int last_kf_id_ = 0; ///< 上一个关键帧的帧ID
/// 局部地图的特征点云：分别存储边缘点和平面点（用于ICP配准）
CloudPtr local_map_edge_ = nullptr, local_map_surf_ = nullptr;
std::vector<SE3> estimated_poses_; ///< 所有帧的估计位姿序列（保存轨迹）
SE3 last_kf_pose_; ///< 上一个关键帧的位姿（用于关键帧判定）
std::deque<CloudPtr> edges_, surfs_; ///< 缓存各帧的边缘点/平面点（用于构建局部地图）
CloudPtr global_map_ = nullptr; ///< 全局地图点云（汇总所有关键帧特征点）
std::shared_ptr<FeatureExtraction> feature_extraction_ = nullptr; ///< 特征提取模块指针（负责分离边缘/平面点）
std::shared_ptr<PCLMapViewer> viewer_ = nullptr; ///< PCL可视化器指针（实时显示点云/轨迹）
pcl::KdTree kdtree_edge_, kdtree_surf_; ///< KD树：分别用于边缘点/平面点的最近邻查找（加速ICP匹配）
};实现流程图：
核心ICP配准流程
由于要同时处理点到线和点到面的配准，它们的流程和ICP部分完全一样，只是多了一部分局部地图的维护过程。
在实测效果中，角点和平面点并不一定能完美分类，它们只是一个大概结果。特征类的点云配准方法，更多的是按照经验设计。许多特征提取的过程也很难有理论支撑，而是凭借经验设计。像LeGO-LOAM、mulls等系统有着更复杂的特征提取方式，但配准部分则和经典算法一致。可以将地面、天花板分离出来单独处理，按照线束曲率大小，把点云进一步细分为角点、不那么尖锐的角点、平面点、不那么平的平面点，等等。这些细化工作都可以进一步提升配准的效果。从性能指标看，该里程计在16线雷达数据上的处理时间约为20毫秒/帧，略慢于前文的增量NDT里程计。这一方面是由于引入了特征提取；另一方面，点到面ICP和点到线ICP还需要重新构建K-d树来查找最近邻。
ps：俺不中嘞，太长时间不学，快忘光啦