Error Propagation and Configurable Sensor Noise Models

elevation_mapping_cupy/config/core/core_param.yaml

@@ -1,7 +1,6 @@
 #### Basic parameters ########
 resolution: 0.04                                # resolution in m.
 map_length: 8.0                                 # map's size in m.
-sensor_noise_factor: 0.05                       # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
 mahalanobis_thresh: 2.0                         # points outside this distance is outlier.
 outlier_variance: 0.01                          # if point is outlier, add this value to the cell.
 drift_compensation_variance_inler: 0.05         # cells under this value is used for drift compensation.

elevation_mapping_cupy/config/core/example_setup.yaml

@@ -22,11 +22,31 @@ image_channel_fusions:
 #    camera_info_topic_name: '/camera/depth/camera_info'
 #    channel_info_topic_name: '/camera/channel_info'
 
+additional_layers: []
 
 subscribers:
   front_cam:
     topic_name: '/camera/depth/points'
     data_type: pointcloud
+    noise_model_name: LiDAR
+    SLS_old_noise_model_params:
+      sensor_noise_factor: 0.05                       # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
+    constant_noise_model_params:
+      constant_variance: 0.01
+    SLS_noise_model_params:
+      a: 6.8e-3
+      b: 28.0e-3
+      c: 38.0e-3
+      # d: (42e-3+2.0e-3)/2.0
+      d: 22.0e-3
+      Sigma_Theta_BS_diag: [1.0e-1, 1.0e-1, 1.0e-1]
+      Sigma_b_r_BS_diag: [1.0e-3, 1.0e-3, 1.0e-3]
+    LiDAR_noise_model_params:
+      a: 0.012
+      b: 6.8
+      c: 0.81
+      Sigma_Theta_BS_diag: [1.0e-1, 1.0e-1, 1.0e-1]
+      Sigma_b_r_BS_diag: [1.0e-3, 1.0e-3, 1.0e-3]
   color_cam: # for color camera
     topic_name: '/camera/rgb/image_raw'
     camera_info_topic_name: '/camera/depth/camera_info'

elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py

@@ -28,6 +28,7 @@
 from elevation_mapping_cupy.kernels import polygon_mask_kernel
 from elevation_mapping_cupy.kernels import image_to_map_correspondence_kernel
 
+from elevation_mapping_cupy.sensor_processor import SensorProcessor, make_3x1vec
 from elevation_mapping_cupy.map_initializer import MapInitializer
 from elevation_mapping_cupy.plugins.plugin_manager import PluginManager
 from elevation_mapping_cupy.semantic_map import SemanticMap
@@ -58,7 +59,7 @@ def __init__(self, param: Parameter):
         self.param = param
         self.data_type = self.param.data_type
         self.resolution = param.resolution
-        self.center = xp.array([0, 0, 0], dtype=self.data_type)
+        self.center = xp.array([[0],[0],[0]], dtype=self.data_type)
         self.base_rotation = xp.eye(3, dtype=self.data_type)
         self.map_length = param.map_length
         self.cell_n = param.cell_n
@@ -84,6 +85,9 @@ def __init__(self, param: Parameter):
         self.elevation_map[1] += self.initial_variance
         self.elevation_map[3] += 1.0
 
+        # Configure sensors
+        self.configure_sensors(param)
+
         # overlap clearance
         cell_range = int(self.param.overlap_clear_range_xy / self.resolution)
         cell_range = np.clip(cell_range, 0, self.cell_n)
@@ -115,6 +119,22 @@ def __init__(self, param: Parameter):
         self.plugin_manager.load_plugin_settings(plugin_config_file)
 
         self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points")
+
+    def configure_sensors(self, param):
+        """Configure the sensor Processors for the elevation map.
+
+        Args:
+            param (elevation_mapping_cupy.parameter.Parameter):
+        """
+        self.sensor_processors = {}
+        for sensor_ID, config in param.subscriber_cfg.items():
+            if config["data_type"] == "pointcloud":
+                nm_name = config["noise_model_name"]
+                nm_param_name = nm_name + "_noise_model_params"
+                nm_params = config.get(nm_param_name, {})
+                sp = SensorProcessor(sensor_ID, nm_name, nm_params, xp)
+                self.sensor_processors[sensor_ID] = sp
+
 
     def clear(self):
         """Reset all the layers of the elevation & the semantic map."""
@@ -238,7 +258,6 @@ def compile_kernels(self):
             self.resolution,
             self.cell_n,
             self.cell_n,
-            self.param.sensor_noise_factor,
             self.param.mahalanobis_thresh,
             self.param.outlier_variance,
             self.param.wall_num_thresh,
@@ -257,7 +276,6 @@ def compile_kernels(self):
             self.resolution,
             self.cell_n,
             self.cell_n,
-            self.param.sensor_noise_factor,
             self.param.mahalanobis_thresh,
             self.param.drift_compensation_variance_inlier,
             self.param.traversability_inlier,
@@ -303,39 +321,57 @@ def shift_translation_to_map_center(self, t):
 
         Args:
             t (cupy._core.core.ndarray): Absolute point position
+
+        Returns:
+            cupy._core.core.ndarray: Translated point position
         """
-        t -= self.center
+        t_shift = t -  self.center
+        return t_shift
 
-    def update_map_with_kernel(self, points_all, channels, R, t, position_noise, orientation_noise):
+    def update_map_with_kernel(self, sensor_ID, points_all, channels, C_MB, B_r_MB, Sigma_b_r_MB, Sigma_Theta_MB):
         """Update map with new measurement.
 
         Args:
+            sensor_ID (str):                            Sensor ID
             points_all (cupy._core.core.ndarray):
-            channels (List[str]):
-            R (cupy._core.core.ndarray):
-            t (cupy._core.core.ndarray):
-            position_noise (float):
-            orientation_noise (float):
+            channels (List[str]):                       List of channels in the point cloud besides x, y, z
+            C_MB (cupy._core.core.ndarray):             Rotation matrix from the map frame to the base frame
+            B_r_MB (cupy._core.core.ndarray):           Translation vector from the map frame to the base frame
+            Sigma_b_r_MB (cupy._core.core.ndarray):     Covariance of base position in map frame.
+            Sigma_Theta_MB (cupy._core.core.ndarray):   Covariance of base orientation in map frame where orientation is defined 
+                                                        as fixed-axis(extrinsic) xyz(roll, pitch, yaw) Euler angles.
         """
         self.new_map *= 0.0
         error = cp.array([0.0], dtype=cp.float32)
         error_cnt = cp.array([0], dtype=cp.float32)
         points = points_all[:, :3]
+        # Obtain the transform from the map frame to the sensor frame
+        C_BS = self.sensor_processors[sensor_ID].C_BS
+        B_r_BS = self.sensor_processors[sensor_ID].B_r_BS
+        C_MS = C_MB @ C_BS
+        B_r_MS = C_MB @ B_r_BS + B_r_MB
         # additional_fusion = self.get_fusion_of_pcl(channels)
         with self.map_lock:
-            self.shift_translation_to_map_center(t)
+            # Now the translation is w.r.t. the map center frame O
+            O_r_OS = self.shift_translation_to_map_center(B_r_MS)
+            # C_MS is equivalent to C_OS because we assume the map center frame O is aligned with the map frame M
+            C_OS = C_MS
             self.error_counting_kernel(
                 self.elevation_map,
                 points,
                 cp.array([0.0], dtype=self.data_type),
                 cp.array([0.0], dtype=self.data_type),
-                R,
-                t,
+                C_OS,
+                O_r_OS,
                 self.new_map,
                 error,
                 error_cnt,
                 size=(points.shape[0]),
             )
+            # Compute position and orientation noise of the base frame in the map frame for
+            # triggering drift compensation
+            position_noise = cp.sqrt(Sigma_b_r_MB[0, 0] + Sigma_b_r_MB[1, 1] + Sigma_b_r_MB[2, 2])
+            orientation_noise = cp.sqrt(Sigma_Theta_MB[0, 0] + Sigma_Theta_MB[1, 1] + Sigma_Theta_MB[2, 2])
             if (
                 self.param.enable_drift_compensation
                 and error_cnt > self.param.min_height_drift_cnt
@@ -348,11 +384,14 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
                 self.additive_mean_error += self.mean_error
                 if np.abs(self.mean_error) < self.param.max_drift:
                     self.elevation_map[0] += self.mean_error * self.param.drift_compensation_alpha
+            # Compute the vertical variance for the sensor using uncertainty propagation for the sensor
+            var_h = self.sensor_processors[sensor_ID].get_z_variance(points, C_MB, B_r_MB, Sigma_Theta_MB, Sigma_b_r_MB)
             self.add_points_kernel(
                 cp.array([0.0], dtype=self.data_type),
                 cp.array([0.0], dtype=self.data_type),
-                R,
-                t,
+                C_OS,
+                O_r_OS,
+                var_h,
                 self.normal_map,
                 points,
                 self.elevation_map,
@@ -361,10 +400,10 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
             )
             self.average_map_kernel(self.new_map, self.elevation_map, size=(self.cell_n * self.cell_n))
 
-            self.semantic_map.update_layers_pointcloud(points_all, channels, R, t, self.new_map)
+            self.semantic_map.update_layers_pointcloud(points_all, channels, C_OS, O_r_OS, self.new_map)
 
             if self.param.enable_overlap_clearance:
-                self.clear_overlap_map(t)
+                self.clear_overlap_map(O_r_OS)
             # dilation before traversability_filter
             self.traversability_input *= 0.0
             self.dilation_filter_kernel(
@@ -426,36 +465,40 @@ def update_upper_bound_with_valid_elevation(self):
 
     def input_pointcloud(
         self,
+        sensor_ID: str,
         raw_points: cp._core.core.ndarray,
         channels: List[str],
-        R: cp._core.core.ndarray,
-        t: cp._core.core.ndarray,
-        position_noise: float,
-        orientation_noise: float,
+        C_MB: cp._core.core.ndarray,
+        B_r_MB: cp._core.core.ndarray,
+        Sigma_b_r_MB: cp._core.core.ndarray,
+        Sigma_Theta_MB: cp._core.core.ndarray,
     ):
         """Input the point cloud and fuse the new measurements to update the elevation map.
 
         Args:
