From 9bf19d5ff3fb8d12be60819a1df2b129d60b87a4 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Sun, 2 Feb 2025 01:50:17 +0800 Subject: [PATCH] =?UTF-8?q?feat:=E6=B7=BB=E5=8A=A0=E6=BF=80=E5=85=89?= =?UTF-8?q?=E9=87=8C=E7=A8=8B=E8=AE=A1=E7=9A=84=E4=B8=AD=E6=96=87=E6=B3=A8?= =?UTF-8?q?=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- oh_my_loam/odometer/odometer.cc | 87 +++++++++++++++++++++++++- oh_my_loam/odometer/odometer.h | 107 +++++++++++++++++++++++++++++++- 2 files changed, 191 insertions(+), 3 deletions(-) diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index a65e133..3b0d84e 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -8,22 +8,31 @@ namespace oh_my_loam { +// 初始化里程计，读取配置参数并设置可视化选项 bool Odometer::Init() { + // 从全局 YAML 配置中获取配置节点 const auto &config = common::YAMLConfig::Instance()->config(); + // 提取与里程计相关的配置参数 config_ = config["odometer_config"]; + // 读取是否启用可视化 is_vis_ = config_["vis"].as(); + // 读取是否启用详细日志输出 verbose_ = config_["verbose"].as(); AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF"); + // 如果启用可视化，则实例化 OdometerVisualizer 对象 if (is_vis_) visualizer_.reset(new OdometerVisualizer); return true; } +// 重置里程计状态，将初始化标志置为 false void Odometer::Reset() { is_initialized_ = false; } +// 主处理函数：根据输入特征更新位姿，并输出最新位姿 void Odometer::Process(double timestamp, const std::vector &features, common::Pose3d *const pose_out) { + // 如果尚未初始化，则直接将当前特征更新为前一帧参考，并初始化各个位姿为单位变换 if (!is_initialized_) { UpdatePre(features); is_initialized_ = true; @@ -33,103 +42,154 @@ void Odometer::Process(double timestamp, const std::vector &features, AINFO << "Odometer initialized..."; return; } + + // 开始计时，用于后续性能调试 BLOCK_TIMER_START; + + // 定义匹配成功的点对集合：点-平面对（pp_pairs）和点-直线对（pl_pairs） std::vector pp_pairs; std::vector pl_pairs; + + // 根据配置中的 ICP 迭代次数进行多次优化迭代 for (int i = 0; i < config_["icp_iter_num"].as(); ++i) { + // 清空上一次迭代的匹配结果 std::vector().swap(pl_pairs); std::vector().swap(pp_pairs); + + // 遍历每个特征集合（通常对应不同扫描线或不同特征提取区域） for (const auto &feature : features) { + // 利用当前帧的锐角角点匹配直线特征对 MatchCorn(*feature.cloud_sharp_corner, &pl_pairs); + // 利用当前帧的平面点匹配平面特征对 MatchSurf(*feature.cloud_flat_surf, &pp_pairs); } + + // 如果启用了详细日志，则输出本次迭代匹配到的数量 AINFO_IF(verbose_) << "Iter " << i << ": matched num: point2line = " << pl_pairs.size() << ", point2plane = " << pp_pairs.size(); + + // 如果匹配到的对应点对总数不足，发出警告并跳过本次迭代 if (pl_pairs.size() + pp_pairs.size() < config_["min_correspondence_num"].as()) { AWARN << "Too less correspondence: " << pl_pairs.size() << " + " << pp_pairs.size(); continue; } + + // 利用当前估计的位姿作为初始值构造位姿求解器 PoseSolver solver(pose_curr2last_); + // 将匹配到的点-直线对添加到求解器中，权重设置为 1.0 for (const auto &pair : pl_pairs) { solver.AddPointLinePair(pair, 1.0); } + // 将匹配到的点-平面对添加到求解器中，权重设置为 1.0 for (const auto &pair : pp_pairs) { solver.AddPointPlanePair(pair, 1.0); } + + // 调用求解器进行优化求解，指定迭代次数和是否打印详细日志 bool is_converge = solver.Solve(config_["solve_iter_num"].as(), verbose_, &pose_curr2last_); + // 如果求解没有收敛，则输出警告信息 AWARN_IF(!is_converge) << "Odometry solve: no_convergence"; AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": " << BLOCK_TIMER_STOP_FMT; } + + // 更新全局位姿：将当前帧相对于上一帧的位姿（pose_curr2last_）叠加到世界位姿中 pose_curr2world_ = pose_curr2world_ * pose_curr2last_; + // 输出最新位姿 *pose_out = pose_curr2world_; AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString(); - // mush called before calling UpdatePre + + // 在更新前调用可视化函数，展示匹配结果和位姿信息 if (is_vis_) Visualize(pl_pairs, pp_pairs); + // 将当前帧特征更新为下一次匹配的参考 UpdatePre(features); AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT; } +// 利用当前帧的角点进行匹配，构造点-直线对 void Odometer::MatchCorn(const TPointCloud &src, std::vector *const pairs) const { + // 如果角点 kd-tree 的输入点云为空，则返回 if (kdtree_corn_.getInputCloud()->empty()) return; + // 从配置中获取角点匹配距离阈值（平方距离） double dist_sq_thresh = config_["corn_match_dist_sq_th"].as(); + // 遍历当前帧中每个角点 for (const auto &pt : src) { + // 将当前点通过当前帧相对上一帧的变换，转换到上一帧坐标系中 TPoint query_pt = TransformToStart(pose_curr2last_, pt); std::vector indices; std::vector dists; + // 在全局角点 kd-tree 中寻找最近的一个邻居 if (kdtree_corn_.nearestKSearch(query_pt, 1, indices, dists) < 1) { continue; } + // 如果距离超过阈值，则忽略该匹配 if (dists[0] >= dist_sq_thresh) continue; + // 获取匹配到的最近邻角点 TPoint pt1 = kdtree_corn_.getInputCloud()->at(indices[0]); + // 取得该点所属的扫描线编号 int pt1_scan_id = GetScanId(pt1); + // 从配置中获取“附近扫描线”的数量，用于在相邻扫描线中寻找第二个匹配点 int nearby_scan_num = config_["nearby_scan_num"].as(); TPoint pt2; bool pt2_fount = false; float min_pt2_dist_sq = dist_sq_thresh; + // 确定搜索范围：从 pt1_scan_id - nearby_scan_num 到 pt1_scan_id + nearby_scan_num int i_begin = std::max(0, pt1_scan_id - nearby_scan_num); int i_end = std::min(kdtrees_scan_corn_.size(), pt1_scan_id + nearby_scan_num + 1); + // 在相邻扫描线中寻找第二个匹配点 for (int i = i_begin; i < i_end; ++i) { - if (i == pt1_scan_id) continue; + if (i == pt1_scan_id) continue; // 跳过与 pt1 同一扫描线 const auto &kdtree = kdtrees_scan_corn_[i]; if (kdtree.getInputCloud()->empty()) continue; if (kdtree.nearestKSearch(query_pt, 1, indices, dists) < 1) { continue; } + // 如果找到的距离更短，则更新 pt2 if (dists[0] < min_pt2_dist_sq) { pt2_fount = true; pt2 = kdtree.getInputCloud()->at(indices[0]); min_pt2_dist_sq = dists[0]; } } + // 如果没有在相邻扫描线中找到第二个匹配点，则跳过该点 if (!pt2_fount) continue; + // 构造点-直线对，将原始点 pt 与匹配到的 pt1、pt2 封装后加入匹配集合 pairs->emplace_back(pt, pt1, pt2); } } +// 利用当前帧的平面点进行匹配，构造点-平面对 void Odometer::MatchSurf(const TPointCloud &src, std::vector *const pairs) const { + // 如果平面点 kd-tree 的输入点云为空，则直接返回 if (kdtree_surf_.getInputCloud()->empty()) return; + // 从配置中获取平面点匹配距离阈值（平方距离） double dist_sq_thresh = config_["surf_match_dist_sq_th"].as(); + // 遍历当前帧中每个平面点 for (const auto &pt : src) { + // 将当前点转换到上一帧坐标系中 TPoint query_pt = TransformToStart(pose_curr2last_, pt); std::vector indices; std::vector dists; + // 在全局平面点 kd-tree 中寻找最近的邻居 if (kdtree_surf_.nearestKSearch(query_pt, 1, indices, dists) < 1) { continue; } + // 如果距离超过设定阈值，则忽略该点 if (dists[0] >= dist_sq_thresh) continue; + // 获取匹配到的最近邻平面点 TPoint pt1 = kdtree_surf_.getInputCloud()->at(indices[0]); int pt1_scan_id = GetScanId(pt1); + // 在相邻扫描线中寻找第二个匹配点 int nearby_scan_num = config_["nearby_scan_num"].as(); TPoint pt2; bool pt2_fount = false; @@ -150,39 +210,54 @@ void Odometer::MatchSurf(const TPointCloud &src, min_pt2_dist_sq = dists[0]; } } + // 如果没有在相邻扫描线中找到第二个匹配点，则跳过该点 if (!pt2_fount) continue; + // 在与 pt1 同一扫描线中进行二次搜索，寻找第二个邻居（pt3）以构造平面 const auto &kdtree = kdtrees_scan_surf_[pt1_scan_id]; if (kdtree.getInputCloud()->empty()) continue; if (kdtree.nearestKSearch(query_pt, 2, indices, dists) < 2) continue; + // 如果第二近邻距离超过阈值，则放弃该匹配 if (dists[1] >= dist_sq_thresh) continue; + // 取第二个最近邻作为 pt3 TPoint pt3 = kdtree.getInputCloud()->at(indices[1]); + // 构造点-平面对，将原始点 pt 与匹配到的 pt1、pt2、pt3 封装后加入匹配集合 pairs->emplace_back(pt, pt1, pt2, pt3); } } +// 更新上一帧的特征，用于下一帧匹配 void Odometer::UpdatePre(const std::vector &features) { + // 根据特征数目调整每个扫描线对应的 kd-tree 数组大小 kdtrees_scan_corn_.resize(features.size()); kdtrees_scan_surf_.resize(features.size()); + // 创建两个点云指针用于累积所有扫描线的平面点和角点 TPointCloudPtr surf_pre(new TPointCloud); TPointCloudPtr corn_pre(new TPointCloud); + // 遍历每个扫描线的特征 for (size_t i = 0; i < features.size(); ++i) { const auto &feature = features[i]; + // 为当前扫描线分配角点和平面点点云 TPointCloudPtr scan_corn_pre(new TPointCloud); TPointCloudPtr scan_surf_pre(new TPointCloud); + // 调整点云大小以匹配当前扫描线中的点数 scan_corn_pre->resize(feature.cloud_corner->size()); scan_surf_pre->resize(feature.cloud_surf->size()); + // 对每个角点进行变换，将其转换到本次扫描结束时的坐标系中 for (size_t j = 0; j < feature.cloud_corner->size(); ++j) { TransformToEnd(pose_curr2last_, feature.cloud_corner->at(j), &scan_corn_pre->at(j)); } + // 对每个平面点进行类似的变换 for (size_t j = 0; j < feature.cloud_surf->size(); ++j) { TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j), &scan_surf_pre->at(j)); } + // 累积所有扫描线的角点和平面点 *corn_pre += *scan_corn_pre; *surf_pre += *scan_surf_pre; + // 分别为当前扫描线的角点和平面点构建 kd-tree（便于后续匹配） if (!scan_corn_pre->empty()) { kdtrees_scan_corn_[i].setInputCloud(scan_corn_pre); } @@ -190,21 +265,29 @@ void Odometer::UpdatePre(const std::vector &features) { kdtrees_scan_surf_[i].setInputCloud(scan_surf_pre); } } + // 为全局角点和平面点分别构建 kd-tree if (!corn_pre->empty()) kdtree_corn_.setInputCloud(corn_pre); if (!surf_pre->empty()) kdtree_surf_.setInputCloud(surf_pre); } +// 利用可视化工具展示当前匹配结果和位姿信息 void Odometer::Visualize(const std::vector &pl_pairs, const std::vector &pp_pairs, double timestamp) const { + // 构造一个可视化帧，用于存放当前帧的各项数据 std::shared_ptr frame(new OdometerVisFrame); frame->timestamp = timestamp; + // 获取全局角点和平面点云的共享指针 frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared(); frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared(); + // 将匹配到的点-直线对和平面点对传入可视化帧 frame->pl_pairs = pl_pairs; frame->pp_pairs = pp_pairs; + // 传入当前帧相对于上一帧和全局的位姿信息 frame->pose_curr2last = pose_curr2last_; frame->pose_curr2world = pose_curr2world_; + // 调用可视化器进行渲染展示 visualizer_->Render(frame); } + } // namespace oh_my_loam diff --git a/oh_my_loam/odometer/odometer.h b/oh_my_loam/odometer/odometer.h index b6d30c5..0dbd58b 100644 --- a/oh_my_loam/odometer/odometer.h +++ b/oh_my_loam/odometer/odometer.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,61 +9,165 @@ namespace oh_my_loam { -// frame to frame lidar Odometer +/** + * @brief 基于帧间匹配的激光雷达里程计 + * + * 该类实现了激光雷达帧间位姿估计，主要功能包括： + * - 初始化参数和可视化组件（Init） + * - 根据当前帧提取的特征计算相对于上一帧以及全局的位姿更新（Process） + * - 角点与平面点的匹配、最近邻搜索及结果可视化等辅助功能 + */ class Odometer { public: + // 默认构造函数 Odometer() = default; + /** + * @brief 初始化里程计 + * + * 读取配置文件中的参数，初始化各项内部变量，并根据配置决定是否启用可视化。 + * + * @return true 初始化成功 + */ bool Init(); + /** + * @brief 处理当前帧特征，更新里程计位姿 + * + * 根据输入的特征集合（如角点与平面点），通过匹配和求解算法估计当前帧相对于上一帧的位姿， + * 并更新全局位姿。 + * + * @param timestamp 当前帧时间戳 + * @param features 当前帧的特征集合 + * @param pose_out 输出的全局位姿 + */ void Process(double timestamp, const std::vector &features, common::Pose3d *const pose_out); + /** + * @brief 获取当前角点点云 + * + * 返回存储在全局角点 kd-tree 中的角点点云数据。 + * + * @return TPointCloudConstPtr 角点点云的常量指针 + */ TPointCloudConstPtr GetCloudCorn() const { return kdtree_corn_.getInputCloud(); } + /** + * @brief 获取当前平面点点云 + * + * 返回存储在全局平面点 kd-tree 中的平面点点云数据。 + * + * @return TPointCloudConstPtr 平面点点云的常量指针 + */ TPointCloudConstPtr GetCloudSurf() const { return kdtree_surf_.getInputCloud(); } + /** + * @brief 重置里程计状态 + * + * 重置内部状态变量，用于系统重启或丢失跟踪后的恢复。 + */ void Reset(); protected: + /** + * @brief 更新上一帧特征数据 + * + * 将当前帧的特征数据转换后存储，作为下一帧匹配的参考数据。 + * + * @param features 当前帧的特征集合 + */ void UpdatePre(const std::vector &features); + /** + * @brief 匹配角点，构造点-直线匹配对 + * + * 在输入的角点点云中搜索匹配点，并构造点-直线匹配对， + * 该匹配对将用于后续位姿求解。 + * + * @param src 输入的角点点云 + * @param pairs 输出的匹配结果（点-直线匹配对集合） + */ void MatchCorn(const TPointCloud &src, std::vector *const pairs) const; + /** + * @brief 匹配平面点，构造点-平面对 + * + * 在输入的平面点点云中搜索匹配点，并构造点-平面对， + * 该匹配对将用于后续位姿求解。 + * + * @param src 输入的平面点点云 + * @param pairs 输出的匹配结果（点-平面对集合） + */ void MatchSurf(const TPointCloud &src, std::vector *const pairs) const; + /** + * @brief 可视化匹配结果与位姿信息 + * + * 利用可视化工具将当前匹配的点对以及位姿信息进行展示， + * 便于调试和监控系统运行状态。 + * + * @param pl_pairs 点-直线匹配对集合 + * @param pp_pairs 点-平面对集合 + * @param timestamp 当前时间戳（默认为 0.0） + */ void Visualize(const std::vector &pl_pairs, const std::vector &pp_pairs, double timestamp = 0.0) const; + /** + * @brief 辅助的最近邻搜索函数 + * + * 在给定的 kd-tree 中查找查询点的 k 个最近邻，如果距离平方满足阈值条件， + * 则返回对应的索引集合。 + * + * @param kdtree 用于搜索的 kd-tree 对象 + * @param query_pt 查询点 + * @param k 搜索的最近邻个数 + * @param dist_sq_th 距离平方阈值 + * @param indices 输出的最近邻索引集合 + * @return true 搜索成功，false 否则 + */ bool NearestSearch(const pcl::KdTreeFLANN &kdtree, const TPoint &query_pt, int k, float dist_sq_th, std::vector *const indices) const; + // 当前帧到世界坐标系的全局位姿 common::Pose3d pose_curr2world_; + // 当前帧相对于上一帧的位姿变换 common::Pose3d pose_curr2last_; + // 存储每个扫描线的平面点 kd-tree，用于加速平面点匹配 std::vector> kdtrees_scan_surf_; + // 存储每个扫描线的角点 kd-tree，用于加速角点匹配 std::vector> kdtrees_scan_corn_; + // 全局角点 kd-tree（用于全局角点匹配） pcl::KdTreeFLANN kdtree_corn_; + // 全局平面点 kd-tree（用于全局平面点匹配） pcl::KdTreeFLANN kdtree_surf_; + // 存储里程计相关的配置参数（例如匹配距离阈值、迭代次数等） YAML::Node config_; + // 里程计是否已初始化 bool is_initialized_ = false; + // 是否启用可视化 bool is_vis_ = false; + // 是否输出详细日志信息 bool verbose_ = false; + // 里程计可视化工具（用于展示匹配和位姿估计结果） std::unique_ptr visualizer_{nullptr}; + // 禁止拷贝构造函数和赋值操作，防止对象被误拷贝 DISALLOW_COPY_AND_ASSIGN(Odometer) };