Indoor Quadruped Traversability

config/localmapping/triage_voxel.yaml

@@ -0,0 +1,33 @@
+mapper_type: voxel
+ema: 0.5 #higher->use recent more
+n_features: 3
+
+# Dynamic obstacle clearing parameters
+passthrough_thresh: 0.4  # Lower = faster clearing (0.5 means 50% miss rate triggers cull)
+hit_miss_decay: 0.85     # Decay factor per frame (lower = faster clearing, 1.0 = no decay)
+range_buffer: 0.1        # Buffer distance (m) for conservative clearing
+max_clear_range: 25.0    # When a lidar bin has NO measurement, assume the ray traveled this far
+max_hit_confidence: 15.0 # Maximum hit confidence (caps accumulated hits to avoid infinite mass)
+min_misses: 3.0          # Minimum number of misses required before a voxel can be culled
+
+metadata:
+    origin: [-15., -15., -3.]
+    length: [30., 30., 6.]
+    resolution: [0.1, 0.1, 0.05]
+
+raytracer:
+    type: frustum
+    sensor:
+        type: OS1-128
+
+        # type: VLP32C
+
+        # type: generic
+
+        # n_el: 72 #number of bins
+        # el_range: [-25., 15.] #min/max angle
+        # el_thresh: default #optional arg to filter based on remainder
+
+        # n_az: 900
+        # az_range: [-180, 180.]
+        # az_thresh: default

config/localmapping/voxel.yaml

@@ -2,9 +2,9 @@ mapper_type: voxel
 ema: 0.5 #higher->use recent more
 n_features: -1
 metadata:
-    origin: [-50., -50., -10.]
-    length: [100., 100., 20.]
-    resolution: [0.4, 0.4, 0.1]
+    origin: [-20., -20., -5.]
+    length: [40., 40., 10.]
+    resolution: [0.1, 0.1, 0.05]
 
 raytracer:
     type: frustum

physics_atv_visual_mapping/feature_key_list.py

@@ -45,6 +45,9 @@ def __repr__(self):
         """
         Make a simpler print that condenses similar keys
         """
+        if not self.label:
+            return "empty"
+
         out = ""
         prev_prefix = None
         prev_metainfo = None

physics_atv_visual_mapping/localmapping/voxel/raytracing.py

@@ -2,9 +2,7 @@
 import torch
 import torch_scatter
 
-from numpy import pi as PI
-
-from physics_atv_visual_mapping.utils import transform_points, DEG_2_RAD, RAD_2_DEG
+from physics_atv_visual_mapping.utils import transform_points, DEG_2_RAD
 
 class FrustumRaytracer:
     """
@@ -23,46 +21,73 @@ def __init__(self, config, device='cpu'):
         self.device = device
 
     # def raytrace(self, pose, voxel_grid_meas, voxel_grid_agg):
-    def raytrace_but_better(self, pose, pc_meas, voxel_grid_agg):
+    def raytrace_but_better(self, pose, pc_meas, voxel_grid_agg, range_buffer=0.15, max_clear_range=25.0):
         """
         Actual raytracing interface
