From e7913bc1dda539d3c1258e841a14a1ebaaf02f02 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Sun, 23 Feb 2025 00:18:06 +0800 Subject: [PATCH] =?UTF-8?q?feat:map=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/map.h | 57 +++- oh_my_loam/mapper/mapper.cc | 541 ++++++++++++++++++++---------------- 2 files changed, 346 insertions(+), 252 deletions(-) diff --git a/oh_my_loam/mapper/map.h b/oh_my_loam/mapper/map.h index 4101cb2..a637908 100644 --- a/oh_my_loam/mapper/map.h +++ b/oh_my_loam/mapper/map.h @@ -11,11 +11,17 @@ class Row; class Grid; +// Index 结构体，表示地图中的一个索引位置，包含三个维度（k, j, i） struct Index { - int k, j, i; + int k, j, i; // 三个维度的索引 + + // 默认构造函数 Index() = default; + + // 构造函数，初始化k, j, i Index(int k, int j, int i) : k(k), j(j), i(i) {} + // 用于排序的比较函数对象，按k、j、i的顺序依次比较 struct Comp { bool operator()(const Index &idx1, const Index &idx2) const { return (idx1.k < idx2.k) || (idx1.k == idx2.k && idx1.j < idx2.j) || @@ -24,53 +30,75 @@ struct Index { }; }; +// Map 类，表示一个三维网格地图 class Map { public: + // 构造函数，使用地图的形状和步长初始化地图 Map(const std::vector &shape, const std::vector &step); + // 构造函数，使用地图的形状和步长初始化地图 Map(const std::vector &shape, double step); + // 根据索引返回指定位置的点云 TPointCloudPtr &at(const Index &index); + // 根据索引返回指定位置的点云（const版本） const TPointCloudPtr &at(const Index &index) const; + // 清空地图 void clear(); + // Z轴方向上平移地图n个单位 void ShiftZ(int n); + // Y轴方向上平移地图n个单位 void ShiftY(int n); + // X轴方向上平移地图n个单位 void ShiftX(int n); + // 返回地图的形状 const std::vector shape() const { return std::vector(shape_, shape_ + 3); } + // 根据点的位置计算该点在地图中的索引 Index GetIndex(const TPoint &point) const; + // 检查索引是否有效 bool CheckIndex(const Index &index) const; + // 获取基于点和子地图形状的点云 TPointCloudPtr GetSubmapPoints(const TPoint &point, const std::vector &submap_shapes) const; + // 获取所有的点云 TPointCloudPtr GetAllPoints() const; + // 向地图中添加点云 void AddPoints(const TPointCloudConstPtr &cloud, std::vector *const indices = nullptr); + // 对点云进行下采样 void Downsample(double voxel_size); + // 对指定的点云索引进行下采样 void Downsample(const std::vector &indices, double voxel_size); private: + // 获取指定的Z轴索引的网格 Grid &at(int z_idx); + // 获取指定的Z轴索引的网格（const版本） const Grid &at(int z_idx) const; + // 获取指定Z轴和Y轴索引的行 Row &at(int z_idx, int y_idx); + // 获取指定Z轴和Y轴索引的行（const版本） const Row &at(int z_idx, int y_idx) const; + // 获取指定Z轴、Y轴和X轴索引的点云 TPointCloudPtr &at(int z_idx, int y_idx, int x_idx); const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const; @@ -78,47 +106,60 @@ class Map { std::vector GetSubmapIndices( const TPoint &point, const std::vector &submap_shapes) const; + // 获取地图的所有索引 std::vector GetAllIndices() const; - int shape_[3], center_[3]; // order: z, y, x + int shape_[3], center_[3]; // 地图的形状和中心位置（z, y, x） - double step_[3]; + double step_[3]; // 步长（分辨率） - std::vector map_; + std::vector map_; // 存储地图的网格 - DISALLOW_COPY_AND_ASSIGN(Map); + DISALLOW_COPY_AND_ASSIGN(Map); // 禁止拷贝和赋值 }; +// Row 类，表示地图中的一行，包含多个点云 class Row { public: + // 构造函数，初始化行的大小 explicit Row(int n); + // 根据索引返回指定位置的点云 TPointCloudPtr &at(int idx); + // 根据索引返回指定位置的点云（const版本） const TPointCloudPtr &at(int idx) const; + // 清空该行的点云 void clear(); + // 获取该行的所有点云 TPointCloudPtr GetAllPoints() const; private: - std::vector row_; + std::vector row_; // 存储行的点云 }; +// Grid 类，表示地图中的一个网格，包含多个行 class Grid { public: + // 构造函数，初始化网格的大小 Grid(int m, int n); + // 清空网格 void clear(); + // 获取指定索引的行 Row &at(int idx); + // 获取指定索引的行（const版本） const Row &at(int idx) const; + // 获取该网格的所有点云 TPointCloudPtr GetAllPoints() const; private: - std::vector grid_; + std::vector grid_; // 存储网格中的行 }; -} // namespace oh_my_loam \ No newline at end of file +} // namespace oh_my_loam diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index 41255bf..27bd1e0 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -10,254 +10,307 @@ namespace oh_my_loam { -namespace { -using common::YAMLConfig; -using LineCoeff = Eigen::Matrix; // 定义直线系数类型，包含 6 个元素 -} // namespace + namespace { + using common::YAMLConfig; + 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); // 如果启用可视化，初始化可视化器 - return true; -} - -// 重置地图 -void Mapper::Reset() { - 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..."; // 打印初始化完成信息 - return; + // 初始化地图参数和设置 + 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); // 如果启用可视化，初始化可视化器 + return true; } - if (GetState() == DONE) { // 如果地图已初始化 - thread_.reset(new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf, - pose_curr2odom)); // 启动新的线程来运行地图更新 - if (thread_->joinable()) thread_->detach(); // 分离线程 + + // 重置地图 + void Mapper::Reset() { + SetState(UN_INIT); // 设置状态为未初始化 } - 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; // 计算当前位姿到地图的变换 - TPoint t_vec(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(), - pose_curr2map.t_vec().z()); // 提取平移向量 - AdjustMap(t_vec); // 调整地图 - TPointCloudPtr cloud_corn_map = - 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 - 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) { // 进行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); // 匹配表面点 - 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_curr2map); // 创建姿态求解器 - for (const auto &pair : pl_pairs) { - solver.AddPointLineCoeffPair(pair, 1.0); // 添加点-直线配对 - } - for (const auto &pair : pp_pairs) { - solver.AddPointPlaneCoeffPair(pair, 1.0); // 添加点-平面配对 - } - if (!solver.Solve(config_["solve_iter_num"].as(), verbose_, - &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(); // 打印位姿 - AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT; - 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; // 如果输入点云为空，返回 - std::vector indices; - std::vector dists; - 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); // 变换当前点 - if (kdtree_corn_.nearestKSearch(pt_query, nearest_neighbor_k, indices, - dists) != nearest_neighbor_k) { - continue; - } - if (dists.back() > neighbor_point_dist_sq_th) continue; // 如果距离太大，跳过 - TPointCloud 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); // 添加点-直线配对 - } -} - -// 匹配表面点 -void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr, - const common::Pose3d &pose_curr2map, - std::vector *const pairs) const { - if (kdtree_surf_.getInputCloud()->empty()) return; // 如果输入点云为空，返回 - std::vector indices; - std::vector dists; - 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); // 变换当前点 - if (kdtree_surf_.nearestKSearch(pt_query, nearest_neighbor_k, indices, - dists) != nearest_neighbor_k) { - continue; - } - if (dists.back() > neighbor_point_dist_sq_th) continue; // 如果距离太大，跳过 - TPointCloud 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); // 生成平面上的点 - pairs->emplace_back(pt, coeff); - } -} - -void Mapper::AdjustMap(const TPoint ¢er) { - Index index = corn_map_->GetIndex(center); - int min_idx_z = submap_shape_[0] / 2 + 1; - int max_idx_z = map_shape_[0] - min_idx_z - 1; - if (index.k < min_idx_z) { - corn_map_->ShiftZ(index.k - min_idx_z); - surf_map_->ShiftZ(index.k - min_idx_z); - } - if (index.k > max_idx_z) { - corn_map_->ShiftZ(index.k - max_idx_z); - surf_map_->ShiftZ(index.k - max_idx_z); - } - int min_idx_y = submap_shape_[0] / 2 + 1; - int max_idx_y = map_shape_[1] - min_idx_y - 1; - if (index.j < min_idx_y) { - corn_map_->ShiftY(index.j - min_idx_y); - surf_map_->ShiftY(index.j - min_idx_y); - } - if (index.j > max_idx_y) { - corn_map_->ShiftY(index.j - max_idx_y); - surf_map_->ShiftY(index.j - max_idx_y); - } - int min_idx_x = submap_shape_[0] / 2 + 1; - int max_idx_x = map_shape_[2] - min_idx_x - 1; - if (index.i < min_idx_x) { - corn_map_->ShiftX(index.i - min_idx_x); - surf_map_->ShiftX(index.i - min_idx_x); - } - if (index.i > max_idx_x) { - corn_map_->ShiftX(index.i - max_idx_x); - surf_map_->ShiftX(index.i - max_idx_x); - } -} - -void Mapper::UpdateMap(const common::Pose3d &pose_curr2map, - const TPointCloudConstPtr &cloud_corn, - const TPointCloudConstPtr &cloud_surf) { - auto update_map = [&](const TPointCloudConstPtr &cloud, - Map *const map /* const TPointCloudPtr &cloud_map*/) { - TPointCloudPtr cloud_trans(new TPointCloud); - common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get()); - std::vector indices; - map->AddPoints(cloud_trans, &indices); - map->Downsample(indices, config_["downsample_voxel_size"].as()); - }; - std::lock_guard lock(map_mutex_); - update_map(cloud_corn, corn_map_.get()); - update_map(cloud_surf, surf_map_.get()); -} - -TPointCloudPtr Mapper::GetMapCloudCorn() const { - std::lock_guard lock(map_mutex_); - return corn_map_->GetAllPoints(); -} - -TPointCloudPtr Mapper::GetMapCloudSurf() const { - std::lock_guard lock(map_mutex_); - return surf_map_->GetAllPoints(); -} - -TPointCloudPtr Mapper::GetMapCloud() const { - TPointCloudPtr map_points(new TPointCloud); - std::lock_guard lock(map_mutex_); - *map_points += *corn_map_->GetAllPoints(); - *map_points += *surf_map_->GetAllPoints(); - return map_points; -} - -Mapper::State Mapper::GetState() const { - std::lock_guard lock(state_mutex_); - return state_; -} - -void Mapper::SetState(State state) { - std::lock_guard lock(state_mutex_); - state_ = state; -} - -void Mapper::Visualize(const std::vector &pl_pairs, - const std::vector &pp_pairs, + // 处理每一帧点云数据 + void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn, + const TPointCloudConstPtr &cloud_surf, const common::Pose3d &pose_curr2odom, - const common::Pose3d &pose_curr2map, double timestamp) { - std::shared_ptr frame(new MapperVisFrame); - 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_curr2odom = pose_curr2odom; - frame->pose_curr2map = pose_curr2map; - visualizer_->Render(frame); -} + 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..."; // 打印初始化完成信息 + return; + } + if (GetState() == DONE) { // 如果地图已初始化 + thread_.reset( + new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf, + 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(); // 打印位姿 + } + + // 执行映射操作 + 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; // 计算当前位姿到地图的变换 + TPoint t_vec(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(), + pose_curr2map.t_vec().z()); // 提取平移向量 + AdjustMap(t_vec); // 调整地图，使得当前位姿处于地图的中心附近 + TPointCloudPtr cloud_corn_map = + 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 + 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) { // 进行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); // 匹配表面点 + 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_curr2map); // 创建姿态求解器 + for (const auto &pair : pl_pairs) { + solver.AddPointLineCoeffPair(pair, 1.0); // 添加点-直线配对 + } + for (const auto &pair : pp_pairs) { + solver.AddPointPlaneCoeffPair(pair, 1.0); // 添加点-平面配对 + } + if (!solver.Solve(config_["solve_iter_num"].as(), verbose_, + &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(); // 打印位姿 + AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT; + 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; // 如果输入点云为空，返回 + std::vector indices; + std::vector dists; + 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); // 变换当前点 + if (kdtree_corn_.nearestKSearch(pt_query, nearest_neighbor_k, indices, + dists) != nearest_neighbor_k) { + continue; + } + if (dists.back() > neighbor_point_dist_sq_th) + continue; // 如果距离太大，跳过 + TPointCloud 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); // 添加点-直线配对 + } + } + + // 匹配表面点 + void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr, + const common::Pose3d &pose_curr2map, + std::vector *const pairs) const { + if (kdtree_surf_.getInputCloud()->empty()) return; // 如果输入点云为空，返回 + std::vector indices; + std::vector dists; + 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); // 变换当前点 + if (kdtree_surf_.nearestKSearch(pt_query, nearest_neighbor_k, indices, + dists) != nearest_neighbor_k) { + continue; + } + if (dists.back() > neighbor_point_dist_sq_th) + continue; // 如果距离太大，跳过 + TPointCloud 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); // 生成平面上的点 + pairs->emplace_back(pt, coeff); + } + } + // 调整地图的显示范围和更新位置 + void Mapper::AdjustMap(const TPoint ¢er) { + // 获取当前点在角点地图中的索引 + Index index = corn_map_->GetIndex(center); + + // Z轴方向上的最小和最大索引值，用于调整地图的上下边界 + int min_idx_z = submap_shape_[0] / 2 + 1; // 最小Z轴索引 + int max_idx_z = map_shape_[0] - min_idx_z - 1; // 最大Z轴索引 + if (index.k < min_idx_z) { // 如果当前索引小于最小值 + // 调整Z轴位置 + corn_map_->ShiftZ(index.k - min_idx_z); + surf_map_->ShiftZ(index.k - min_idx_z); + } + if (index.k > max_idx_z) { // 如果当前索引大于最大值 + // 调整Z轴位置 + corn_map_->ShiftZ(index.k - max_idx_z); + surf_map_->ShiftZ(index.k - max_idx_z); + } + + // Y轴方向上的最小和最大索引值，用于调整地图的前后边界 + int min_idx_y = submap_shape_[0] / 2 + 1; // 最小Y轴索引 + int max_idx_y = map_shape_[1] - min_idx_y - 1; // 最大Y轴索引 + if (index.j < min_idx_y) { // 如果当前索引小于最小值 + // 调整Y轴位置 + corn_map_->ShiftY(index.j - min_idx_y); + surf_map_->ShiftY(index.j - min_idx_y); + } + if (index.j > max_idx_y) { // 如果当前索引大于最大值 + // 调整Y轴位置 + corn_map_->ShiftY(index.j - max_idx_y); + surf_map_->ShiftY(index.j - max_idx_y); + } + + // X轴方向上的最小和最大索引值，用于调整地图的左右边界 + int min_idx_x = submap_shape_[0] / 2 + 1; // 最小X轴索引 + int max_idx_x = map_shape_[2] - min_idx_x - 1; // 最大X轴索引 + if (index.i < min_idx_x) { // 如果当前索引小于最小值 + // 调整X轴位置 + corn_map_->ShiftX(index.i - min_idx_x); + surf_map_->ShiftX(index.i - min_idx_x); + } + if (index.i > max_idx_x) { // 如果当前索引大于最大值 + // 调整X轴位置 + corn_map_->ShiftX(index.i - max_idx_x); + surf_map_->ShiftX(index.i - max_idx_x); + } + } + + // 更新地图：根据当前的位姿和角点、表面点云更新地图 + void Mapper::UpdateMap(const common::Pose3d &pose_curr2map, + const TPointCloudConstPtr &cloud_corn, + const TPointCloudConstPtr &cloud_surf) { + // 定义一个局部函数，用于更新地图 + auto update_map = [&](const TPointCloudConstPtr &cloud, + Map *const map /* const TPointCloudPtr &cloud_map*/) { + TPointCloudPtr cloud_trans(new TPointCloud); // 创建存储变换后点云的指针 + common::TransformPointCloud(pose_curr2map, *cloud, + cloud_trans.get()); // 根据位姿变换点云 + std::vector indices; // 存储点云索引 + map->AddPoints(cloud_trans, &indices); // 将变换后的点云加入地图 + map->Downsample( + indices, + config_["downsample_voxel_size"].as()); // 对点云进行下采样 + }; + + // 使用锁来保证多线程安全 + std::lock_guard lock(map_mutex_); + + // 更新角点地图和表面地图 + update_map(cloud_corn, corn_map_.get()); + update_map(cloud_surf, surf_map_.get()); + } + + // 获取角点地图的所有点云 + TPointCloudPtr Mapper::GetMapCloudCorn() const { + std::lock_guard lock(map_mutex_); // 锁住，避免多线程竞争 + return corn_map_->GetAllPoints(); // 返回角点地图中的所有点 + } + + // 获取表面地图的所有点云 + TPointCloudPtr Mapper::GetMapCloudSurf() const { + std::lock_guard lock(map_mutex_); // 锁住，避免多线程竞争 + return surf_map_->GetAllPoints(); // 返回表面地图中的所有点 + } + + // 获取所有地图（包括角点和表面点云） + TPointCloudPtr Mapper::GetMapCloud() const { + TPointCloudPtr map_points(new TPointCloud); // 创建一个新的点云指针 + std::lock_guard lock(map_mutex_); // 锁住，避免多线程竞争 + + // 将角点地图和表面地图的所有点加入到最终的地图中 + *map_points += *corn_map_->GetAllPoints(); + *map_points += *surf_map_->GetAllPoints(); + + return map_points; // 返回完整的地图 + } + + // 获取当前的地图状态 + Mapper::State Mapper::GetState() const { + std::lock_guard lock(state_mutex_); // 锁住状态 + return state_; // 返回当前状态 + } + + // 设置地图的状态 + void Mapper::SetState(State state) { + std::lock_guard lock(state_mutex_); // 锁住状态 + state_ = state; // 设置新的状态 + } + + // 可视化匹配的点对和位姿信息 + void Mapper::Visualize(const std::vector &pl_pairs, + const std::vector &pp_pairs, + const common::Pose3d &pose_curr2odom, + const common::Pose3d &pose_curr2map, double timestamp) { + std::shared_ptr frame(new MapperVisFrame); // 创建可视化帧 + 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_curr2odom = pose_curr2odom; // 设置当前里程计位姿 + frame->pose_curr2map = pose_curr2map; // 设置当前地图位姿 + + // 渲染可视化帧 + visualizer_->Render(frame); + } } // namespace oh_my_loam \ No newline at end of file