Add option for gmm confidence output

leggedrobotics · Jan 29, 2024 · 8754b65 · 8754b65
1 parent 77da852
commit 8754b65
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 21 deletions.
diff --git a/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz b/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz
@@ -10,11 +10,12 @@ Panels:
         - /Wild Visual Navigation1/Self Supervision1
         - /Wild Visual Navigation1/Depth Sensors1/realsense-front1
         - /Wild Visual Navigation1/Prediction1
+        - /Wild Visual Navigation1/Prediction1/Traversability Raw1
         - /Wild Visual Navigation1/Prediction1/Traversability Overlay1
         - /Elevation Map WIFI1/Local Planner (SDF)1
         - /Elevation Map RAW1
       Splitter Ratio: 0.5789473652839661
-    Tree Height: 298
+    Tree Height: 756
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -843,7 +844,7 @@ Visualization Manager:
               Value: true
             - Class: rviz/Image
               Enabled: true
-              Image Topic: /anomaly_overlay/traversability
+              Image Topic: /anomaly_overlay2/image
               Max Value: 1
               Median window: 5
               Min Value: 0
@@ -1453,7 +1454,7 @@ Window Geometry:
     collapsed: false
   F Traversability Overlay Replay:
     collapsed: false
-  Height: 918
+  Height: 1376
   Hide Left Dock: false
   Hide Right Dock: true
   Input Image:
@@ -1468,7 +1469,7 @@ Window Geometry:
     collapsed: false
   Placeholder:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   R Confidence Raw:
     collapsed: false
   R Input Image:
@@ -1495,6 +1496,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2470
-  X: 81
-  Y: 56
+  Width: 2488
+  X: 72
+  Y: 27
diff --git a/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml b/wild_visual_navigation_ros/config/wild_visual_navigation/default.yaml
@@ -33,6 +33,7 @@ scale_traversability_max_fpr: 0.25
 min_samples_for_training: 5
 prediction_per_pixel: True
 traversability_threshold: 0.55
+gmm_confidence: True
 clip_to_binary: False
 
 # Supervision Generator

diff --git a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py
@@ -139,6 +139,7 @@ def read_params(self):
 
         self.confidence_std_factor = rospy.get_param("~confidence_std_factor")
         self.scale_traversability = rospy.get_param("~scale_traversability")
+        self.gmm_confidence = rospy.get_param("~gmm_confidence")
         self.scale_traversability_max_fpr = rospy.get_param("~scale_traversability_max_fpr")
         self.status_thread_rate = rospy.get_param("~status_thread_rate")
         self.prediction_per_pixel = rospy.get_param("~prediction_per_pixel")
@@ -304,8 +305,7 @@ def image_callback(self, image_msg: Image, cam: str):  #  info_msg: CameraInfo
                 traversability = traversability.clip(0, 1)
                 out_trav = traversability
 
-                # # TODO: make this optional
-                if False:
+                if self.gmm_confidence:
                     loss_reco = F.mse_loss(prediction[:, 1:], data.x, reduction="none").mean(dim=1)
 
                     loss_reco_flat = loss_reco.flatten().detach().cpu().numpy().reshape(-1, 1)
@@ -327,9 +327,6 @@ def image_callback(self, image_msg: Image, cam: str):  #  info_msg: CameraInfo
 
                     conf_mask_np = conf_mask_np.astype(np.uint8) * 255
 
-                    # Save the confident mask as an image
-                    # cv2.imwrite("/home/rschmid/conf_mask.jpg", conf_mask_np.astype(np.uint8) * 255)
-
                     # Publish the confident mask
                     conf_mask_msg = rc.numpy_to_ros_image(conf_mask_np, "passthrough")
                     conf_mask_msg.header = image_msg.header
@@ -370,15 +367,15 @@ def image_callback(self, image_msg: Image, cam: str):  #  info_msg: CameraInfo
             self.camera_handler[cam]["input_pub"].publish(msg)
 
         # Publish confidence
-        # if self.camera_topics[cam]["publish_confidence"]:
-        #     loss_reco = F.mse_loss(prediction[:, 1:], data.x, reduction="none").mean(dim=1)
-        #     confidence = self.confidence_generator.inference_without_update(x=loss_reco)
-        #     out_confidence = confidence.reshape(H, W)
-        #     msg = rc.numpy_to_ros_image(out_confidence.cpu().numpy(), "passthrough")
-        #     msg.header = image_msg.header
-        #     msg.width = out_confidence.shape[0]
-        #     msg.height = out_confidence.shape[1]
-        #     self.camera_handler[cam]["conf_pub"].publish(msg)
+        if self.camera_topics[cam]["publish_confidence"] and not self.gmm_confidence:
+            loss_reco = F.mse_loss(prediction[:, 1:], data.x, reduction="none").mean(dim=1)
+            confidence = self.confidence_generator.inference_without_update(x=loss_reco)
+            out_confidence = confidence.reshape(H, W)
+            msg = rc.numpy_to_ros_image(out_confidence.cpu().numpy(), "passthrough")
+            msg.header = image_msg.header
+            msg.width = out_confidence.shape[0]
+            msg.height = out_confidence.shape[1]
+            self.camera_handler[cam]["conf_pub"].publish(msg)
 
         # Publish features and feature_segments
         if self.camera_topics[cam]["use_for_training"]: