feat:map添加中文注释
parent
e7913bc1dd
commit
f8e150ef6a
|
|
@ -4,174 +4,196 @@
|
|||
|
||||
namespace oh_my_loam {
|
||||
|
||||
// 构造函数：初始化地图的大小和步长
|
||||
Map::Map(const std::vector<int> &shape, const std::vector<double> &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<int> &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<int>(point.z / step_[0]) + center_[0];
|
||||
index.j = static_cast<int>(point.y / step_[1]) + center_[1];
|
||||
index.i = static_cast<int>(point.x / step_[2]) + center_[2];
|
||||
index.k = static_cast<int>(point.z / step_[0]) + center_[0]; // 计算 z 方向的索引
|
||||
index.j = static_cast<int>(point.y / step_[1]) + center_[1]; // 计算 y 方向的索引
|
||||
index.i = static_cast<int>(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<int> &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<Index> *const indices) {
|
||||
std::set<Index, Index::Comp> 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<Index> &indices, double voxel_size) {
|
||||
for (const auto &index : indices) {
|
||||
if (!CheckIndex(index)) continue;
|
||||
common::VoxelDownSample<TPoint>(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<Index> Map::GetSubmapIndices(
|
||||
const TPoint &point, const std::vector<int> &submap_shapes) const {
|
||||
std::vector<Index> indices;
|
||||
|
|
@ -187,13 +209,14 @@ std::vector<Index> 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<Index> Map::GetAllIndices() const {
|
||||
std::vector<Index> indices;
|
||||
for (int k = 0; k <= shape_[0]; ++k) {
|
||||
|
|
@ -206,45 +229,55 @@ std::vector<Index> 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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue