(fix) Fixes init for processing baysian update in local mapper

automatika-robotics · Nov 25, 2024 · e628b08 · e628b08
1 parent 517dca2
commit e628b08
Showing 1 changed file with 8 additions and 3 deletions.
diff --git a/src/kompass_core/mapping/local_mapper.py b/src/kompass_core/mapping/local_mapper.py
@@ -156,16 +156,21 @@ def __init__(self, config: MapConfig, scan_model_config: LaserScanModelConfig):
         self.lower_right_corner_pose = PoseData()
 
         self.scan_update_model = scan_model_config
+
         self.grid_data = GridData(
             width=self.grid_width,
             height=self.grid_height,
             p_prior=self.scan_update_model.p_prior,
         )
 
         # for bayesian update
-        self.previous_grid_prob_transformed = np.copy(
-            self.grid_data.scan_occupancy_prob
-        )
+        if self.config.baysian_update:
+            self.previous_grid_prob_transformed = np.full(
+                (self.grid_data.width, self.grid_data.height),
+                self.grid_data.p_prior,
+                dtype=np.float32,
+                order="F",
+            )
         # turned to true after the first map update is done
         self.processed = False