12345qiupeng
/
oh_my_loam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat:添加中文注释

main
邱棚 2025-02-21 17:22:15 +08:00
parent 33f470c6c5
commit a92e8b2a9c
1 changed files with 74 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

142
oh_my_loam/mapper/mapper.cc
View File

@ -12,151 +12,157 @@ namespace oh_my_loam {
namespace { namespace {
using common::YAMLConfig; using common::YAMLConfig;
using LineCoeff = Eigen::Matrix<double, 6, 1>; using LineCoeff = Eigen::Matrix<double, 6, 1>; // 定义直线系数类型，包含 6 个元素
} // namespace } // namespace
// 初始化地图参数和设置
bool Mapper::Init() { bool Mapper::Init() {
const auto &config = YAMLConfig::Instance()->config(); const auto &config = YAMLConfig::Instance()->config(); // 获取配置文件
config_ = config["mapper_config"]; config_ = config["mapper_config"]; // 读取映射配置
is_vis_ = config_["vis"].as<bool>(); is_vis_ = config_["vis"].as<bool>(); // 是否启用可视化
verbose_ = config_["verbose"].as<bool>(); verbose_ = config_["verbose"].as<bool>(); // 是否输出详细日志
AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF"); AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF"); // 打印可视化状态
map_shape_ = YAMLConfig::GetSeq<int>(config_["map_shape"]); map_shape_ = YAMLConfig::GetSeq<int>(config_["map_shape"]); // 获取地图的形状
submap_shape_ = YAMLConfig::GetSeq<int>(config_["submap_shape"]); submap_shape_ = YAMLConfig::GetSeq<int>(config_["submap_shape"]); // 获取子地图的形状
map_step_ = config_["map_step"].as<double>(); map_step_ = config_["map_step"].as<double>(); // 获取地图分辨率
corn_map_.reset(new Map(map_shape_, map_step_)); corn_map_.reset(new Map(map_shape_, map_step_)); // 初始化角点地图
surf_map_.reset(new Map(map_shape_, map_step_)); surf_map_.reset(new Map(map_shape_, map_step_)); // 初始化表面地图
if (is_vis_) visualizer_.reset(new MapperVisualizer); if (is_vis_) visualizer_.reset(new MapperVisualizer); // 如果启用可视化，初始化可视化器
return true; return true;
} }
// 重置地图
void Mapper::Reset() { void Mapper::Reset() {
SetState(UN_INIT); SetState(UN_INIT); // 设置状态为未初始化
} }
// 处理每一帧点云数据
void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn, void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf, const TPointCloudConstPtr &cloud_surf,
const common::Pose3d &pose_curr2odom, const common::Pose3d &pose_curr2odom,
common::Pose3d *const pose_curr2map) { common::Pose3d *const pose_curr2map) {
if (GetState() == UN_INIT) { if (GetState() == UN_INIT) { // 如果地图尚未初始化
pose_curr2map->SetIdentity(); pose_curr2map->SetIdentity(); // 设置当前位姿为单位矩阵
pose_odom2map_.SetIdentity(); pose_odom2map_.SetIdentity(); // 设置里程计到地图的变换为单位矩阵
UpdateMap(*pose_curr2map, cloud_corn, cloud_surf); UpdateMap(*pose_curr2map, cloud_corn, cloud_surf); // 更新地图
SetState(DONE); SetState(DONE); // 设置状态为已完成
AINFO << "Mapper initialized..."; AINFO << "Mapper initialized..."; // 打印初始化完成信息
return; return;
} }
if (GetState() == DONE) { if (GetState() == DONE) { // 如果地图已初始化
thread_.reset(new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf, thread_.reset(new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf,
pose_curr2odom)); pose_curr2odom)); // 启动新的线程来运行地图更新
if (thread_->joinable()) thread_->detach(); if (thread_->joinable()) thread_->detach(); // 分离线程
} }
std::lock_guard<std::mutex> lock(state_mutex_); std::lock_guard<std::mutex> lock(state_mutex_); // 锁住状态，避免多线程竞争
*pose_curr2map = pose_odom2map_ * pose_curr2odom; *pose_curr2map = pose_odom2map_ * pose_curr2odom; // 计算当前位姿到地图的变换
AINFO << "Pose_curr2map = " << pose_curr2map->ToString(); AINFO << "Pose_curr2map = " << pose_curr2map->ToString(); // 打印位姿
} }
// 执行映射操作
void Mapper::Run(const TPointCloudConstPtr &cloud_corn, void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf, const TPointCloudConstPtr &cloud_surf,
const common::Pose3d &pose_curr2odom) { const common::Pose3d &pose_curr2odom) {
BLOCK_TIMER_START; BLOCK_TIMER_START;
SetState(RUNNING); SetState(RUNNING); // 设置状态为运行中
common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom; common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom; // 计算当前位姿到地图的变换
TPoint t_vec(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(), TPoint t_vec(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
pose_curr2map.t_vec().z()); pose_curr2map.t_vec().z()); // 提取平移向量
AdjustMap(t_vec); AdjustMap(t_vec); // 调整地图
TPointCloudPtr cloud_corn_map = TPointCloudPtr cloud_corn_map =
corn_map_->GetSubmapPoints(t_vec, submap_shape_); corn_map_->GetSubmapPoints(t_vec, submap_shape_); // 获取角点子地图
TPointCloudPtr cloud_surf_map = TPointCloudPtr cloud_surf_map =
surf_map_->GetSubmapPoints(t_vec, submap_shape_); surf_map_->GetSubmapPoints(t_vec, submap_shape_); // 获取表面子地图
kdtree_corn_.setInputCloud(cloud_corn_map); kdtree_corn_.setInputCloud(cloud_corn_map); // 设置角点的kdtree
kdtree_surf_.setInputCloud(cloud_surf_map); kdtree_surf_.setInputCloud(cloud_surf_map); // 设置表面点云的kdtree
std::vector<PointLineCoeffPair> pl_pairs; std::vector<PointLineCoeffPair> pl_pairs;
std::vector<PointPlaneCoeffPair> pp_pairs; std::vector<PointPlaneCoeffPair> pp_pairs;
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) { for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) { // 进行ICP迭代
std::vector<PointLineCoeffPair>().swap(pl_pairs); std::vector<PointLineCoeffPair>().swap(pl_pairs);
std::vector<PointPlaneCoeffPair>().swap(pp_pairs); std::vector<PointPlaneCoeffPair>().swap(pp_pairs);
MatchCorn(cloud_corn, pose_curr2map, &pl_pairs); MatchCorn(cloud_corn, pose_curr2map, &pl_pairs); // 匹配角点
MatchSurf(cloud_surf, pose_curr2map, &pp_pairs); MatchSurf(cloud_surf, pose_curr2map, &pp_pairs); // 匹配表面点
AINFO_IF(verbose_) << "Iter " << i AINFO_IF(verbose_) << "Iter " << i
<< ": matched num: point2line = " << pl_pairs.size() << ": matched num: point2line = " << pl_pairs.size()
<< ", point2plane = " << pp_pairs.size(); << ", point2plane = " << pp_pairs.size(); // 输出匹配结果
if (pl_pairs.size() + pp_pairs.size() < if (pl_pairs.size() + pp_pairs.size() <
config_["min_correspondence_num"].as<size_t>()) { config_["min_correspondence_num"].as<size_t>()) { // 如果匹配点对太少，跳过当前迭代
AWARN << "Too less correspondence: " << pl_pairs.size() << " + " AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
<< pp_pairs.size(); << pp_pairs.size();
continue; continue;
} }
PoseSolver solver(pose_curr2map); PoseSolver solver(pose_curr2map); // 创建姿态求解器
for (const auto &pair : pl_pairs) { for (const auto &pair : pl_pairs) {
solver.AddPointLineCoeffPair(pair, 1.0); solver.AddPointLineCoeffPair(pair, 1.0); // 添加点-直线配对
} }
for (const auto &pair : pp_pairs) { for (const auto &pair : pp_pairs) {
solver.AddPointPlaneCoeffPair(pair, 1.0); solver.AddPointPlaneCoeffPair(pair, 1.0); // 添加点-平面配对
} }
if (!solver.Solve(config_["solve_iter_num"].as<int>(), verbose_, if (!solver.Solve(config_["solve_iter_num"].as<int>(), verbose_,
&pose_curr2map)) { &pose_curr2map)) { // 解决优化问题
AWARN << "Mapping solve: no_convergence"; AWARN << "Mapping solve: no_convergence";
} }
AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": " AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": "
<< BLOCK_TIMER_STOP_FMT; << BLOCK_TIMER_STOP_FMT;
} }
UpdateMap(pose_curr2map, cloud_corn, cloud_surf); UpdateMap(pose_curr2map, cloud_corn, cloud_surf); // 更新地图
std::lock_guard<std::mutex> lock(state_mutex_); std::lock_guard<std::mutex> lock(state_mutex_); // 锁住状态，避免多线程竞争
pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv(); pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv(); // 计算新的里程计到地图的变换
AINFO << "Pose_curr2map = " << pose_curr2map.ToString(); AINFO << "Pose_curr2map = " << pose_curr2map.ToString(); // 打印位姿
AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT; AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(pl_pairs, pp_pairs, pose_curr2odom, pose_curr2map); if (is_vis_) Visualize(pl_pairs, pp_pairs, pose_curr2odom, pose_curr2map); // 可视化结果
state_ = DONE; // be careful with deadlock state_ = DONE; // 设置状态为已完成，注意死锁问题
} }
// 匹配角点
void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr, void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map, const common::Pose3d &pose_curr2map,
std::vector<PointLineCoeffPair> *const pairs) const { std::vector<PointLineCoeffPair> *const pairs) const {
if (kdtree_corn_.getInputCloud()->empty()) return; if (kdtree_corn_.getInputCloud()->empty()) return; // 如果输入点云为空，返回
std::vector<int> indices; std::vector<int> indices;
std::vector<float> dists; std::vector<float> dists;
int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>(); int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>(); // 获取最近邻数目
float neighbor_point_dist_sq_th = float neighbor_point_dist_sq_th =
config_["neighbor_point_dist_sq_th"].as<float>(); config_["neighbor_point_dist_sq_th"].as<float>(); // 获取邻点距离阈值
for (const auto &pt : *cloud_curr) { for (const auto &pt : *cloud_curr) { // 遍历当前点云
TPoint pt_query = common::TransformPoint(pose_curr2map, pt); TPoint pt_query = common::TransformPoint(pose_curr2map, pt); // 变换当前点
if (kdtree_corn_.nearestKSearch(pt_query, nearest_neighbor_k, indices, if (kdtree_corn_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
dists) != nearest_neighbor_k) { dists) != nearest_neighbor_k) {
continue; continue;
} }
if (dists.back() > neighbor_point_dist_sq_th) continue; if (dists.back() > neighbor_point_dist_sq_th) continue; // 如果距离太大，跳过
TPointCloud neighbor_pts; TPointCloud neighbor_pts;
pcl::copyPointCloud(*kdtree_corn_.getInputCloud(), indices, neighbor_pts); pcl::copyPointCloud(*kdtree_corn_.getInputCloud(), indices, neighbor_pts); // 获取邻域点云
double fit_score = 0.0; double fit_score = 0.0;
LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score); LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score); // 拟合直线
if (fit_score < config_["min_line_fit_score"].as<double>()) continue; if (fit_score < config_["min_line_fit_score"].as<double>()) continue; // 如果拟合分数太低，跳过
pairs->emplace_back(pt, coeff); pairs->emplace_back(pt, coeff); // 添加点-直线配对
} }
} }
// 匹配表面点
void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr, void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map, const common::Pose3d &pose_curr2map,
std::vector<PointPlaneCoeffPair> *const pairs) const { std::vector<PointPlaneCoeffPair> *const pairs) const {
if (kdtree_surf_.getInputCloud()->empty()) return; if (kdtree_surf_.getInputCloud()->empty()) return; // 如果输入点云为空，返回
std::vector<int> indices; std::vector<int> indices;
std::vector<float> dists; std::vector<float> dists;
int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>(); int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>(); // 获取最近邻数目
float neighbor_point_dist_sq_th = float neighbor_point_dist_sq_th =
config_["neighbor_point_dist_sq_th"].as<float>(); config_["neighbor_point_dist_sq_th"].as<float>(); // 获取邻点距离阈值
for (const auto &pt : *cloud_curr) { for (const auto &pt : *cloud_curr) { // 遍历当前点云
TPoint pt_query = common::TransformPoint(pose_curr2map, pt); TPoint pt_query = common::TransformPoint(pose_curr2map, pt); // 变换当前点
if (kdtree_surf_.nearestKSearch(pt_query, nearest_neighbor_k, indices, if (kdtree_surf_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
dists) != nearest_neighbor_k) { dists) != nearest_neighbor_k) {
continue; continue;
} }
if (dists.back() > neighbor_point_dist_sq_th) continue; if (dists.back() > neighbor_point_dist_sq_th) continue; // 如果距离太大，跳过
TPointCloud neighbor_pts; TPointCloud neighbor_pts;
pcl::copyPointCloud(*kdtree_surf_.getInputCloud(), indices, neighbor_pts); pcl::copyPointCloud(*kdtree_surf_.getInputCloud(), indices, neighbor_pts); // 获取邻域点云
double fit_score = 0.0; double fit_score = 0.0;
Eigen::Vector4d coeff = common::FitPlane(neighbor_pts, &fit_score); Eigen::Vector4d coeff = common::FitPlane(neighbor_pts, &fit_score); // 拟合平面
if (fit_score < config_["min_plane_fit_score"].as<double>()) continue; if (fit_score < config_["min_plane_fit_score"].as<double>()) continue; // 如果拟合分数太低，跳过
TPoint pt1(coeff[0], coeff[1], coeff[2], 0.0); TPoint pt1(coeff[0], coeff[1], coeff[2], 0.0); // 生成平面上的点
pairs->emplace_back(pt, coeff); pairs->emplace_back(pt, coeff);
} }
} }