From 8b087d00f4d58ede2fdd49322df4c3b6a289937f Mon Sep 17 00:00:00 2001 From: TJU_Lu Date: Mon, 7 Jul 2025 14:55:29 +0800 Subject: [PATCH] add wall environment --- Simulator/src/config/config.yaml | 10 ++- Simulator/src/include/maps.hpp | 6 ++ Simulator/src/src/maps.cpp | 94 +++++++++++++++++++++++++++ Simulator/src/src/sensor_simulator.cu | 12 ++-- YOPO/yopo.rviz | 18 ++--- 5 files changed, 123 insertions(+), 17 deletions(-) diff --git a/Simulator/src/config/config.yaml b/Simulator/src/config/config.yaml index cfacd8f..1f146d9 100644 --- a/Simulator/src/config/config.yaml +++ b/Simulator/src/config/config.yaml @@ -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: diff --git a/Simulator/src/include/maps.hpp b/Simulator/src/include/maps.hpp index 893e890..18b60f0 100644 --- a/Simulator/src/include/maps.hpp +++ b/Simulator/src/include/maps.hpp @@ -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 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(); diff --git a/Simulator/src/src/maps.cpp b/Simulator/src/src/maps.cpp index a515afe..0ccc5ce 100644 --- a/Simulator/src/src/maps.cpp +++ b/Simulator/src/src/maps.cpp @@ -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 rand_x(_x_l, _x_h); + std::uniform_real_distribution rand_y(_y_l, _y_h); + std::uniform_real_distribution rand_w(_wall_w_l, _wall_w_h); + std::uniform_real_distribution rand_h(_h_l, _h_h); + std::uniform_real_distribution rand_yaw(-M_PI, M_PI); + std::uniform_real_distribution 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::Ptr ground_cloud = generateGround(info.cloud, _resolution); + *info.cloud += *ground_cloud; + + if (_wall_ceiling){ + pcl::PointCloud::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(); window_size_min = config["window_size_min"].as(); window_size_max = config["window_size_max"].as(); + // wall + _wall_w_l = config["wall_width_min"].as(); + _wall_w_h = config["wall_width_max"].as(); + _wall_thick = config["wall_thick"].as(); + _wall_num = config["wall_number"].as(); + _wall_ceiling = config["wall_ceiling"].as(); } @@ -703,6 +794,9 @@ Maps::generate(int type) case 6: room(); break; + case 7: + wall(); + break; } } diff --git a/Simulator/src/src/sensor_simulator.cu b/Simulator/src/src/sensor_simulator.cu index dbcf33a..e13a14e 100644 --- a/Simulator/src/src/sensor_simulator.cu +++ b/Simulator/src/src/sensor_simulator.cu @@ -3,12 +3,12 @@ namespace raycast { GridMap::GridMap(pcl::PointCloud::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 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]++; diff --git a/YOPO/yopo.rviz b/YOPO/yopo.rviz index 2d56b3c..dd44c99 100644 --- a/YOPO/yopo.rviz +++ b/YOPO/yopo.rviz @@ -1,13 +1,13 @@ Panels: - Class: rviz/Displays - Help Height: 138 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Map1/Autocompute Value Bounds1 - /Trajectory1 Splitter Ratio: 0.6625221967697144 - Tree Height: 476 + Tree Height: 387 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -25,7 +25,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: Depth + SyncSource: Map Preferences: PromptSaveOnExit: true Toolbars: @@ -74,7 +74,7 @@ Visualization Manager: Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: AxisColor - Decay Time: 60 + Decay Time: 10 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -240,10 +240,10 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1600 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000003310000053afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000002d40000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a00440065007000740068010000034e0000025a0000002600ffffff00000001000001b90000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b700000005efc0100000002fb0000000800540069006d0065010000000000000b70000006dc00fffffffb0000000800540069006d00650100000000000004500000000000000000000008330000053a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002700000033afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001c0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680100000203000001740000001600ffffff00000001000001b90000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000005efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c20000033a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -252,6 +252,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 2928 - X: 144 - Y: 54 + Width: 1848 + X: 72 + Y: 27