hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add wall environment

Browse Source
This commit is contained in:
TJU_Lu
2025-07-07 14:55:29 +08:00
parent b8725720da
commit 8b087d00f4
5 changed files with 123 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Simulator/src/config/config.yaml
View File

@@ -19,12 +19,12 @@ expand_x_times: 0 # x方向复制次数（cuda版不使用）
expand_y_times: 0 # y方向复制次数（cuda版不使用）
occupy_threshold: 0 # 大于多少个点才认为占据（cpu版不使用）
# 2.2 随机地图
# 2.2 随机地图 ======
seed: 3
x_length: 60 # 真正随机范围(XY更大范围处为其镜像)
y_length: 60
z_length: 15
maze_type: 5 # 1: 溶洞 2: 柱子 3:迷宫 5:森林 6:房间 (当前策略支持1,2,5)
maze_type: 5 # 1: 溶洞 2: 柱子 3:迷宫 5:森林 6:房间 7:墙面 (当前策略支持1,2,5,7)
# 溶洞
complexity: 0.02 # 复杂度 0.0-0.5
fill: 0.1 # 填充率 0.0-0.4
@@ -47,6 +47,12 @@ max_windows: 2 # 每面墙最多窗口数
window_size_min: 2.0
window_size_max: 2.8
add_ceiling: 0 # 是否添加天花板
# 墙面
wall_width_min: 0.5
wall_width_max: 8.0
wall_thick: 0.5
wall_number: 100 # 墙面数量
wall_ceiling: 1 # 是否添加天花板
# 3. 传感器参数 ==============================================================
camera:

6
Simulator/src/include/maps.hpp
View File

@@ -56,6 +56,11 @@ private:
int max_windows;
int add_ceiling;
double window_size_min, window_size_max;
// wall
double _wall_w_l, _wall_w_h;
double _wall_thick;
int _wall_num;
int _wall_ceiling;
std::uniform_real_distribution<double> dis_window_x, dis_window_z, dis_window_size;
std::default_random_engine window_eng;
@@ -64,6 +69,7 @@ private:
void maze2D();
void randomMapGenerate();
void Maze3DGen();
void wall();
void recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi &maze);
void recursizeDivisionMaze(Eigen::MatrixXi &maze);
void optimizeMap();

94
Simulator/src/src/maps.cpp
View File

