Skip to content

Commit

Permalink
Merge pull request #259 from leggedrobotics/dev/fixed_confidence_scaling
Browse files Browse the repository at this point in the history
Fixed confidence & Reverted wrong elevation mapping configuraton

Former-commit-id: f3fef0b
  • Loading branch information
JonasFrey96 authored Apr 29, 2023
2 parents 3c75ea1 + 0e4d5a9 commit 857aee0
Show file tree
Hide file tree
Showing 27 changed files with 18,497 additions and 81 deletions.
Binary file added assets/graph/seg.pt
Binary file not shown.
1 change: 0 additions & 1 deletion assets/graph/seg.pt.REMOVED.git-id

This file was deleted.

Binary file added assets/images/forest_clean.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
1 change: 0 additions & 1 deletion assets/images/forest_clean.png.REMOVED.git-id

This file was deleted.

Binary file added assets/images/forest_grass.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
1 change: 0 additions & 1 deletion assets/images/forest_grass.png.REMOVED.git-id

This file was deleted.

18,401 changes: 18,401 additions & 0 deletions assets/twist_measurements/current_robot_twist_short.csv

Large diffs are not rendered by default.

This file was deleted.

2 changes: 1 addition & 1 deletion cfg/exp/ablation/feature_threshold/slic100_dino224_16.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,4 @@ general:
loss:
anomaly_balanced: False
w_reco: 0.0
confidence_std_factor: 0.5
confidence_std_factor: 1.0
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,4 @@ general:
loss:
anomaly_balanced: False
w_reco: 0.0
confidence_std_factor: 0.5
confidence_std_factor: 1.0
2 changes: 1 addition & 1 deletion cfg/exp/ablation/feature_threshold/slic100_resnet18.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,4 @@ general:
loss:
anomaly_balanced: False
w_reco: 0.0
confidence_std_factor: 0.5
confidence_std_factor: 1.0
2 changes: 1 addition & 1 deletion cfg/exp/ablation/feature_threshold/slic100_resnet50.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,4 @@ general:
loss:
anomaly_balanced: False
w_reco: 0.0
confidence_std_factor: 0.5
confidence_std_factor: 1.0
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,4 @@ general:
loss:
anomaly_balanced: False
w_reco: 0.0
confidence_std_factor: 0.5
confidence_std_factor: 1.0
2 changes: 1 addition & 1 deletion cfg/exp/ablation/feature_threshold/slic100_sift.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -5,4 +5,4 @@ general:
loss:
anomaly_balanced: False
w_reco: 0.0
confidence_std_factor: 0.5
confidence_std_factor: 1.0
2 changes: 1 addition & 1 deletion cfg/exp/ablation/loss_function/anomaly_only.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ loss:
w_temp: 0
w_reco: 0.5
w_trav: 0.0
confidence_std_factor: 0.5
confidence_std_factor: 1.0

general:
use_threshold: False
2 changes: 1 addition & 1 deletion cfg/exp/ablation/loss_function/mse_and_anomaly.yaml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
general:
use_threshold: False
loss:
confidence_std_factor: 0.5
confidence_std_factor: 1.0
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
general:
use_threshold: True
loss:
confidence_std_factor: 0.5
confidence_std_factor: 1.0
2 changes: 1 addition & 1 deletion cfg/exp/ablation/loss_function/mse_only.yaml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
loss:
anomaly_balanced: False
w_reco: 0.0
confidence_std_factor: 0.5
confidence_std_factor: 1.0
general:
use_threshold: False
2 changes: 1 addition & 1 deletion cfg/exp/ablation/scene_adaptation/default.yaml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
general:
use_threshold: True
loss:
confidence_std_factor: 0.5
confidence_std_factor: 1.0
2 changes: 1 addition & 1 deletion wild_visual_navigation/cfg/experiment_params.py
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ class LossParams:
w_reco: float = 0.5
w_temp: float = 0.0 # 0.75
method: str = "latest_measurment"
confidence_std_factor: float = 2.0
confidence_std_factor: float = 1.0
trav_cross_entropy: bool = False