+
+        For each ray direction (el/az bin), we find the max measured range.
+        Then for each aggregated voxel, if its range is LESS than the measured range
+        (plus a buffer for tolerance), it means the ray passed through that voxel,
+        indicating the voxel should be cleared (dynamic obstacle that moved).
+
+        Args:
+            pose: Robot pose [x, y, z, qx, qy, qz, qw]
+            pc_meas: Measured point cloud (FeaturePointCloudTorch)
+            voxel_grid_agg: Aggregated voxel grid to update
+            range_buffer: Buffer distance (m) subtracted from measured range for safer clearing
+            max_clear_range: Maximum range (m) to use for clearing when no measurement in bin.
 
         TODO: we're raycasting in global but the sensor is in local. Rotate the bins into local using pose
         """
-        # voxel_pts = voxel_grid_meas.grid_indices_to_pts(voxel_grid_meas.raster_indices_to_grid_indices(voxel_grid_meas.raster_indices))
-
-        # import pdb;pdb.set_trace()
+        # We only want to record measurements where we actually hit something.
         voxel_pts = pc_meas.pts
         voxel_el_az_range = get_el_az_range_from_xyz(pose, voxel_pts)
+
+        # This remains strict. If a hit is OOB, we ignore it.
         voxel_maxdist_el_az_bins = bin_el_az_range(voxel_el_az_range, sensor_model=self.sensor_model, reduce='max')
 
-        voxel_from_el_az = get_xyz_from_el_az_range(pose, voxel_el_az_range)
+        # For bins with no measurement, assume the ray traveled to max_clear_range
+        voxel_maxdist_el_az_bins[voxel_maxdist_el_az_bins < 1e-6] = max_clear_range
 
+
+        # We check existing voxels against the measurement bins.
         agg_voxel_pts = voxel_grid_agg.grid_indices_to_pts(voxel_grid_agg.raster_indices_to_grid_indices(voxel_grid_agg.raster_indices))
         agg_voxel_el_az_range = get_el_az_range_from_xyz(pose, agg_voxel_pts)
-        agg_voxel_bin_idxs = get_el_az_range_bin_idxs(agg_voxel_el_az_range, sensor_model=self.sensor_model)
-
-        #bin idx == -1 iff. outside sensor fov
-        agg_voxel_valid_bin = (agg_voxel_bin_idxs >= 0)
-
-        #set to large negative to not filter on misses
-        # voxel_maxdist_el_az_bins[voxel_maxdist_el_az_bins < 1e-6] = -1e10
-
-        #set to lidar range to filter on misses
-        voxel_maxdist_el_az_bins[voxel_maxdist_el_az_bins < 1e-6] = 200.
-
-        el_az = torch.stack(torch.meshgrid(self.sensor_model["el_bins"][:-1], self.sensor_model["az_bins"][:-1], indexing='ij'), dim=-1)
-        voxel_maxdist_sph = torch.cat([el_az.view(-1, 2), voxel_maxdist_el_az_bins.view(-1, 1)], dim=-1)
-        voxel_maxdist_sph_hits = voxel_maxdist_sph[voxel_maxdist_sph[:, 2] > 1e-6]
-        voxel_maxdist_xyz_hits = get_xyz_from_el_az_range(pose, voxel_maxdist_sph_hits)
+
+        # Extract sensor constants
+        sensor = self.sensor_model
+        n_az = sensor["az_bins"].shape[0] - 1
+        n_el = sensor["el_bins"].shape[0] - 1
+
+        # Calculate raw indices for the AGGREGATED voxels
+        el_idxs = torch.bucketize(agg_voxel_el_az_range[:, 0], sensor["el_bins"]) - 1
+        az_idxs = torch.bucketize(agg_voxel_el_az_range[:, 1], sensor["az_bins"]) - 1
+
+        # Clamp elevation to valid range [0, n_el-1]
+        # Any voxel above the FOV gets mapped to the top-most beam (n_el - 1)
+        # Any voxel below the FOV gets mapped to the bottom-most beam (0)
+        el_idxs_clamped = el_idxs.clamp(min=0, max=n_el - 1)
+
+        # Compute raster indices using the clamped elevation
+        agg_voxel_bin_idxs = el_idxs_clamped * n_az + az_idxs
 
-        voxel_maxdist_sph_misses = voxel_maxdist_sph[voxel_maxdist_sph[:, 2] < 1e-6]
-        voxel_maxdist_sph_misses[:, 2] = 50.
-        voxel_maxdist_xyz_misses = get_xyz_from_el_az_range(pose, voxel_maxdist_sph_misses)
+        # Check for Azimuth validity only (Azimuth should wrap, but check OOB just in case)
+        az_oob = (az_idxs < 0) | (az_idxs >= n_az)
+
+        # We consider the bin valid if Azimuth is good. 
+        # We intentionally ignore Elevation OOB because we clamped it.
+        agg_voxel_valid_bin = (~az_oob)
 
         agg_ranges = agg_voxel_el_az_range[:, 2]
-        query_ranges = voxel_maxdist_el_az_bins[agg_voxel_bin_idxs]
-        passthrough_mask = (query_ranges > agg_ranges) & agg_voxel_valid_bin
+
+        # clamp indices to avoid negative indexing (which would access the last element)
+        agg_bin_idxs_clamped = agg_voxel_bin_idxs.clamp(min=0)
+        query_ranges = voxel_maxdist_el_az_bins[agg_bin_idxs_clamped]
+
+        # A voxel is "passed through" if the ray traveled beyond it (measured range > voxel range)
+        # Subtract range_buffer from measured range to be more conservative about clearing
+        # This prevents clearing voxels right at the edge of a surface
+        passthrough_mask = ((query_ranges - range_buffer) > agg_ranges) & agg_voxel_valid_bin
 
         #dont increment hits, do that in the aggregate step
         voxel_grid_agg.misses += passthrough_mask.float()
@@ -88,7 +113,7 @@ def raytrace_but_better(self, pose, pc_meas, voxel_grid_agg):
         return
 
     def to(self, device):
-        self.device = self.device
+        self.device = device
         for k,v in self.sensor_model.items():
             self.sensor_model[k] = v.to(device)
         return self
@@ -142,6 +167,7 @@ def setup_sensor_model(sensor_config, device='cpu'):
             "az_bins": az_bins,
             "az_thresh": az_thresh,
         }
+
     elif sensor_config["type"] == "VLP32C-front":
         #from the spec sheet @ 600RPM (https://icave2.cse.buffalo.edu/resources/sensor-modeling/VLP32CManual.pdf)
         az_bins = DEG_2_RAD * torch.linspace(-90., 90., 451, dtype=torch.float, device=device)
@@ -166,6 +192,23 @@ def setup_sensor_model(sensor_config, device='cpu'):
             "az_bins": az_bins,
             "az_thresh": az_thresh,
         }
+
+    elif sensor_config["type"] == "OS1-128":
+        # OS1-128 has 1024 horizontal channels with 360° total horizontal FOV
+        az_bins = DEG_2_RAD * torch.linspace(-180., 180., 1025, dtype=torch.float, device=device)
+        az_thresh = (az_bins[1:] - az_bins[:-1]).min()
+
+        # OS1-128 has 128 vertical channels with 45° total vertical FOV (-22.5° to +22.5°)
+        el_bins = DEG_2_RAD * torch.linspace(-22.5, 22.5, 129, dtype=torch.float, device=device)
+        el_thresh = (el_bins[1:] - el_bins[:-1]).min()
+
+        return {
+            "el_bins": el_bins,
+            "el_thresh": el_thresh,
+            "az_bins": az_bins, 
+            "az_thresh": az_thresh,
+        }
+
     else:
         print("unsupported sensor model type {}".format(sensor_config["type"]))
         exit(1)
@@ -236,7 +279,8 @@ def get_el_az_range_from_xyz(pose, pts, apply_rotation=True):
     if apply_rotation:
         R = htm_from_quat(pose[3:7])
         #pts are in global, so apply inverse of R to transform to local
-        pts_to_pos_dx = transform_points(pts_to_pos_dx, torch.linalg.inv(R))
+        # R is a rotation matrix in 4x4 form (orthogonal), so inverse is transpose
+        pts_to_pos_dx = transform_points(pts_to_pos_dx, R.transpose(-1, -2))
 
     ranges = torch.linalg.norm(pts_to_pos_dx, dim=-1)
     ranges_2d = torch.linalg.norm(pts_to_pos_dx[..., :2], dim=-1)
@@ -268,10 +312,12 @@ def bin_el_az_range(el_az_range, sensor_model, reduce='max'):
     n_el = sensor_model["el_bins"].shape[0] - 1
     raster_idxs = get_el_az_range_bin_idxs(el_az_range, sensor_model)
 
-    valid_mask = (raster_idxs > 0)
+    valid_mask = (raster_idxs >= 0)
 
-    #placeholder of zero should be ok?
-    out = torch_scatter.scatter(src=el_az_range[..., 2][valid_mask], index=raster_idxs[valid_mask], dim_size=n_el*n_az, reduce=reduce)
+    # Initialize with -1.0 to represent "no measurement"
+    out = torch.full((n_el*n_az,), -1.0, device=el_az_range.device, dtype=el_az_range.dtype)
+
+    torch_scatter.scatter(src=el_az_range[..., 2][valid_mask], index=raster_idxs[valid_mask], out=out, dim_size=n_el*n_az, reduce=reduce)
 
     return out
 
@@ -290,8 +336,14 @@ def get_el_az_range_bin_idxs(el_az_range, sensor_model):
     raster_idxs = el_idxs * n_az + az_idxs
 
     #compute and evaluate residual, as elems can fall outside bin edges
-    el_res = (el_az_range[:, 0] - sensor_model["el_bins"][el_idxs])
-    az_res = (el_az_range[:, 1] - sensor_model["az_bins"][az_idxs])
+    # Handle out-of-bounds indices safely
+    in_range = (el_idxs >= 0) & (el_idxs < n_el) & (az_idxs >= 0) & (az_idxs < n_az)
+
+    el_res = torch.zeros_like(el_az_range[:, 0])
+    az_res = torch.zeros_like(el_az_range[:, 1])
+
+    el_res[in_range] = (el_az_range[in_range, 0] - sensor_model["el_bins"][el_idxs[in_range]])
+    az_res[in_range] = (el_az_range[in_range, 1] - sensor_model["az_bins"][az_idxs[in_range]])
 
     el_invalid = (el_res < 0.) | (el_res > sensor_model["el_thresh"])
     az_invalid = (az_res < 0.) | (az_res > sensor_model["az_thresh"])

physics_atv_visual_mapping/localmapping/voxel/voxel_localmapper.py

@@ -1,6 +1,5 @@
 import numpy as np
 import matplotlib.pyplot as plt
-import matplotlib.cm as cm
 import open3d as o3d
 import torch
 import torch_scatter
@@ -12,8 +11,6 @@
 from physics_atv_visual_mapping.utils import *
 
 def hist(var):
-    import matplotlib.pyplot as plt
-    import numpy as np
 
     var_np = var.cpu().numpy()
     ncols = var_np.shape[1] if var_np.ndim > 1 else 1
@@ -63,7 +60,9 @@ def create_grid(size=10, step=1.0):
 class VoxelLocalMapper(LocalMapper):
     """Class for local mapping voxels"""
 
-    def __init__(self, metadata, feature_keys, ema, raytracer=None, n_features=-1, device='cpu'):
+    def __init__(self, metadata, feature_keys, ema, raytracer=None, n_features=-1, device='cpu',
+                 passthrough_thresh=0.4, hit_miss_decay=0.85, range_buffer=0.1,
+                 max_clear_range=25.0, max_hit_confidence=15.0, min_misses=3.0):
         super().__init__(metadata, device)
         assert metadata.ndims == 3, "VoxelLocalMapper requires 3d metadata"
         self.n_features = len(feature_keys) if n_features == -1 else n_features
@@ -76,6 +75,12 @@ def __init__(self, metadata, feature_keys, ema, raytracer=None, n_features=-1, d
         self.do_raytrace = self.raytracer is not None
         self.ema = ema
         self.assert_feat_match = True
+        self.passthrough_thresh = passthrough_thresh
+        self.hit_miss_decay = hit_miss_decay
+        self.range_buffer = range_buffer
+        self.max_clear_range = max_clear_range
+        self.max_hit_confidence = max_hit_confidence
+        self.min_misses = min_misses
 
     def clear(self):
         self.voxel_grid = VoxelGrid(self.metadata, self.feature_keys, self.device)
@@ -95,15 +100,17 @@ def update_pose(self, pose: torch.Tensor):
         self.voxel_grid.shift(px_shift)
         self.metadata.origin = new_origin
 
-    def add_feature_pc(self, pos: torch.Tensor, feat_pc: FeaturePointCloudTorch, do_raytrace=False, debug=False):
+    def add_feature_pc(self, pos: torch.Tensor, feat_pc: FeaturePointCloudTorch, debug=False):
         voxel_grid_new = VoxelGrid.from_feature_pc(feat_pc, self.metadata, self.n_features, pos, strategy='mindist')
 
         if self.assert_feat_match:
             assert self.voxel_grid.feature_keys == voxel_grid_new.feature_keys, f"voxel feat key mismatch: mapper has {self.voxel_grid.feature_keys}, added pc has {voxel_grid_new.feature_keys}"
 
         if self.do_raytrace:
             # self.raytracer.raytrace(pos, voxel_grid_meas=voxel_grid_new, voxel_grid_agg=self.voxel_grid)
-            self.raytracer.raytrace_but_better(pos, pc_meas=feat_pc, voxel_grid_agg=self.voxel_grid)
+            self.raytracer.raytrace_but_better(pos, pc_meas=feat_pc, voxel_grid_agg=self.voxel_grid, 
+                                                range_buffer=self.range_buffer,
+                                                max_clear_range=self.max_clear_range)
 
         #first map all indices with features
         all_raster_idxs = torch.cat([self.voxel_grid.raster_indices, voxel_grid_new.raster_indices])
@@ -174,6 +181,13 @@ def add_feature_pc(self, pos: torch.Tensor, feat_pc: FeaturePointCloudTorch, do_
         self.voxel_grid.hits = hit_buf
         self.voxel_grid.misses = miss_buf
 
+        # Prevents "Infinite Mass" problem by capping the number of hits
+        self.voxel_grid.hits = torch.clamp(self.voxel_grid.hits, max=self.max_hit_confidence)
+
+        # Decaying misses only allows the map to "forgive" transient noise over time
+        if self.hit_miss_decay < 1.0:
+            self.voxel_grid.misses *= self.hit_miss_decay
+
         min_coords_buf = 1e10 * torch.ones(unique_raster_idxs.shape[0], 3, device=self.voxel_grid.device)
         min_coords_buf[vg1_inv_idxs] = torch.minimum(min_coords_buf[vg1_inv_idxs], self.voxel_grid.min_coords)
         min_coords_buf[vg2_inv_idxs] = torch.minimum(min_coords_buf[vg2_inv_idxs], voxel_grid_new.min_coords)
@@ -185,9 +199,13 @@ def add_feature_pc(self, pos: torch.Tensor, feat_pc: FeaturePointCloudTorch, do_
         self.voxel_grid.max_coords = max_coords_buf
 
         #compute passthrough rate
-        passthrough_rate = self.voxel_grid.misses / (self.voxel_grid.hits + self.voxel_grid.misses)
+        passthrough_rate = self.voxel_grid.misses / (self.voxel_grid.hits + self.voxel_grid.misses + 1e-8)
 
-        cull_mask = passthrough_rate > 0.75
+        # In order to remove a voxel:
+        # 1. Ratio must be bad (passthrough > threshold)
+        # 2. AND we must have seen enough misses to be sure it's gone (> min_misses)
+        # The min_misses check prevents a single lucky ray from deleting a voxel
+        cull_mask = (passthrough_rate > self.passthrough_thresh) & (self.voxel_grid.misses > self.min_misses)
 
         # print('culling {} voxels...'.format(cull_mask.sum()))
 
@@ -201,7 +219,6 @@ def add_feature_pc(self, pos: torch.Tensor, feat_pc: FeaturePointCloudTorch, do_
         cull_mask = cull_mask & ~bottom_voxel_mask
 
         if debug:
-            import open3d as o3d
             pts = self.voxel_grid.grid_indices_to_pts(self.voxel_grid.raster_indices_to_grid_indices(self.voxel_grid.raster_indices))
             #solid=black, porous=green, cull=red
             colors = torch.stack([torch.zeros_like(passthrough_rate), passthrough_rate, torch.zeros_like(passthrough_rate)], dim=-1)