added simulated noise to lidar data

ctu-mrs · Jun 21, 2024 · 3f92fec · 3f92fec
1 parent 73158bd
commit 3f92fec
Show file tree

Hide file tree

Showing 4 changed files with 75 additions and 44 deletions.
diff --git a/cfg/unreal_simulator.cfg b/cfg/unreal_simulator.cfg
@@ -18,6 +18,7 @@ lidar = gen.add_group("LiDAR");
 
 lidar.add("lidar_enabled", bool_t, 0, "LiDAR enabled", True)
 lidar.add("lidar_rate", double_t, 0, "LiDAR rate", 1.0, 1.0, 200.0)
+lidar.add("lidar_noise_enabled", bool_t, 0, "LiDAR noise enabled", True)
 lidar.add("lidar_std_at_1m", double_t, 0, "LiDAR STD at 1m", 0.0, 0.01, 1.0)
 lidar.add("lidar_std_slope", double_t, 0, "LiDAR STD slope", 0.0, 0.01, 1.0)
 

diff --git a/config/unreal_simulator.yaml b/config/unreal_simulator.yaml
@@ -40,7 +40,7 @@ sensors:
       enabled: true
 
       std_at_1m: 0.01 # [m]
-      std_slope: 0.2 [-] # multiplies std_at_1m for each meter of measured distances
+      std_slope: 0.2 # [-] # multiplies std_at_1m for each meter of measured distances
 
     show_beams: false # TODO broken
 

diff --git a/src/unreal_simulator.cpp b/src/unreal_simulator.cpp
@@ -42,6 +42,8 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/common/transforms.h>
 
+#include <random>
+
 //}
 
 /* defines //{ */
@@ -240,6 +242,8 @@ class UnrealSimulator : public nodelet::Nodelet {
   std::vector<double> last_stereo_ue_stamp_;
 
   double uedsToWallTime(const double ueds_time);
+
+  std::default_random_engine rng;
 };
 
 //}
@@ -289,6 +293,9 @@ void UnrealSimulator::onInit() {
   param_loader.loadParam("sensors/lidar/rate", drs_params_.lidar_rate);
   param_loader.loadParam("sensors/lidar/lidar_segmented/enabled", drs_params_.lidar_seg_enabled);
   param_loader.loadParam("sensors/lidar/lidar_segmented/rate", drs_params_.lidar_seg_rate);
+  param_loader.loadParam("sensors/lidar/noise/enabled", drs_params_.lidar_noise_enabled);
+  param_loader.loadParam("sensors/lidar/noise/std_at_1m", drs_params_.lidar_std_at_1m);
+  param_loader.loadParam("sensors/lidar/noise/std_slope", drs_params_.lidar_std_slope);
 
   param_loader.loadParam("sensors/lidar/horizontal_fov", lidar_horizontal_fov_);
   param_loader.loadParam("sensors/lidar/vertical_fov", lidar_vertical_fov_);
@@ -854,7 +861,18 @@ void UnrealSimulator::timerLidar([[maybe_unused]] const ros::TimerEvent& event)
 
       tf::Vector3 dir = tf::Vector3(ray.directionX, ray.directionY, ray.directionZ);
 
-      dir = dir.normalized() * (ray.distance / 100.0);
+      double ray_distance = ray.distance / 100.0;
+
+      if (drs_params.lidar_noise_enabled && ray_distance > 0) {
+
+        const double std = ray_distance * drs_params.lidar_std_slope * drs_params.lidar_std_at_1m;
+
+        std::normal_distribution<double> distribution(0, std);
+
+        ray_distance += distribution(rng);
+      }
+
+      dir = dir.normalized() * ray_distance;
 
       *iterX         = dir.x();
       *iterY         = -dir.y();  // convert left-hand to right-hand coordinates
@@ -913,8 +931,21 @@ void UnrealSimulator::timerSegLidar([[maybe_unused]] const ros::TimerEvent& even
     for (const ueds_connector::LidarSegData& ray : lidarSegData) {
 
       pcl::PointXYZRGB point;
-      tf::Vector3      dir = tf::Vector3(ray.directionX, ray.directionY, ray.directionZ);
-      dir                  = dir.normalized() * (ray.distance / 100.0);
+
+      tf::Vector3 dir = tf::Vector3(ray.directionX, ray.directionY, ray.directionZ);
+
+      double ray_distance = ray.distance / 100.0;
+
+      if (drs_params.lidar_noise_enabled && ray_distance > 0) {
+
+        const double std = ray_distance * drs_params.lidar_std_slope * drs_params.lidar_std_at_1m;
+
+        std::normal_distribution<double> distribution(0, std);
+
+        ray_distance += distribution(rng);
+      }
+
+      dir = dir.normalized() * ray_distance;
 
       point.x = dir.x();
       point.y = -dir.y();  // convert left-hand to right-hand coordinates

diff --git a/tmux/one_drone/rviz.rviz b/tmux/one_drone/rviz.rviz
@@ -6,9 +6,8 @@ Panels:
       Expanded:
         - /TF1/Frames1
         - /TF1/Tree1
-        - /stereo_cloud1
       Splitter Ratio: 0.6970587968826294
-    Tree Height: 145
+    Tree Height: 280
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -502,80 +501,64 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: -0.7296195030212402
-        Min Value: -3.15236234664917
+        Max Value: 10
+        Min Value: -10
         Value: true
       Axis: Z
       Channel Name: intensity
       Class: rviz/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: AxisColor
+      Color Transformer: RGB8
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
-      Name: lidar
+      Name: stereo_cloud
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.10000000149011612
       Style: Flat Squares
-      Topic: /uav1/lidar/points
+      Topic: /uav1/stereo/points2
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 12.273218154907227
-        Min Value: -3.3037233352661133
+        Max Value: 1.359739065170288
+        Min Value: -1.1742526292800903
         Value: true
       Axis: Z
       Channel Name: intensity
       Class: rviz/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: RGB8
+      Color Transformer: AxisColor
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
-      Name: lidar_segmented
+      Name: lidar
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.10000000149011612
       Style: Flat Squares
-      Topic: /uav1/lidar_segmented/points
+      Topic: /uav1/lidar/points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Class: rviz/MarkerArray
-      Enabled: false
-      Marker Topic: /uav1/octomap_global_vis/occupied_cells_vis_array_throttled
-      Name: global_map
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: false
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /uav1/octomap_local_vis/occupied_cells_vis_array
-      Name: local_map
-      Namespaces:
-        map: true
-      Queue Size: 100
-      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
+        Max Value: 12.273218154907227
+        Min Value: -3.3037233352661133
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -587,18 +570,34 @@ Visualization Manager:
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
-      Name: stereo_cloud
+      Name: lidar_segmented
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.10000000149011612
       Style: Flat Squares
-      Topic: /uav1/stereo/points2
+      Topic: /uav1/lidar_segmented/points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz/MarkerArray
+      Enabled: false
+      Marker Topic: /uav1/octomap_global_vis/occupied_cells_vis_array_throttled
+      Name: global_map
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /uav1/octomap_local_vis/occupied_cells_vis_array
+      Name: local_map
+      Namespaces:
+        map: true
+      Queue Size: 100
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255
@@ -640,7 +639,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 23.506206512451172
+      Distance: 18.203205108642578
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -656,17 +655,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.28539878129959106
+      Pitch: 0.3053988218307495
       Target Frame: uav1/fcu
-      Yaw: 2.7804408073425293
+      Yaw: 2.53544020652771
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 940
+  Height: 1495
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000002bb00000356fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000cc000000c700fffffffb0000001400760069006f005f00630061006d0065007200610000000122000000bd0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000006007200670062010000010d000000e70000001600fffffffb0000001400760069006f005f00630061006d00650072006100000001400000009f0000000000000000fb0000001a007200670062005f007300650067006d0065006e0074006500640000000292000000f70000001600fffffffb0000001600730074006500720065006f005f006c00650066007401000001fa000000ca0000001600fffffffb0000001800730074006500720065006f005f0072006900670068007401000002ca000000c70000001600fffffffb0000000a0049006d00610067006501000004d5000000db00000000000000000000000100000116000001a4fc0200000004fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000003b000000e40000005c00fffffffb0000000a005600690065007700730000000125000000ba000000a000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000016d0000016e0000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000038f0000003efc0100000002fb0000000800540069006d006500000000000000038f0000035c00fffffffb0000000800540069006d006501000000000000045000000000000000000000028f0000035600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002bb00000581fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000153000000c700fffffffb0000001400760069006f005f00630061006d0065007200610000000122000000bd0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000060072006700620100000194000001810000001600fffffffb0000001400760069006f005f00630061006d00650072006100000001400000009f0000000000000000fb0000001a007200670062005f007300650067006d0065006e0074006500640000000292000000f70000001600fffffffb0000001600730074006500720065006f005f006c006500660074010000031b000001500000001600fffffffb0000001800730074006500720065006f005f0072006900670068007401000004710000014b0000001600fffffffb0000000a0049006d00610067006501000004d5000000db00000000000000000000000100000116000001a4fc0200000004fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000003b000000e40000005c00fffffffb0000000a005600690065007700730000000125000000ba000000a000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000016d0000016e0000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000038f0000003efc0100000002fb0000000800540069006d006500000000000000038f0000035c00fffffffb0000000800540069006d00650100000000000004500000000000000000000007330000058100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -675,9 +674,9 @@ Window Geometry:
     collapsed: true
   Views:
     collapsed: true
-  Width: 1360
-  X: 4739
-  Y: 351
+  Width: 2548
+  X: -32
+  Y: -32
   rgb:
     collapsed: false
   rgb_segmented: