feat:添加激光里程计的中文注释

main
邱棚 2025-02-02 01:50:17 +08:00
parent 42e60fba95
commit 9bf19d5ff3
2 changed files with 191 additions and 3 deletions
oh_my_loam/odometer

View File

@ -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

View File

@ -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)
};