feat:给特征提取添加中文注释
parent
d696fe4871
commit
42e60fba95
|
|
@ -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 (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<size_t>(ix + i) >= scan.size()) break;
|
||||
if (picked->at(ix + i)) continue;
|
||||
|
|
|
|||
|
|
@ -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,53 +9,168 @@
|
|||
|
||||
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);
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue