feat:添加激光里程计的中文注释
parent
42e60fba95
commit
9bf19d5ff3
|
|
@ -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<bool>();
|
||||
// 读取是否启用详细日志输出
|
||||
verbose_ = config_["verbose"].as<bool>();
|
||||
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<Feature> &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<Feature> &features,
|
|||
AINFO << "Odometer initialized...";
|
||||
return;
|
||||
}
|
||||
|
||||
// 开始计时，用于后续性能调试
|
||||
BLOCK_TIMER_START;
|
||||
|
||||
// 定义匹配成功的点对集合：点-平面对（pp_pairs）和点-直线对（pl_pairs）
|
||||
std::vector<PointPlanePair> pp_pairs;
|
||||
std::vector<PointLinePair> pl_pairs;
|
||||
|
||||
// 根据配置中的 ICP 迭代次数进行多次优化迭代
|
||||
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
|
||||
// 清空上一次迭代的匹配结果
|
||||
std::vector<PointLinePair>().swap(pl_pairs);
|
||||
std::vector<PointPlanePair>().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<size_t>()) {
|
||||
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<int>(),
|
||||
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<PointLinePair> *const pairs) const {
|
||||
// 如果角点 kd-tree 的输入点云为空，则返回
|
||||
if (kdtree_corn_.getInputCloud()->empty()) return;
|
||||
// 从配置中获取角点匹配距离阈值（平方距离）
|
||||
double dist_sq_thresh = config_["corn_match_dist_sq_th"].as<double>();
|
||||
// 遍历当前帧中每个角点
|
||||
for (const auto &pt : src) {
|
||||
// 将当前点通过当前帧相对上一帧的变换，转换到上一帧坐标系中
|
||||
TPoint query_pt = TransformToStart(pose_curr2last_, pt);
|
||||
std::vector<int> indices;
|
||||
std::vector<float> 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<int>();
|
||||
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<int>(0, pt1_scan_id - nearby_scan_num);
|
||||
int i_end = std::min<int>(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<PointPlanePair> *const pairs) const {
|
||||
// 如果平面点 kd-tree 的输入点云为空，则直接返回
|
||||
if (kdtree_surf_.getInputCloud()->empty()) return;
|
||||
// 从配置中获取平面点匹配距离阈值（平方距离）
|
||||
double dist_sq_thresh = config_["surf_match_dist_sq_th"].as<double>();
|
||||
// 遍历当前帧中每个平面点
|
||||
for (const auto &pt : src) {
|
||||
// 将当前点转换到上一帧坐标系中
|
||||
TPoint query_pt = TransformToStart(pose_curr2last_, pt);
|
||||
std::vector<int> indices;
|
||||
std::vector<float> 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<int>();
|
||||
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<Feature> &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<Feature> &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<PointLinePair> &pl_pairs,
|
||||
const std::vector<PointPlanePair> &pp_pairs,
|
||||
double timestamp) const {
|
||||
// 构造一个可视化帧，用于存放当前帧的各项数据
|
||||
std::shared_ptr<OdometerVisFrame> 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
|
||||
|
|
|
|||
|
|
@ -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<Feature> &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<Feature> &features);
|
||||
|
||||
/**
|
||||
* @brief 匹配角点，构造点-直线匹配对
|
||||
*
|
||||
* 在输入的角点点云中搜索匹配点，并构造点-直线匹配对，
|
||||
* 该匹配对将用于后续位姿求解。
|
||||
*
|
||||
* @param src 输入的角点点云
|
||||
* @param pairs 输出的匹配结果（点-直线匹配对集合）
|
||||
*/
|
||||
void MatchCorn(const TPointCloud &src,
|
||||
std::vector<PointLinePair> *const pairs) const;
|
||||
|
||||
/**
|
||||
* @brief 匹配平面点，构造点-平面对
|
||||
*
|
||||
* 在输入的平面点点云中搜索匹配点，并构造点-平面对，
|
||||
* 该匹配对将用于后续位姿求解。
|
||||
*
|
||||
* @param src 输入的平面点点云
|
||||
* @param pairs 输出的匹配结果（点-平面对集合）
|
||||
*/
|
||||
void MatchSurf(const TPointCloud &src,
|
||||
std::vector<PointPlanePair> *const pairs) const;
|
||||
|
||||
/**
|
||||
* @brief 可视化匹配结果与位姿信息
|
||||
*
|
||||
* 利用可视化工具将当前匹配的点对以及位姿信息进行展示，
|
||||
* 便于调试和监控系统运行状态。
|
||||
*
|
||||
* @param pl_pairs 点-直线匹配对集合
|
||||
* @param pp_pairs 点-平面对集合
|
||||
* @param timestamp 当前时间戳（默认为 0.0）
|
||||
*/
|
||||
void Visualize(const std::vector<PointLinePair> &pl_pairs,
|
||||
const std::vector<PointPlanePair> &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<TPoint> &kdtree,
|
||||
const TPoint &query_pt, int k, float dist_sq_th,
|
||||
std::vector<int> *const indices) const;
|
||||
|
||||
// 当前帧到世界坐标系的全局位姿
|
||||
common::Pose3d pose_curr2world_;
|
||||
// 当前帧相对于上一帧的位姿变换
|
||||
common::Pose3d pose_curr2last_;
|
||||
|
||||
// 存储每个扫描线的平面点 kd-tree，用于加速平面点匹配
|
||||
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_scan_surf_;
|
||||
// 存储每个扫描线的角点 kd-tree，用于加速角点匹配
|
||||
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_scan_corn_;
|
||||
// 全局角点 kd-tree（用于全局角点匹配）
|
||||
pcl::KdTreeFLANN<TPoint> kdtree_corn_;
|
||||
// 全局平面点 kd-tree（用于全局平面点匹配）
|
||||
pcl::KdTreeFLANN<TPoint> kdtree_surf_;
|
||||
|
||||
// 存储里程计相关的配置参数（例如匹配距离阈值、迭代次数等）
|
||||
YAML::Node config_;
|
||||
|
||||
// 里程计是否已初始化
|
||||
bool is_initialized_ = false;
|
||||
|
||||
// 是否启用可视化
|
||||
bool is_vis_ = false;
|
||||
|
||||
// 是否输出详细日志信息
|
||||
bool verbose_ = false;
|
||||
|
||||
// 里程计可视化工具（用于展示匹配和位姿估计结果）
|
||||
std::unique_ptr<OdometerVisualizer> visualizer_{nullptr};
|
||||
|
||||
// 禁止拷贝构造函数和赋值操作，防止对象被误拷贝
|
||||
DISALLOW_COPY_AND_ASSIGN(Odometer)
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue