From 8754b65f9f3e3d28ded80eee7d64144563441b68 Mon Sep 17 00:00:00 2001 From: RobinSchmid7 Date: Mon, 29 Jan 2024 11:23:57 +0100 Subject: [PATCH] Add option for gmm confidence output --- .../config/rviz/anymal_dodo.rviz | 15 +++++------ .../wild_visual_navigation/default.yaml | 1 + .../scripts/wvn_feature_extractor_node.py | 25 ++++++++----------- 3 files changed, 20 insertions(+), 21 deletions(-) diff --git a/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz b/wild_visual_navigation_ros/config/rviz/anymal_dodo.rviz index 8fcf34a9..87875ae1 100644 --- 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 index f236de63..ad7cfef7 100644 --- 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 index 3d639695..ec77eef7 100644 --- 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"]: