diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index b0166b2..43f72ba 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/oh_my_loam/extractor/extractor.cc @@ -6,87 +6,162 @@ namespace oh_my_loam { +// 定义一些局部常量 namespace { +// 2π，用于角度补偿 const double kTwoPi = 2 * M_PI; +// 极小值阈值，防止数值误差 const double kEps = 1e-6; } // namespace +/** + * @brief 初始化特征提取器 + * + * 从全局 YAML 配置文件中读取参数，包括是否启用可视化、是否打印详细信息等， + * 并根据配置决定是否构建可视化器实例。 + * + * @return true 初始化成功 + */ bool Extractor::Init() { + // 从单例配置管理器中获取 YAML 配置 const auto &config = common::YAMLConfig::Instance()->config(); + // 提取出与特征提取相关的配置部分 config_ = config["extractor_config"]; + // 从配置中获取是否启用可视化 is_vis_ = config_["vis"].as(); + // 是否输出详细日志信息 verbose_ = config_["verbose"].as(); AINFO << "Extraction visualizer: " << (is_vis_ ? "ON" : "OFF"); + // 如果启用了可视化，则实例化视觉化器 if (is_vis_) visualizer_.reset(new ExtractorVisualizer); return true; } +/** + * @brief 对输入点云进行特征提取 + * + * 该函数对传入的点云进行处理，步骤包括： + * 1. 检查点数是否足够。 + * 2. 将点云分割成多个扫描线。 + * 3. 对每条扫描线计算曲率。 + * 4. 根据曲率对每个点分配类别（如角点、平面点等）。 + * 5. 将不同类别的点整理到特征集合中。 + * 6. 如果启用可视化，则进行渲染显示。 + * + * @param timestamp 当前帧时间戳 + * @param cloud 输入的原始点云（常量指针） + * @param features 输出的特征集合（包含角点、平面点等） + */ void Extractor::Process(double timestamp, const common::PointCloudConstPtr &cloud, std::vector *const features) { + // 开始计时（宏定义，便于性能调试） BLOCK_TIMER_START; + + // 若输入点云的点数小于配置要求，则发出警告并返回 if (cloud->size() < config_["min_point_num"].as()) { AWARN << "Too few input points: num = " << cloud->size() << " (< " << config_["min_point_num"].as() << ")"; return; } - // split point cloud int scans + + // 1. 划分扫描线：将整体点云按照激光束数或扫描行数划分为多个扫描线 std::vector scans; SplitScan(*cloud, &scans); AINFO_IF(verbose_) << "Extractor::SplitScan: " << BLOCK_TIMER_STOP_FMT; - // compute curvature to each point + + // 2. 对每个扫描线中的点计算曲率（局部平滑性指标） for (auto &scan : scans) { ComputeCurvature(&scan); } AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << BLOCK_TIMER_STOP_FMT; - // assign type to each point: FLAT, LESS_FLAT, NORMAL, LESS_SHARP or SHARP + + // 3. 根据计算的曲率值，为每个扫描线内的点分配类别（角点、平面点等） for (auto &scan : scans) { AssignType(&scan); } AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT; - // store points into feature point clouds based on their type + + // 4. 根据不同的点类型，将点整理到特征集合中 for (size_t i = 0; i < scans.size(); ++i) { Feature feature; GenerateFeature(scans[i], &feature); features->push_back(std::move(feature)); } AINFO << "Extractor::Process: " << BLOCK_TIMER_STOP_FMT; + + // 5. 如果启用了可视化，调用可视化函数展示当前帧点云和提取的特征 if (is_vis_) Visualize(cloud, *features, timestamp); } +/** + * @brief 划分扫描线 + * + * 将输入的原始点云按照激光扫描行（或激光束）进行划分。每个点根据其角度 + * 被分配到不同的扫描线中，方便后续的局部处理（如计算曲率、提取特征等）。 + * + * @param cloud 输入的原始点云 + * @param scans 输出的扫描线集合，每个元素对应一条扫描线 + */ void Extractor::SplitScan(const common::PointCloud &cloud, std::vector *const scans) const { + // 根据激光束数量（num_scans_）调整输出容器大小 scans->resize(num_scans_); + + // 在点云前 num_scans_ 个点中，找到一个极角最小（或最大，视具体传感器安装角度而定）的点， + // 用于确定扫描起始的角度 auto it = std::min_element(cloud.begin(), cloud.begin() + num_scans(), [](const auto &p1, const auto &p2) { return -std::atan2(p1.y, p1.x) < -std::atan2(p2.y, p2.x); }); + // 记录扫描起始角度（负号可能是为了调整坐标系的方向） double yaw_start = -std::atan2(it->y, it->x); + // 标记是否已经跨过半圈（用于处理角度环绕问题） bool half_passed = false; + + // 遍历整个点云，为每个点计算其所属扫描线ID for (const auto &pt : cloud) { int scan_id = GetScanID(pt); - if (scan_id >= num_scans_ || scan_id < 0) continue; + if (scan_id >= num_scans_ || scan_id < 0) continue; // 跳过无效扫描线 + // 计算该点的横向角度（这里也取负号，和 yaw_start 保持一致） double yaw = -std::atan2(pt.y, pt.x); + // 计算该点与扫描起始点之间的角度差，并归一化到 [-π, π) 或 [0, 2π) double yaw_diff = common::NormalizeAngle(yaw - yaw_start); if (yaw_diff >= -kEps) { + // 如果已经跨过半圈，则需要加上 2π 补偿 if (half_passed) yaw_diff += kTwoPi; } else { + // 如果角度差为负，说明刚好跨过 0 度，此时设置 half_passed 标记 half_passed = true; yaw_diff += kTwoPi; } + // 原来的时间戳计算方式被注释掉了，改为直接用固定值加上扫描线编号（可能用于区分扫描线） // double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id; double time = 0.999999 + scan_id; + // 将点封装为 TCTPoint，并将曲率初始化为 NaN（后续会计算） scans->at(scan_id).push_back( {pt.x, pt.y, pt.z, static_cast(time), std::nanf("")}); } } +/** + * @brief 计算曲率 + * + * 对于输入的单个扫描线，使用前后各 5 个点（共 10 个邻域点）与当前点进行比较， + * 计算曲率（局部差异度量），作为后续判断点是角点还是平面点的重要依据。 + * + * @param scan 输入的单个扫描线，计算后每个点的 curvature 成员会被更新 + */ void Extractor::ComputeCurvature(TCTPointCloud *const scan) const { + // 从配置中获取扫描线段的分段数（用于后续点分配可能有影响） size_t seg_num = config_["scan_seg_num"].as(); + // 如果点数不足（边界点不能计算曲率），则直接返回 if (scan->size() <= 10 + seg_num) return; auto &pts = scan->points; + // 对每个中间点（避开前 5 个和后 5 个边界点）计算曲率 for (size_t i = 5; i < pts.size() - 5; ++i) { + // 分别计算 x, y, z 方向上的局部差值 float dx = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x + pts[i - 1].x + pts[i + 1].x + pts[i + 2].x + pts[i + 3].x + pts[i + 4].x + pts[i + 5].x - 10 * pts[i].x; @@ -96,20 +171,35 @@ void Extractor::ComputeCurvature(TCTPointCloud *const scan) const { float dz = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z + pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z + pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z; + // 使用欧氏范数计算曲率（局部变化量大小） pts[i].curvature = std::hypot(dx, dy, dz); } + // 移除曲率值非有限（inf 或 NaN）的点，保证后续处理数据合法 common::RemovePoints(*scan, scan, [](const TCTPoint &pt) { return !std::isfinite(pt.curvature); }); } +/** + * @brief 根据曲率为扫描线内的点分配类型 + * + * 将扫描线内的点根据曲率大小划分为角点（包括尖锐角点和一般角点）和 + * 平面点（FLAT_SURF 类型），并对相邻点进行排除（防止密集点重复选择）。 + * + * @param scan 输入的单个扫描线，处理后每个点的 type 成员会被更新 + */ void Extractor::AssignType(TCTPointCloud *const scan) const { int pt_num = scan->size(); + // 从配置中获取扫描分段数量 int seg_num = config_["scan_seg_num"].as(); if (pt_num < seg_num) return; + // 计算每个分段的点数，确保每段覆盖整个扫描线 int seg_pt_num = (pt_num - 1) / seg_num + 1; + // 用于记录某个点是否已经被选为特征，避免同一区域内多个点重复被选 std::vector picked(pt_num, false); + // 构造一个从 0 到 pt_num-1 的索引数组，便于排序和遍历 std::vector indices = common::Range(pt_num); + // 从配置中读取各类特征点的数量限制和曲率阈值 int sharp_corner_point_num = config_["sharp_corner_point_num"].as(); int corner_point_num = config_["corner_point_num"].as(); int flat_surf_point_num = config_["flat_surf_point_num"].as(); @@ -117,36 +207,47 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { config_["corner_point_curvature_th"].as(); float surf_point_curvature_th = config_["surf_point_curvature_th"].as(); + + // 对扫描线按段进行处理，分段的目的是防止不同区域特征密度不均 for (int seg = 0; seg < seg_num; ++seg) { + // 定义当前分段的起始和结束索引 int b = seg * seg_pt_num; int e = std::min((seg + 1) * seg_pt_num, pt_num); if (b >= e) break; - // sort by curvature for each segment: large -> small + // 在当前分段内，根据曲率从大到小排序（高曲率的点更可能是角点） std::sort(indices.begin() + b, indices.begin() + e, [&](int i, int j) { return scan->at(i).curvature > scan->at(j).curvature; }); - // pick corner points + // 从排序后的数组中选取角点（包括尖锐角点和普通角点） int corner_point_picked_num = 0; for (int i = b; i < e; ++i) { int ix = indices[i]; + // 如果该点还未被选取且曲率超过角点阈值，则选取之 if (!picked.at(ix) && scan->at(ix).curvature > corner_point_curvature_th) { ++corner_point_picked_num; if (corner_point_picked_num <= sharp_corner_point_num) { + // 前几个高曲率点作为尖锐角点 scan->at(ix).type = PointType::SHARP_CORNER; } else if (corner_point_picked_num <= corner_point_num) { + // 后续点作为一般角点 scan->at(ix).type = PointType::CORNER; } else { + // 如果已达到角点数目上限，则退出 break; } + // 标记该点已被选取 picked.at(ix) = true; + // 更新该点邻域内的点，避免相邻点重复被选为特征 UpdateNeighborsPicked(*scan, ix, &picked); } } - // pick surface points + // 选取平面特征点（平面点一般曲率较小） int surf_point_picked_num = 0; + // 反向遍历，即从曲率较小的点开始选取 for (int i = e - 1; i >= b; --i) { int ix = indices[i]; + // 如果该点未被选且曲率低于平面点阈值，则选取之 if (!picked.at(ix) && scan->at(ix).curvature < surf_point_curvature_th) { ++surf_point_picked_num; if (surf_point_picked_num <= flat_surf_point_num) { @@ -154,65 +255,113 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { } else { break; } + // 标记该点已被选取 picked.at(ix) = true; + // 同样更新邻域内的点状态 UpdateNeighborsPicked(*scan, ix, &picked); } } } } +/** + * @brief 根据扫描线生成特征点集合 + * + * 遍历一条扫描线内的所有点，根据点的类型将其归类到不同的特征集合中， + * 包括角点和表面点。此外，对平面点进行体素降采样，以减小数据量。 + * + * @param scan 输入的单条扫描线（已经分配好类型） + * @param feature 输出的特征数据结构，包含角点、平面点等多个点云 + */ void Extractor::GenerateFeature(const TCTPointCloud &scan, Feature *const feature) const { + // 遍历扫描线内的每个点，根据点的类型进行分发 for (const auto &pt : scan) { + // 将 TCTPoint 转换为 TPoint（一般只保留位置信息和时间戳） TPoint point(pt.x, pt.y, pt.z, pt.time); switch (pt.type) { case PointType::FLAT_SURF: + // 对于平面点，先存入专用的平面点云集合 feature->cloud_flat_surf->push_back(point); - // no break: FLAT_SURF points are also SURF points + // 注意：这里没有 break，意味着 FLAT_SURF 点同时也算作 SURF 点 case PointType::SURF: + // 将平面点（以及标记为 SURF 的点）加入 SURF 点集合 feature->cloud_surf->push_back(point); break; case PointType::SHARP_CORNER: + // 对于尖锐角点，先存入尖锐角点集合 feature->cloud_sharp_corner->push_back(point); - // no break: SHARP_CORNER points are also CORNER points + // 同样，继续下落至角点集合 case PointType::CORNER: + // 将角点加入角点集合 feature->cloud_corner->push_back(point); break; default: + // 对于未标记的点，默认归入 SURF 点集合 feature->cloud_surf->push_back(point); break; } } + // 对平面点云进行体素滤波降采样，降低点密度 TPointCloudPtr dowm_sampled(new TPointCloud); common::VoxelDownSample( feature->cloud_surf, dowm_sampled.get(), config_["downsample_voxel_size"].as()); + // 更新平面点云为降采样后的结果 feature->cloud_surf = dowm_sampled; } +/** + * @brief 可视化函数 + * + * 将当前帧的原始点云和提取到的特征点组合成一个帧数据，然后调用视觉化器进行渲染， + * 便于调试和结果展示。 + * + * @param cloud 当前帧的原始点云 + * @param features 当前帧提取到的特征点集合 + * @param timestamp 当前帧的时间戳 + */ void Extractor::Visualize(const common::PointCloudConstPtr &cloud, const std::vector &features, double timestamp) const { + // 构造视觉化帧数据 std::shared_ptr frame(new ExtractorVisFrame); frame->timestamp = timestamp; frame->cloud = cloud; frame->features = features; + // 调用视觉化器进行渲染 visualizer_->Render(frame); } +/** + * @brief 更新邻域点状态 + * + * 当一个点被选为特征点后，为了防止其邻域内的点过于集中地被选中， + * 本函数将选取该点周围一定范围内（前后各 5 个点）且距离较近的点标记为已选。 + * + * @param scan 当前扫描线 + * @param ix 当前选中的点索引 + * @param picked 全局标记数组，记录各点是否已被选为特征 + */ void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, int ix, std::vector *const picked) const { + // 定义 lambda 函数，用于计算两个点之间的欧氏距离平方 auto dist_sq = [&](size_t i, size_t j) -> double { return common::DistanceSquare(scan[i], scan[j]); }; + // 从配置中获取邻域点距离平方的阈值 double neighbor_point_dist_sq_th = config_["neighbor_point_dist_sq_th"].as(); + // 向前遍历最多 5 个邻近点 for (int i = 1; i <= 5; ++i) { - if (ix - i < 0) break; - if (picked->at(ix - i)) continue; + if (ix - i < 0) break; // 超出数组边界 + if (picked->at(ix - i)) continue; // 如果该点已经被选，则跳过 + // 如果相邻两个点的距离超过阈值，则认为该处变化较大，不再继续标记 if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_sq_th) break; + // 标记该邻域点为已选，避免重复选取 picked->at(ix - i) = true; } + // 向后遍历最多 5 个邻近点 for (int i = 1; i <= 5; ++i) { if (static_cast(ix + i) >= scan.size()) break; if (picked->at(ix + i)) continue; diff --git a/oh_my_loam/extractor/extractor.h b/oh_my_loam/extractor/extractor.h index a187732..f5caadb 100644 --- a/oh_my_loam/extractor/extractor.h +++ b/oh_my_loam/extractor/extractor.h @@ -1,5 +1,6 @@ #pragma once +// 包含必要的公共头文件和依赖模块 #include "common/common.h" #include "oh_my_loam/base/feature.h" #include "oh_my_loam/base/utils.h" @@ -8,54 +9,169 @@ namespace oh_my_loam { +/** + * @brief 特征提取器基类 + * + * 本类定义了点云特征提取的公共接口和基本实现，包括： + * - 初始化（Init） + * - 处理输入点云数据并提取特征（Process） + * - 分割扫描线、计算曲率、分配点类型以及生成特征点等步骤 + * + * 注意：具体的扫描线编号获取方法（GetScanID）由派生类根据激光雷达的具体结构实现。 + */ class Extractor { public: + // 默认构造函数 Extractor() = default; + // 默认虚析构函数，确保派生类析构时正确释放资源 virtual ~Extractor() = default; + /** + * @brief 初始化特征提取器 + * + * 读取配置文件中与特征提取相关的参数，并进行必要的初始化设置， + * 如是否开启可视化等。 + * + * @return true 初始化成功 + */ bool Init(); + /** + * @brief 处理输入点云数据，提取特征 + * + * 根据输入的点云数据，依次执行以下步骤： + * - 将点云分割为多个扫描线 + * - 计算每个扫描线中各点的曲率 + * - 根据曲率为点分配类型（例如角点或平面点） + * - 根据不同的点类型生成特征点集合 + * - （可选）进行可视化展示 + * + * @param timestamp 当前帧的时间戳 + * @param cloud 输入的点云数据（常量指针） + * @param features 输出的特征集合，包含角点、平面点等 + */ void Process(double timestamp, const common::PointCloudConstPtr &cloud, std::vector *const features); + /** + * @brief 获取激光扫描线的数量 + * + * 返回激光雷达的扫描线数，该值通常由配置文件指定。 + * + * @return int 扫描线数量 + */ int num_scans() const { return num_scans_; } + /** + * @brief 重置提取器状态 + * + * 虚函数，允许派生类根据需要重置内部状态（例如清空缓存）。 + */ virtual void Reset() {} protected: + /** + * @brief 获取点所属的扫描线ID + * + * 根据输入点的坐标信息，计算该点属于哪一条激光扫描线。 + * 此函数为纯虚函数，必须由具体的雷达类型派生类实现。 + * + * @param pt 输入点 + * @return int 扫描线ID + */ virtual int GetScanID(const common::Point &pt) const = 0; + /** + * @brief 分割点云为多个扫描线 + * + * 根据输入的点云数据和激光雷达扫描线数量，将点云数据按照水平角度等信息 + * 分割到各个扫描线中，为后续计算曲率和提取特征做准备。 + * + * @param cloud 输入的点云数据 + * @param scans 输出的扫描线集合，每个扫描线存储在一个 TCTPointCloud 对象中 + */ virtual void SplitScan(const common::PointCloud &cloud, std::vector *const scans) const; + /** + * @brief 计算扫描线中每个点的曲率 + * + * 针对每条扫描线，利用点的邻域信息计算局部曲率，曲率反映了点的局部 + * 平面性或角点特性，是后续分配点类型的重要依据。 + * + * @param scan 输入的扫描线数据，计算后会更新每个点的曲率值 + */ virtual void ComputeCurvature(TCTPointCloud *const scan) const; + /** + * @brief 根据曲率为扫描线中的点分配类型 + * + * 根据计算的曲率以及预设阈值，将扫描线中的点分配为不同类型， + * 例如角点（包括尖锐角点与一般角点）和表面点（平面点）。 + * + * @param scan 输入的扫描线数据，处理后每个点的类型会被更新 + */ virtual void AssignType(TCTPointCloud *const scan) const; + /** + * @brief 根据扫描线生成特征点集合 + * + * 遍历已分配好类型的扫描线，将点根据其类型分别存入不同的特征集合中， + * 如角点和平面点，并可对平面点进行降采样处理。 + * + * @param scan 输入的扫描线数据（已分配类型） + * @param feature 输出的特征数据结构，包含各类特征点 + */ virtual void GenerateFeature(const TCTPointCloud &scan, Feature *const feature) const; + /** + * @brief 可视化提取的特征 + * + * 将当前帧的原始点云和提取的特征数据封装成一个视觉化帧， + * 通过视觉化工具进行渲染，便于调试和结果展示。 + * + * @param cloud 当前帧的原始点云数据 + * @param features 当前帧提取的特征集合 + * @param timestamp 当前帧的时间戳（默认为 0.0） + */ virtual void Visualize(const common::PointCloudConstPtr &cloud, const std::vector &features, double timestamp = 0.0) const; + // 激光雷达扫描线的数量，通常由配置文件中设置 int num_scans_ = 0; + // 存储特征提取相关的配置参数（YAML 格式） YAML::Node config_; + // 指向特征提取结果可视化工具的智能指针 std::unique_ptr visualizer_{nullptr}; + // 是否打印详细日志信息的标志 bool verbose_ = false; private: + /** + * @brief 更新邻域点的标记状态 + * + * 当某个点被选为特征点后，为避免其邻域内的点过于密集地被选取， + * 将该点附近一定范围内的点标记为已选。 + * + * @param scan 当前扫描线数据 + * @param ix 当前选中点在扫描线中的索引 + * @param picked 标记数组，记录每个点是否已经被选取 + */ void UpdateNeighborsPicked(const TCTPointCloud &scan, int ix, std::vector *const picked) const; + // 标志是否开启可视化，true 表示开启 bool is_vis_ = false; + // 禁止拷贝和赋值操作的宏定义，确保对象不可拷贝 DISALLOW_COPY_AND_ASSIGN(Extractor); }; -} // namespace oh_my_loam \ No newline at end of file +} // namespace oh_my_loam