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