@@ -74,6 +74,91 @@ Maps::randomMapGenerate()
info.cloud->is_dense = true;
}
void
Maps::wall()
{
std::default_random_engine eng(info.seed);
double _resolution = 1 / info.scale;
double _x_l = -info.sizeX / (2 * info.scale);
double _x_h = info.sizeX / (2 * info.scale);
double _y_l = -info.sizeY / (2 * info.scale);
double _y_h = info.sizeY / (2 * info.scale);
double _h_l = 0.3 * info.sizeZ / info.scale;
double _h_h = info.sizeZ / info.scale;
std::uniform_real_distribution<double> rand_x(_x_l, _x_h);
std::uniform_real_distribution<double> rand_y(_y_l, _y_h);
std::uniform_real_distribution<double> rand_w(_wall_w_l, _wall_w_h);
std::uniform_real_distribution<double> rand_h(_h_l, _h_h);
std::uniform_real_distribution<float> rand_yaw(-M_PI, M_PI);
std::uniform_real_distribution<float> rand_pitch(-M_PI / 10, M_PI / 10);
pcl::PointXYZ pt_random;
for (int i = 0; i < _wall_num; ++i)
{
float cx = rand_x(eng);
float cy = rand_y(eng);
float cz = 0.0f;
float yaw = rand_yaw(eng);
float pitch = rand_pitch(eng);
float width = rand_w(eng);
float height = rand_h(eng);
float thick = _wall_thick;
int w_steps = std::ceil(width / _resolution);
int h_steps = std::ceil(height / _resolution);
int t_steps = std::ceil(thick / _resolution);
float w0 = -width / 2.0f;
float h0 = 0.0f;
float t0 = -thick / 2.0f;
float cosy = std::cos(yaw), siny = std::sin(yaw);
float cosp = std::cos(pitch), sinp = std::sin(pitch);
for (int i = 0; i < w_steps; ++i)
{
for (int j = 0; j < h_steps; ++j)
{
for (int k = 0; k < t_steps; ++k)
{
float x_local = w0 + i * _resolution;
float y_local = t0 + k * _resolution;
float z_local = h0 + j * _resolution;
float x1 = cosy * x_local - siny * y_local;
float y1 = siny * x_local + cosy * y_local;
float z1 = z_local;
float y2 = cosp * y1 - sinp * z1;
float z2 = sinp * y1 + cosp * z1;
float x2 = x1;
pcl::PointXYZ pt;
pt.x = cx + x2;
pt.y = cy + y2;
pt.z = cz + z2;
info.cloud->points.push_back(pt);
}
}
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud = generateGround(info.cloud, _resolution);
*info.cloud += *ground_cloud;
if (_wall_ceiling){
pcl::PointCloud<pcl::PointXYZ>::Ptr ceiling_cloud = generateGround(info.cloud, _resolution, _h_h);
*info.cloud += *ceiling_cloud;
}
info.cloud->width = info.cloud->points.size();
info.cloud->height = 1;
info.cloud->is_dense = true;
}
void
Maps::perlin3D()
{
@@ -674,6 +759,12 @@ Maps::setParam(const YAML::Node& config)
add_ceiling = config["add_ceiling"].as<int>();
window_size_min = config["window_size_min"].as<double>();
window_size_max = config["window_size_max"].as<double>();
// wall
_wall_w_l = config["wall_width_min"].as<double>();
_wall_w_h = config["wall_width_max"].as<double>();
_wall_thick = config["wall_thick"].as<double>();
_wall_num = config["wall_number"].as<int>();
_wall_ceiling = config["wall_ceiling"].as<int>();
}
@@ -703,6 +794,9 @@ Maps::generate(int type)
case 6:
room();
break;
case 7:
wall();
break;
}
}

12
Simulator/src/src/sensor_simulator.cu
View File

@@ -3,12 +3,12 @@
namespace raycast
{
GridMap::GridMap(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float resolution, int occupy_threshold = 1){
const float epsilon = 0.001f; // 避免数值误差导致 (1)建图空行 (2)边缘点被忽略
Eigen::Vector4f min_pt, max_pt;
pcl::getMinMax3D(*cloud, min_pt, max_pt);
float length = max_pt(0) - min_pt(0); // X方向的长度
float width = max_pt(1) - min_pt(1); // Y方向的宽度
float height = max_pt(2) - min_pt(2); // Z方向的高度
float length = max_pt(0) - min_pt(0) + 2 * epsilon; // 保证各个边界最大值能被取到
float width = max_pt(1) - min_pt(1) + 2 * epsilon;
float height = max_pt(2) - min_pt(2) + 2 * epsilon;
Vector3f origin(min_pt(0), min_pt(1), min_pt(2));
Vector3f map_size(length, width, height);
origin_x_ = origin.x;
@@ -30,9 +30,9 @@ namespace raycast
raycast_step_ = resolution;
std::vector<int> h_map(grid_total_size, 0);
// 有时候会有全空的行，加个很小的偏移
// 点云全位于体素边界，有时候会有全空的行，加个很小的偏移
for (size_t i = 0; i < cloud->points.size(); i++) {
Vector3f point(cloud->points[i].x + 0.001, cloud->points[i].y + 0.001, cloud->points[i].z + 0.001);
Vector3f point(cloud->points[i].x + epsilon, cloud->points[i].y + epsilon, cloud->points[i].z + epsilon);
int idx = Vox2Idx(Pos2Vox(point));
if (idx < grid_total_size) {
h_map[idx]++;