feat:map添加中文注释

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<int> &shape, const std::vector<double> &step);
 
+  // 构造函数，使用地图的形状和步长初始化地图
   Map(const std::vector<int> &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<int> shape() const {
     return std::vector<int>(shape_, shape_ + 3);
   }
 
+  // 根据点的位置计算该点在地图中的索引
   Index GetIndex(const TPoint &point) const;
 
+  // 检查索引是否有效
   bool CheckIndex(const Index &index) const;
 
+  // 获取基于点和子地图形状的点云
   TPointCloudPtr GetSubmapPoints(const TPoint &point,
                                  const std::vector<int> &submap_shapes) const;
 
+  // 获取所有的点云
   TPointCloudPtr GetAllPoints() const;
 
+  // 向地图中添加点云
   void AddPoints(const TPointCloudConstPtr &cloud,
                  std::vector<Index> *const indices = nullptr);
 
+  // 对点云进行下采样
   void Downsample(double voxel_size);
 
+  // 对指定的点云索引进行下采样
   void Downsample(const std::vector<Index> &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<Index> GetSubmapIndices(
       const TPoint &point, const std::vector<int> &submap_shapes) const;
 
+  // 获取地图的所有索引
   std::vector<Index> 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<Grid> map_;
+  std::vector<Grid> 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<TPointCloudPtr> row_;
+  std::vector<TPointCloudPtr> 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<Row> grid_;
+  std::vector<Row> grid_;  // 存储网格中的行
 };
 
 }  // namespace oh_my_loam

oh_my_loam/mapper/mapper.cc

@@ -12,7 +12,8 @@ namespace oh_my_loam {
 
   namespace {
   using common::YAMLConfig;
-using LineCoeff = Eigen::Matrix<double, 6, 1>;   // 定义直线系数类型，包含 6 个元素
+  using LineCoeff =
+      Eigen::Matrix<double, 6, 1>;  // 定义直线系数类型，包含 6 个元素
   }  // namespace
 
   // 初始化地图参数和设置
@@ -21,13 +22,16 @@ bool Mapper::Init() {
     config_ = config["mapper_config"];                      // 读取映射配置
     is_vis_ = config_["vis"].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"]);  // 获取地图的形状
-  submap_shape_ = YAMLConfig::GetSeq<int>(config_["submap_shape"]);   // 获取子地图的形状
+    submap_shape_ =
+        YAMLConfig::GetSeq<int>(config_["submap_shape"]);  // 获取子地图的形状
     map_step_ = config_["map_step"].as<double>();     // 获取地图分辨率
     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);  // 如果启用可视化，初始化可视化器
+    if (is_vis_)
+      visualizer_.reset(new MapperVisualizer);  // 如果启用可视化，初始化可视化器
     return true;
   }
 
@@ -50,7 +54,8 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
       return;
     }
     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));  // 启动新的线程来运行地图更新
       if (thread_->joinable()) thread_->detach();  // 分离线程
     }
@@ -65,10 +70,11 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
                    const common::Pose3d &pose_curr2odom) {
     BLOCK_TIMER_START;
     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(),
                  pose_curr2map.t_vec().z());  // 提取平移向量
-  AdjustMap(t_vec);   // 调整地图
+    AdjustMap(t_vec);                         // 调整地图，使得当前位姿处于地图的中心附近
     TPointCloudPtr cloud_corn_map =
         corn_map_->GetSubmapPoints(t_vec, submap_shape_);  // 获取角点子地图
     TPointCloudPtr cloud_surf_map =
@@ -84,9 +90,11 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
       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<size_t>()) {   // 如果匹配点对太少，跳过当前迭代
+          config_["min_correspondence_num"]
+              .as<size_t>()) {  // 如果匹配点对太少，跳过当前迭代
         AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
               << pp_pairs.size();
         continue;
@@ -107,10 +115,12 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
     }
     UpdateMap(pose_curr2map, cloud_corn, cloud_surf);  // 更新地图
     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();  // 打印位姿
     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;  // 设置状态为已完成，注意死锁问题
   }
 
@@ -121,7 +131,8 @@ void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
     if (kdtree_corn_.getInputCloud()->empty()) return;  // 如果输入点云为空，返回
     std::vector<int> indices;
     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 =
         config_["neighbor_point_dist_sq_th"].as<float>();  // 获取邻点距离阈值
     for (const auto &pt : *cloud_curr) {                   // 遍历当前点云
@@ -130,12 +141,15 @@ void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
                                       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<double>()) continue; // 如果拟合分数太低，跳过
+      if (fit_score < config_["min_line_fit_score"].as<double>())
+        continue;                      // 如果拟合分数太低，跳过
       pairs->emplace_back(pt, coeff);  // 添加点-直线配对
     }
   }
@@ -147,7 +161,8 @@ void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr,
     if (kdtree_surf_.getInputCloud()->empty()) return;  // 如果输入点云为空，返回
     std::vector<int> indices;
     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 =
         config_["neighbor_point_dist_sq_th"].as<float>();  // 获取邻点距离阈值
     for (const auto &pt : *cloud_curr) {                   // 遍历当前点云
@@ -156,107 +171,145 @@ void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr,
                                       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);   // 拟合平面
+      Eigen::Vector4d coeff =
-    if (fit_score < config_["min_plane_fit_score"].as<double>()) continue;    // 如果拟合分数太低，跳过
+          common::FitPlane(neighbor_pts, &fit_score);  // 拟合平面
+      if (fit_score < config_["min_plane_fit_score"].as<double>())
+        continue;  // 如果拟合分数太低，跳过
       TPoint pt1(coeff[0], coeff[1], coeff[2], 0.0);  // 生成平面上的点
       pairs->emplace_back(pt, coeff);
     }
   }
