add wall environment

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:

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<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();

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<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;
   }
 }
 

Simulator/src/src/sensor_simulator.cu

@@ -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 length = max_pt(0) - min_pt(0) + 2 * epsilon;  // 保证各个边界最大值能被取到
-        float width  = max_pt(1) - min_pt(1);   // Y方向的宽度
+        float width  = max_pt(1) - min_pt(1) + 2 * epsilon;
-        float height = max_pt(2) - min_pt(2);   // Z方向的高度
+        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]++;

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
+  Width: 1848
-  X: 144
+  X: 72
-  Y: 54
+  Y: 27