From ae0d176d3f63168554dc7c5d5248217bb2e86b19 Mon Sep 17 00:00:00 2001 From: Jonas Frey Date: Sat, 17 Feb 2024 22:31:46 +0100 Subject: [PATCH] confidence and traversability look good --- .../utils/confidence_generator.py | 30 +++++++++---------- .../scripts/wvn_feature_extractor_node.py | 2 +- .../scripts/wvn_learning_node.py | 2 +- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/wild_visual_navigation/utils/confidence_generator.py b/wild_visual_navigation/utils/confidence_generator.py index e0803366..c8914182 100644 --- a/wild_visual_navigation/utils/confidence_generator.py +++ b/wild_visual_navigation/utils/confidence_generator.py @@ -107,14 +107,14 @@ def update_running_mean(self, x: torch.tensor, x_positive: torch.tensor): confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) else: # Then the confidence is computed as the distance to the center of the Gaussian given factor*sigma - confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) - confidence[x < self.mean] = 1.0 + # confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) + # confidence[x < self.mean] = 1.0 - # shifted_mean = self.mean + self.std*self.std_factor - # interval_min = shifted_mean - 2 * self.std - # interval_max = shifted_mean + 2 * self.std - # x = torch.clip( x , interval_min, interval_max) - # confidence = 1 - ((x - interval_min) / (interval_max - interval_min)) + shifted_mean = self.mean + self.std*self.std_factor + interval_min = shifted_mean - 2 * self.std + interval_max = shifted_mean + 2 * self.std + x = torch.clip( x , interval_min, interval_max) + confidence = 1 - ((x - interval_min) / (interval_max - interval_min)) return confidence.type(torch.float32) @@ -192,14 +192,14 @@ def inference_without_update(self, x: torch.tensor): confidence = (x - torch.min(x)) / (torch.max(x) - torch.min(x)) else: - # shifted_mean = self.mean + self.std*self.std_factor - # interval_min = shifted_mean - 2 * self.std - # interval_max = shifted_mean + 2 * self.std - # x = torch.clip( x , interval_min, interval_max) - # confidence = 1 - ((x - interval_min) / (interval_max - interval_min)) - - confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) - confidence[x < self.mean] = 1.0 + shifted_mean = self.mean + self.std*self.std_factor + interval_min = shifted_mean - 2 * self.std + interval_max = shifted_mean + 2 * self.std + x = torch.clip( x , interval_min, interval_max) + confidence = 1 - ((x - interval_min) / (interval_max - interval_min)) + + # confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5) + # confidence[x < self.mean] = 1.0 return confidence.type(torch.float32) 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 f72f4fd5..e7778a24 100644 --- a/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_feature_extractor_node.py @@ -445,7 +445,7 @@ def load_model(self, stamp): reload_rosparams( enabled=rospy.get_param("~reload_default_params", True), node_name=node_name, - camera_cfg="wide_angle_dual_resize", + camera_cfg="wide_angle_dual", ) wvn = WvnFeatureExtractor(node_name) diff --git a/wild_visual_navigation_ros/scripts/wvn_learning_node.py b/wild_visual_navigation_ros/scripts/wvn_learning_node.py index 0cd24cdf..6d458496 100644 --- a/wild_visual_navigation_ros/scripts/wvn_learning_node.py +++ b/wild_visual_navigation_ros/scripts/wvn_learning_node.py @@ -946,7 +946,7 @@ def query_tf(self, parent_frame: str, child_frame: str, stamp: Optional[rospy.Ti reload_rosparams( enabled=rospy.get_param("~reload_default_params", True), node_name=node_name, - camera_cfg="wide_angle_dual_resize", + camera_cfg="wide_angle_dual", ) wvn = WvnLearning(node_name) rospy.spin()