-
+  // 调整地图的显示范围和更新位置
   void Mapper::AdjustMap(const TPoint &center) {
+    // 获取当前点在角点地图中的索引
     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;
+    // Z轴方向上的最小和最大索引值，用于调整地图的上下边界
-  if (index.k < min_idx_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) {
+    if (index.k > max_idx_z) {  // 如果当前索引大于最大值
+      // 调整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;
+    // Y轴方向上的最小和最大索引值，用于调整地图的前后边界
-  if (index.j < min_idx_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) {
+    if (index.j > max_idx_y) {  // 如果当前索引大于最大值
+      // 调整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;
+    // X轴方向上的最小和最大索引值，用于调整地图的左右边界
-  if (index.i < min_idx_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) {
+    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);
+      TPointCloudPtr cloud_trans(new TPointCloud);  // 创建存储变换后点云的指针
-    common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get());
+      common::TransformPointCloud(pose_curr2map, *cloud,
-    std::vector<Index> indices;
+                                  cloud_trans.get());  // 根据位姿变换点云
-    map->AddPoints(cloud_trans, &indices);
+      std::vector<Index> indices;                      // 存储点云索引
-    map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
+      map->AddPoints(cloud_trans, &indices);  // 将变换后的点云加入地图
+      map->Downsample(
+          indices,
+          config_["downsample_voxel_size"].as<double>());  // 对点云进行下采样
     };
+
+    // 使用锁来保证多线程安全
     std::lock_guard<std::mutex> lock(map_mutex_);
+
+    // 更新角点地图和表面地图
     update_map(cloud_corn, corn_map_.get());
     update_map(cloud_surf, surf_map_.get());
   }
 
+  // 获取角点地图的所有点云
   TPointCloudPtr Mapper::GetMapCloudCorn() const {
-  std::lock_guard<std::mutex> lock(map_mutex_);
+    std::lock_guard<std::mutex> lock(map_mutex_);  // 锁住，避免多线程竞争
-  return corn_map_->GetAllPoints();
+    return corn_map_->GetAllPoints();  // 返回角点地图中的所有点
   }
 
+  // 获取表面地图的所有点云
   TPointCloudPtr Mapper::GetMapCloudSurf() const {
-  std::lock_guard<std::mutex> lock(map_mutex_);
+    std::lock_guard<std::mutex> lock(map_mutex_);  // 锁住，避免多线程竞争
-  return surf_map_->GetAllPoints();
+    return surf_map_->GetAllPoints();  // 返回表面地图中的所有点
   }
 
+  // 获取所有地图（包括角点和表面点云）
   TPointCloudPtr Mapper::GetMapCloud() const {
-  TPointCloudPtr map_points(new TPointCloud);
+    TPointCloudPtr map_points(new TPointCloud);  // 创建一个新的点云指针
-  std::lock_guard<std::mutex> lock(map_mutex_);
+    std::lock_guard<std::mutex> lock(map_mutex_);  // 锁住，避免多线程竞争
+
+    // 将角点地图和表面地图的所有点加入到最终的地图中
     *map_points += *corn_map_->GetAllPoints();
     *map_points += *surf_map_->GetAllPoints();
-  return map_points;
+
+    return map_points;  // 返回完整的地图
   }
 
+  // 获取当前的地图状态
   Mapper::State Mapper::GetState() const {
-  std::lock_guard<std::mutex> lock(state_mutex_);
+    std::lock_guard<std::mutex> lock(state_mutex_);  // 锁住状态
-  return state_;
+    return state_;                                   // 返回当前状态
   }
 
+  // 设置地图的状态
   void Mapper::SetState(State state) {
-  std::lock_guard<std::mutex> lock(state_mutex_);
+    std::lock_guard<std::mutex> lock(state_mutex_);  // 锁住状态
-  state_ = state;
+    state_ = state;                                  // 设置新的状态
   }
 
+  // 可视化匹配的点对和位姿信息
   void Mapper::Visualize(const std::vector<PointLineCoeffPair> &pl_pairs,
                          const std::vector<PointPlaneCoeffPair> &pp_pairs,
                          const common::Pose3d &pose_curr2odom,
                          const common::Pose3d &pose_curr2map, double timestamp) {
-  std::shared_ptr<MapperVisFrame> frame(new MapperVisFrame);
+    std::shared_ptr<MapperVisFrame> frame(new MapperVisFrame);  // 创建可视化帧
-  frame->timestamp = timestamp;
+    frame->timestamp = timestamp;                               // 设置时间戳
-  frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared();
+    frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared();  // 设置角点云
-  frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared();
+    frame->cloud_surf =
-  frame->pl_pairs = pl_pairs;
+        kdtree_surf_.getInputCloud()->makeShared();  // 设置表面点云
-  frame->pp_pairs = pp_pairs;
+    frame->pl_pairs = pl_pairs;                      // 设置点-直线配对
-  frame->pose_curr2odom = pose_curr2odom;
+    frame->pp_pairs = pp_pairs;                      // 设置点-平面配对
-  frame->pose_curr2map = pose_curr2map;
+    frame->pose_curr2odom = pose_curr2odom;  // 设置当前里程计位姿
+    frame->pose_curr2map = pose_curr2map;    // 设置当前地图位姿
+
+    // 渲染可视化帧
     visualizer_->Render(frame);
   }