-            raw_points (cupy._core.core.ndarray):
-            channels (List[str]):
-            R  (cupy._core.core.ndarray):
-            t (cupy._core.core.ndarray):
-            position_noise (float):
-            orientation_noise (float):
-
+            sensor_ID (str):                            Sensor ID
+            raw_points (cupy._core.core.ndarray):       Points in the sensor frame (S_r_SP)
+            channels (List[str]):                       List of channels in the point cloud including x, y, z as
+                                                        the first three channels
+            C_MB (cupy._core.core.ndarray):             Rotation matrix from the map frame to the base frame
+            B_r_MB (cupy._core.core.ndarray):           Translation vector from the map frame to the base frame
+            Sigma_b_r_MB (cupy._core.core.ndarray):     Covariance of base position in map frame.
+            Sigma_Theta_MB (cupy._core.core.ndarray):   Covariance of base orientation in map frame where orientation is defined 
+                                                        as fixed-axis(extrinsic) xyz(roll, pitch, yaw) Euler angles.
         Returns:
             None:
         """
         raw_points = cp.asarray(raw_points, dtype=self.data_type)
         additional_channels = channels[3:]
         raw_points = raw_points[~cp.isnan(raw_points).any(axis=1)]
         self.update_map_with_kernel(
+            sensor_ID,
             raw_points,
             additional_channels,
-            cp.asarray(R, dtype=self.data_type),
-            cp.asarray(t, dtype=self.data_type),
-            position_noise,
-            orientation_noise,
+            cp.asarray(C_MB, dtype=self.data_type),
+            make_3x1vec(cp.asarray(B_r_MB, dtype=self.data_type)),
+            cp.asarray(Sigma_b_r_MB, dtype=self.data_type),
+            cp.asarray(Sigma_Theta_MB, dtype=self.data_type),
         )
 
     def input_image(
@@ -876,16 +919,38 @@ def initialize_map(self, points, method="cubic"):
     #  Test script for profiling.
     #  $ python -m cProfile -o profile.stats elevation_mapping.py
     #  $ snakeviz profile.stats
+
+    # Get the directory of the script
+    # Which should be located at: ws_dir/elevation_mapping_cupy/elevation_mapping_cupy/script/elevation_mapping.py
+    script_dir = os.path.dirname(os.path.realpath(__file__))
+    # Back up two directories to find the base directory of the package
+    # located here: ws_dir/elevation_mapping_cupy/
+    base_dir = os.path.dirname(os.path.dirname(script_dir))
+    # Navigate down to the core config directory
+    # located here: ws_dir/elevation_mapping_cupy/elevation_mapping_cupy/config/core/plugin_config.yaml
+    core_dir = os.path.join(base_dir, 'config', 'core')
+
     xp.random.seed(123)
-    R = xp.random.rand(3, 3)
-    t = xp.random.rand(3)
-    print(R, t)
+    C_MB = xp.eye(3,3)
+    b_r_MB = xp.zeros(3)
+    b_r_MB[2] += 1.0
+    print(C_MB, b_r_MB)
+
+    Sigma_b_r_MB = xp.eye(3,3)*0.1
+    Sigma_Theta_MB = xp.eye(3,3)*0.1
     param = Parameter(
-        use_chainer=False, weight_file="../config/weights.dat", plugin_config_file="../config/plugin_config.yaml",
+        use_chainer=False, weight_file=os.path.join(core_dir, "weights.dat"), plugin_config_file=os.path.join(core_dir, "plugin_config.yaml"),
     )
-    param.additional_layers = ["rgb", "grass", "tree", "people"]
-    param.fusion_algorithms = ["color", "class_bayesian", "class_bayesian", "class_bayesian"]
+    param.load_from_yaml(os.path.join(core_dir, "core_param.yaml"))
+    param.load_from_yaml(os.path.join(core_dir, "example_setup.yaml"))
+    # Override weights file location and plugin config file location that are set in the yaml file
+    # Not removing there for ROS compatability
+    param.weight_file = os.path.join(core_dir, "weights.dat")
+    param.plugin_config_file = os.path.join(core_dir, "plugin_config.yaml")
     param.update()
+    param.additional_layers = []
+    # Should be front_cam
+    sensor_ID = list(param.subscriber_cfg.keys())[0]
     elevation = ElevationMap(param)
     layers = [
         "elevation",
@@ -896,16 +961,23 @@ def initialize_map(self, points, method="cubic"):
         "inpaint",
         "rgb",
     ]
-    points = xp.random.rand(100000, len(layers))
+    # Points within a 1m.x1m.x1m. cube
+    points = xp.random.rand(10000, 3 + len(param.additional_layers))
+    # Set starting elevation to -1.0 meter
+    points[:, 2] = -1.0
 
     channels = ["x", "y", "z"] + param.additional_layers
     print(channels)
     data = np.zeros((elevation.cell_n - 2, elevation.cell_n - 2), dtype=np.float32)
     for i in range(50):
-        elevation.input_pointcloud(points, channels, R, t, 0, 0)
-        elevation.update_normal(elevation.elevation_map[0])
-        pos = np.array([i * 0.01, i * 0.02, i * 0.01])
-        elevation.move_to(pos, R)
+        # Move points along xy direction and up to test mapping
+        points[:, 0] += param.resolution*2
+        points[:, 1] += param.resolution*2
+        points[:, 2] += 0.1
+        elevation.input_pointcloud(sensor_ID, points, channels, C_MB, b_r_MB, Sigma_b_r_MB, Sigma_Theta_MB)
+        # elevation.update_normal(elevation.elevation_map[0])
+        # pos = np.array([i * 0.01, i * 0.02, i * 0.01])
+        # elevation.move_to(pos, R)
         for layer in layers:
             elevation.get_map_with_name_ref(layer, data)
         print(i)