loss: LossParams = LossParams()
Expand Down
38 changes: 29 additions & 9 deletions wild_visual_navigation/feature_extractor/feature_extractor.py
Original file line number Diff line number Diff line change
Expand Up @@ -228,7 +228,7 @@ def compute_torchvision(self, img: torch.tensor, seg: torch.tensor, center: torc
def compute_stego(self, img: torch.tensor, seg: torch.tensor, center: torch.tensor, **kwargs):
return self.extractor.features

def sparsify_features(self, dense_features: torch.tensor, seg: torch.tensor):
def sparsify_features(self, dense_features: torch.tensor, seg: torch.tensor, cumsum_trick=False):
if self._feature_type not in ["histogram"] and self._segmentation_type not in ["none"]:
# Get median features for each cluster

Expand Down Expand Up @@ -281,13 +281,33 @@ def sparsify_features(self, dense_features: torch.tensor, seg: torch.tensor):
return torch.stack(sparse_features, dim=1).T

else:
# Single scale feature extraction
sparse_features = []
for i in range(seg.max() + 1):
m = seg == i
x, y = torch.where(m)
feat = dense_features[0, :, x, y].mean(dim=1)
sparse_features.append(feat)
return torch.stack(sparse_features, dim=1).T
if cumsum_trick:
# Cumsum is slightly slower for 100 segments
# Trick: sort the featuers according to the segments and then use cumsum for summing
dense_features = dense_features[0].permute(1, 2, 0).reshape(-1, dense_features.shape[1])
seg = seg.reshape(-1)
sorts = seg.argsort()
dense_features_sort, seg_sort = dense_features[sorts], seg[sorts]
x = dense_features_sort
# The cumsum operation is the only one that takes times
x = x.cumsum(0)
kept = torch.ones(x.shape[0], device=x.device, dtype=torch.bool)
elements_sumed = torch.arange(x.shape[0], device=x.device, dtype=torch.int)
kept[:-1] = seg_sort[1:] != seg_sort[:-1]
x = x[kept]
x = torch.cat((x[:1], x[1:] - x[:-1]))

elements_sumed = elements_sumed[kept]
elements_sumed[1:] = elements_sumed[1:] - elements_sumed[:-1]
x /= elements_sumed[:, None]
return x
else:
sparse_features = []
for i in range(seg.max() + 1):
m = seg == i
x, y = torch.where(m)
feat = dense_features[0, :, x, y].mean(dim=1)
sparse_features.append(feat)
return torch.stack(sparse_features, dim=1).T
else:
return dense_features
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,7 @@ def reset(self):
# self._model.train()

# # Re-create optimizer
# self._optimizer = torch.optim.AdamW(self._model.parameters(), lr=self._exp_cfg["optimizer"]["lr"])
# self._optimizer = torch.optim.Adam(self._model.parameters(), lr=self._exp_cfg["optimizer"]["lr"])

# # Re-create traversability loss
# self._traversability_loss = TraversabilityLoss(
Expand Down
6 changes: 3 additions & 3 deletions wild_visual_navigation/utils/confidence_generator.py
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ def update_running_mean(self, x: torch.tensor, x_positive: torch.tensor):
self.std[0] = torch.sqrt(self.var)

# Then the confidence is computed as the distance to the center of the Gaussian given factor*sigma
confidence = torch.exp(-(((x - self.mean) / (2 * self.std * self.std_factor)) ** 2))
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)
Expand All @@ -98,7 +98,7 @@ def update_kalman_filter(self, x: torch.tensor, x_positive: torch.tensor):
self.std[0] = torch.sqrt(self.var)[0, 0]

# Then the confidence is computed as the distance to the center of the Gaussian given factor*sigma
confidence = torch.exp(-(((x - self.mean) / (2 * self.std * self.std_factor)) ** 2))
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)
Expand Down Expand Up @@ -131,7 +131,7 @@ def inference_without_update(self, x: torch.tensor):
if x.device != self.mean.device:
return torch.zeros_like(x)

confidence = torch.exp(-(((x - self.mean) / (2 * self.std * self.std_factor)) ** 2))
confidence = torch.exp(-(((x - self.mean) / (self.std * self.std_factor)) ** 2) * 0.5)
return confidence.type(torch.float32)

def forward(self, x: torch.tensor):
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#### Basic parameters ########
resolution: 0.1 # resolution in m.
map_length: 25 # map's size in m.
resolution: 0.04 # resolution in m.
map_length: 8 # map's size in m.
sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
mahalanobis_thresh: 2.0 # points outside this distance is outlier.
outlier_variance: 0.01 # if point is outlier, add this value to the cell.
Expand All @@ -19,7 +19,7 @@ orientation_noise_thresh: 0.01 # if the orientation change is b
position_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
orientation_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
min_valid_distance: 0.5 # points with shorter distance will be filtered out.
max_height_range: 2.0 # points higher than this value from sensor will be filtered out to disable ceiling.
max_height_range: 1.0 # points higher than this value from sensor will be filtered out to disable ceiling.
ramped_height_range_a: 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
ramped_height_range_b: 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
ramped_height_range_c: 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
Expand All @@ -40,9 +40,9 @@ max_unsafe_n: 10 # if the number of cells under s
overlap_clear_range_xy: 4.0 # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting)
overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting)

