feat:给特征提取添加中文注释

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<bool>();
+  // 是否输出详细日志信息
   verbose_ = config_["verbose"].as<bool>();
   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<Feature> *const features) {
+  // 开始计时（宏定义，便于性能调试）
   BLOCK_TIMER_START;
+
+  // 若输入点云的点数小于配置要求，则发出警告并返回
   if (cloud->size() < config_["min_point_num"].as<size_t>()) {
     AWARN << "Too few input points: num = " << cloud->size() << " (< "
           << config_["min_point_num"].as<int>() << ")";
     return;
   }
-  // split point cloud int scans
+
+  // 1. 划分扫描线：将整体点云按照激光束数或扫描行数划分为多个扫描线
   std::vector<TCTPointCloud> 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<TCTPointCloud> *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<float>(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<int>();
+  // 如果点数不足（边界点不能计算曲率），则直接返回
   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<TCTPoint>(*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<int>();
   if (pt_num < seg_num) return;
+  // 计算每个分段的点数，确保每段覆盖整个扫描线
   int seg_pt_num = (pt_num - 1) / seg_num + 1;
+  // 用于记录某个点是否已经被选为特征，避免同一区域内多个点重复被选
   std::vector<bool> picked(pt_num, false);
+  // 构造一个从 0 到 pt_num-1 的索引数组，便于排序和遍历
   std::vector<int> indices = common::Range(pt_num);
+  // 从配置中读取各类特征点的数量限制和曲率阈值
   int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>();
   int corner_point_num = config_["corner_point_num"].as<int>();
   int flat_surf_point_num = config_["flat_surf_point_num"].as<int>();
@@ -117,36 +207,47 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
       config_["corner_point_curvature_th"].as<float>();
   float surf_point_curvature_th =
       config_["surf_point_curvature_th"].as<float>();
+
+  // 对扫描线按段进行处理，分段的目的是防止不同区域特征密度不均
   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<TPoint>(
       feature->cloud_surf, dowm_sampled.get(),
       config_["downsample_voxel_size"].as<double>());
+  // 更新平面点云为降采样后的结果
   feature->cloud_surf = dowm_sampled;
 }
 
+/**
+ * @brief 可视化函数
+ *
+ * 将当前帧的原始点云和提取到的特征点组合成一个帧数据，然后调用视觉化器进行渲染，
+ * 便于调试和结果展示。
+ *
+ * @param cloud 当前帧的原始点云
+ * @param features 当前帧提取到的特征点集合
+ * @param timestamp 当前帧的时间戳
+ */
 void Extractor::Visualize(const common::PointCloudConstPtr &cloud,
                           const std::vector<Feature> &features,
                           double timestamp) const {
+  // 构造视觉化帧数据
   std::shared_ptr<ExtractorVisFrame> 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<bool> *const picked) const {
+  // 定义 lambda 函数，用于计算两个点之间的欧氏距离平方
   auto dist_sq = [&](size_t i, size_t j) -> double {
     return common::DistanceSquare<TCTPoint>(scan[i], scan[j]);
   };
+  // 从配置中获取邻域点距离平方的阈值
   double neighbor_point_dist_sq_th =
       config_["neighbor_point_dist_sq_th"].as<float>();
+  // 向前遍历最多 5 个邻近点
   for (int i = 1; i <= 5; ++i) {
-    if (ix - i < 0) break;
+    if (ix - i < 0) break;  // 超出数组边界
-    if (picked->at(ix - i)) continue;
+    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<size_t>(ix + i) >= scan.size()) break;
     if (picked->at(ix + i)) continue;

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<Feature> *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<TCTPointCloud> *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<Feature> &features,
                          double timestamp = 0.0) const;
 
+  // 激光雷达扫描线的数量，通常由配置文件中设置
   int num_scans_ = 0;
 
+  // 存储特征提取相关的配置参数（YAML 格式）
   YAML::Node config_;
 
+  // 指向特征提取结果可视化工具的智能指针
   std::unique_ptr<ExtractorVisualizer> visualizer_{nullptr};
 
+  // 是否打印详细日志信息的标志
   bool verbose_ = false;
 
  private:
+  /**
+   * @brief 更新邻域点的标记状态
+   *
+   * 当某个点被选为特征点后，为避免其邻域内的点过于密集地被选取，
+   * 将该点附近一定范围内的点标记为已选。
+   *
+   * @param scan 当前扫描线数据
+   * @param ix 当前选中点在扫描线中的索引
+   * @param picked 标记数组，记录每个点是否已经被选取
+   */
   void UpdateNeighborsPicked(const TCTPointCloud &scan, int ix,
                              std::vector<bool> *const picked) const;
 
+  // 标志是否开启可视化，true 表示开启
   bool is_vis_ = false;
 
+  // 禁止拷贝和赋值操作的宏定义，确保对象不可拷贝
   DISALLOW_COPY_AND_ASSIGN(Extractor);
 };
 
-}  // namespace oh_my_loam
+}  // namespace oh_my_loam