From f8e150ef6a02397f9bdee1eeaa168918f2e5a66d Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Mon, 24 Feb 2025 17:29:03 +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.cc | 107 +++++++++++++++++++++++++-------------- 1 file changed, 70 insertions(+), 37 deletions(-) diff --git a/oh_my_loam/mapper/map.cc b/oh_my_loam/mapper/map.cc index 38df0ee..4904f22 100644 --- a/oh_my_loam/mapper/map.cc +++ b/oh_my_loam/mapper/map.cc @@ -4,174 +4,196 @@ namespace oh_my_loam { +// 构造函数：初始化地图的大小和步长 Map::Map(const std::vector &shape, const std::vector &step) { - ACHECK(shape.size() == 3); - std::copy_n(shape.begin(), 3, shape_); - ACHECK(shape_[0] % 2 == 1 && shape_[1] % 2 == 1 && shape_[2] % 2 == 1); - std::copy_n(step.begin(), 3, step_); - ACHECK(step_[0] > 0.0 && step_[1] > 0.0 && step_[2] > 0.0); - center_[0] = shape_[0] / 2; - center_[1] = shape_[1] / 2; - center_[2] = shape_[2] / 2; - for (int i = 0; i < shape_[0]; ++i) map_.emplace_back(shape_[1], shape_[2]); + ACHECK(shape.size() == 3); // 确保 shape 的大小为 3 + std::copy_n(shape.begin(), 3, shape_); // 将 shape 赋值给成员变量 shape_ + ACHECK(shape_[0] % 2 == 1 && shape_[1] % 2 == 1 && shape_[2] % 2 == 1); // 确保各维度是奇数 + std::copy_n(step.begin(), 3, step_); // 将 step 赋值给成员变量 step_ + ACHECK(step_[0] > 0.0 && step_[1] > 0.0 && step_[2] > 0.0); // 确保步长大于 0 + center_[0] = shape_[0] / 2; // 计算中心点的 z 坐标 + center_[1] = shape_[1] / 2; // 计算中心点的 y 坐标 + center_[2] = shape_[2] / 2; // 计算中心点的 x 坐标 + for (int i = 0; i < shape_[0]; ++i) map_.emplace_back(shape_[1], shape_[2]); // 初始化 map_，为每个 z 层创建一个 2D 网格 } +// 另一个构造函数：只传递步长参数 Map::Map(const std::vector &shape, double step) - : Map(shape, {step, step, step}) {} + : Map(shape, {step, step, step}) {} // 调用上面的构造函数 +// 清空地图 void Map::clear() { for (auto &grid : map_) grid.clear(); } +// 获取地图中特定位置的点云数据 TPointCloudPtr &Map::at(int z_idx, int y_idx, int x_idx) { return map_.at(z_idx).at(y_idx).at(x_idx); } +// 获取地图中特定位置的点云数据（常量版本） const TPointCloudPtr &Map::at(int z_idx, int y_idx, int x_idx) const { return map_.at(z_idx).at(y_idx).at(x_idx); } +// 使用 Index 结构体来获取点云数据 TPointCloudPtr &Map::at(const Index &index) { return map_.at(index.k).at(index.j).at(index.i); } +// 使用 Index 结构体来获取点云数据（常量版本） const TPointCloudPtr &Map::at(const Index &index) const { return map_.at(index.k).at(index.j).at(index.i); } +// z 方向移动地图 void Map::ShiftZ(int n) { if (n == 0) return; if (n < 0) { for (int k = shape_[0] - 1; k >= -n; --k) { - std::swap(this->at(k), this->at(k + n)); + std::swap(this->at(k), this->at(k + n)); // 交换位置 } - for (int k = 0; k < -n; ++k) this->at(k).clear(); + for (int k = 0; k < -n; ++k) this->at(k).clear(); // 清空移出区域 } else { for (int k = 0; k < shape_[0] - n; ++k) { - std::swap(this->at(k), this->at(k + n)); + std::swap(this->at(k), this->at(k + n)); // 交换位置 } for (int k = shape_[0] - n; k < shape_[0]; ++k) { - this->at(k).clear(); + this->at(k).clear(); // 清空移出区域 } } - center_[0] -= n; + center_[0] -= n; // 更新中心点 } +// y 方向移动地图 void Map::ShiftY(int n) { if (n == 0) return; for (int k = 0; k < shape_[0]; ++k) { if (n < 0) { for (int j = shape_[1] - 1; j >= -n; --j) { - std::swap(this->at(k, j), this->at(k, j + n)); + std::swap(this->at(k, j), this->at(k, j + n)); // 交换位置 } - for (int j = 0; j < -n; ++j) this->at(k, j).clear(); + for (int j = 0; j < -n; ++j) this->at(k, j).clear(); // 清空移出区域 } else { for (int j = 0; j < shape_[1] - n; ++j) { - std::swap(this->at(k, j), this->at(k, j + n)); + std::swap(this->at(k, j), this->at(k, j + n)); // 交换位置 } for (int j = shape_[1] - n; j < shape_[1]; ++j) { - this->at(k, j).clear(); + this->at(k, j).clear(); // 清空移出区域 } } } - center_[1] -= n; + center_[1] -= n; // 更新中心点 } +// x 方向移动地图 void Map::ShiftX(int n) { if (n == 0) return; for (int k = 0; k < shape_[0]; ++k) { for (int j = 0; j < shape_[1]; ++j) { if (n < 0) { for (int i = shape_[2] - 1; i >= -n; --i) { - std::swap(this->at(k, j, i), this->at(k, j, i + n)); + std::swap(this->at(k, j, i), this->at(k, j, i + n)); // 交换位置 } - for (int i = 0; i < -n; ++i) this->at(k, j, i)->clear(); + for (int i = 0; i < -n; ++i) this->at(k, j, i)->clear(); // 清空移出区域 } else { for (int i = 0; i < shape_[2] - n; ++i) { - std::swap(this->at(k, j, i), this->at(k, j, i + n)); + std::swap(this->at(k, j, i), this->at(k, j, i + n)); // 交换位置 } for (int i = shape_[2] - n; i < shape_[2]; ++i) { - this->at(k, j, i)->clear(); + this->at(k, j, i)->clear(); // 清空移出区域 } } } } - center_[2] -= n; + center_[2] -= n; // 更新中心点 } +// 根据点的位置获取其在地图中的索引 Index Map::GetIndex(const TPoint &point) const { Index index; - index.k = static_cast(point.z / step_[0]) + center_[0]; - index.j = static_cast(point.y / step_[1]) + center_[1]; - index.i = static_cast(point.x / step_[2]) + center_[2]; + index.k = static_cast(point.z / step_[0]) + center_[0]; // 计算 z 方向的索引 + index.j = static_cast(point.y / step_[1]) + center_[1]; // 计算 y 方向的索引 + index.i = static_cast(point.x / step_[2]) + center_[2]; // 计算 x 方向的索引 return index; } +// 检查索引是否有效 bool Map::CheckIndex(const Index &index) const { int k = index.k, j = index.j, i = index.i; return k >= 0 && k < shape_[0] && j >= 0 && j < shape_[1] && i >= 0 && - i < shape_[2]; + i < shape_[2]; // 确保索引在合法范围内 } +// 获取地图中的子地图点云 TPointCloudPtr Map::GetSubmapPoints( const TPoint &point, const std::vector &submap_shapes) const { TPointCloudPtr cloud(new TPointCloud); for (const auto &index : GetSubmapIndices(point, submap_shapes)) { - *cloud += *this->at(index); + *cloud += *this->at(index); // 将子地图的点云加入到 cloud 中 } return cloud; } +// 获取整个地图的所有点云 TPointCloudPtr Map::GetAllPoints() const { TPointCloudPtr cloud_all(new TPointCloud); - for (const auto &grid : map_) *cloud_all += *grid.GetAllPoints(); + for (const auto &grid : map_) *cloud_all += *grid.GetAllPoints(); // 将每个网格的点云加入到 cloud_all 中 return cloud_all; } +// 向地图中添加点云 void Map::AddPoints(const TPointCloudConstPtr &cloud, std::vector *const indices) { std::set index_set; for (const auto &point : *cloud) { Index index = GetIndex(point); if (!CheckIndex(index)) continue; - this->at(index)->push_back(point); - if (indices) index_set.insert(index); + this->at(index)->push_back(point); // 将点云加入到地图中 + if (indices) index_set.insert(index); // 记录索引 } if (indices) { - for (const auto &index : index_set) indices->push_back(index); + for (const auto &index : index_set) indices->push_back(index); // 返回所有有效的索引 } } +// 下采样整个地图 void Map::Downsample(double voxel_size) { auto indices = GetAllIndices(); - Downsample(indices, voxel_size); + Downsample(indices, voxel_size); // 调用重载的 Downsample 函数 } +// 下采样指定的索引 void Map::Downsample(const std::vector &indices, double voxel_size) { for (const auto &index : indices) { if (!CheckIndex(index)) continue; common::VoxelDownSample(this->at(index), this->at(index).get(), - voxel_size); + voxel_size); // 对指定位置进行下采样 } } +// 获取指定 z 层的网格 Grid &Map::at(int z_idx) { return map_.at(z_idx); } +// 获取指定 z 层的网格（常量版本） const Grid &Map::at(int z_idx) const { return map_.at(z_idx); } +// 获取指定 z 层和 y 层的网格 Row &Map::at(int z_idx, int y_idx) { return map_.at(z_idx).at(y_idx); } +// 获取指定 z 层和 y 层的网格（常量版本） const Row &Map::at(int z_idx, int y_idx) const { return map_.at(z_idx).at(y_idx); } +// 获取指定点周围的所有子地图索引 std::vector Map::GetSubmapIndices( const TPoint &point, const std::vector &submap_shapes) const { std::vector indices; @@ -187,13 +209,14 @@ std::vector Map::GetSubmapIndices( for (int i = -nx; i <= nx; ++i) { int idx_i = index.i + i; if (idx_i < 0 || idx_i >= shape_[2]) continue; - indices.emplace_back(idx_k, idx_j, idx_i); + indices.emplace_back(idx_k, idx_j, idx_i); // 将有效的索引加入结果 } } } return indices; } +// 获取整个地图的所有索引 std::vector Map::GetAllIndices() const { std::vector indices; for (int k = 0; k <= shape_[0]; ++k) { @@ -206,45 +229,55 @@ std::vector Map::GetAllIndices() const { return indices; } +// Row 类的构造函数，初始化每一行的点云数据 Row::Row(int n) { row_.resize(n); for (auto &cloud : row_) cloud.reset(new TPointCloud); } +// 获取指定位置的点云 TPointCloudPtr &Row::at(int idx) { return row_.at(idx); } +// 获取指定位置的点云（常量版本） const TPointCloudPtr &Row::at(int idx) const { return row_.at(idx); } +// 清空该行的所有点云 void Row::clear() { for (auto &cloud : row_) cloud->clear(); } +// 获取该行所有点云的集合 TPointCloudPtr Row::GetAllPoints() const { TPointCloudPtr cloud_all(new TPointCloud); for (const auto &cloud : row_) *cloud_all += *cloud; return cloud_all; } +// Grid 类的构造函数，初始化网格 Grid::Grid(int m, int n) { for (int i = 0; i < m; ++i) grid_.emplace_back(n); } +// 清空网格中的所有点云 void Grid::clear() { for (auto &row : grid_) row.clear(); } +// 获取指定位置的行 Row &Grid::at(int idx) { return grid_.at(idx); } +// 获取指定位置的行（常量版本） const Row &Grid::at(int idx) const { return grid_.at(idx); } +// 获取该网格中所有点云的集合 TPointCloudPtr Grid::GetAllPoints() const { TPointCloudPtr cloud_all(new TPointCloud); for (const auto &row : grid_) *cloud_all += *row.GetAllPoints();