map_frame: 'map' # The map frame where the odometry source uses.
map_frame: 'odom' # The map frame where the odometry source uses.
base_frame: 'base' # The robot's base frame. This frame will be a center of the map.
corrected_map_frame: 'map_new_corrected'
corrected_map_frame: 'odom_corrected'

#### Feature toggles ########
enable_edge_sharpen: true
Expand Down Expand Up @@ -75,7 +75,7 @@ publishers:
fps: 5.0

semantic_map_raw:
layers: ['elevation', 'traversability', 'visual_traversability', 'elevation_with_semantic', 'visual_traversability_filtered', 'rgb_image']
layers: ['elevation', 'traversability', 'visual_traversability', 'elevation_with_semantic']
basic_layers: ['elevation', 'traversability']
fps: 5.0
# elevation_map_recordable:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ smooth_filter:
# Apply inpainting using opencv
inpainting:
enable: True
fill_nan: True
fill_nan: False
is_height_layer: True
layer_name: "inpaint"
extra_params:
Expand All @@ -35,26 +35,4 @@ elevation_semantic:
is_height_layer: True
layer_name: "elevation_with_semantic"
extra_params:
method: "telea" # telea or ns
# Apply smoothing for inpainted layer

# Apply inpainting using opencv
visual_traversability_filtered:
enable: True
fill_nan: False
is_height_layer: False
layer_name: "visual_traversability_filtered"
extra_params:
method: "telea" # telea or ns
# Apply smoothing for inpainted layer


# Apply smoothing.
# smooth_filter_visual_traversability:
# name: smooth_filter
# enable: True
# fill_nan: False
# is_height_layer: False
# layer_name: "visual_traversability_smooth"
# extra_params:
# input_layer_name: "visual_traversability"
method: "telea"
Original file line number Diff line number Diff line change
Expand Up @@ -80,19 +80,7 @@ subscribers:
# channels: ["visual_confidence"]
# data_type: image

# Depth cameras
front_bpearl:
channels: []
fusion: []
topic_name: /robot_self_filter/bpearl_front/point_cloud
data_type: pointcloud

rear_bpearl:
channels: []
fusion: []
topic_name: /robot_self_filter/bpearl_rear/point_cloud
data_type: pointcloud

front_depth:
channels: []
fusion: []
Expand All @@ -117,21 +105,37 @@ subscribers:
topic_name: /depth_camera_right/point_cloud_self_filtered
data_type: pointcloud

velodyne:

# velodyne:
# channels: []
# fusion: []
# topic_name: /point_cloud_filter/lidar/point_cloud_filtered
# data_type: pointcloud


# Cerberus
front_bpearl:
channels: []
fusion: []
topic_name: /point_cloud_filter/lidar/point_cloud_filtered
topic_name: /robot_self_filter/bpearl_front/point_cloud
data_type: pointcloud

# Cameras for D
front_depth_d:
rear_bpearl:
channels: []
fusion: []
topic_name: /depth_camera_front_upper/point_cloud_self_filtered
topic_name: /robot_self_filter/bpearl_rear/point_cloud
data_type: pointcloud


rear_depth_d:
channels: []
fusion: []
topic_name: /depth_camera_rear_upper/point_cloud_self_filtered
data_type: pointcloud
# Cameras for D
# front_depth_d:
# channels: []
# fusion: []
# topic_name: /depth_camera_front_upper/point_cloud_self_filtered
# data_type: pointcloud

# rear_depth_d:
# channels: []
# fusion: []
# topic_name: /depth_camera_rear_upper/point_cloud_self_filtered
# data_type: pointcloud
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
camera_topics:
front:
image_topic: "/alphasense_driver_ros/cam4/debayered/compressed"
info_topic: "/alphasense_driver_ros/cam4/camera_info"
use_for_training: true
# left:
# image_topic: "/alphasense_driver_ros/cam3/debayered"
# info_topic: "/alphasense_driver_ros/cam3/camera_info"
# use_for_training: false
# right:
# image_topic: "/alphasense_driver_ros/cam5/debayered"
# info_topic: "/alphasense_driver_ros/cam5/camera_info"
# use_for_training: false

# Provides 1080 (height) x 1920 (width) images
network_input_image_height: 224 # 448
network_input_image_width: 224 # 448

0 comments on commit 857aee0

Please sign in to comment.