confidence and traversability look good

leggedrobotics · Feb 17, 2024 · ae0d176 · ae0d176
1 parent af50296
commit ae0d176
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 17 deletions.
diff --git 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
@@ -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
@@ -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()