diff --git a/config/localmapping/triage_voxel.yaml b/config/localmapping/triage_voxel.yaml new file mode 100644 index 0000000..0b69673 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/config/localmapping/voxel.yaml b/config/localmapping/voxel.yaml index 877ed8a..6bb7f3c 100644 --- a/config/localmapping/voxel.yaml +++ b/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 diff --git a/physics_atv_visual_mapping/feature_key_list.py b/physics_atv_visual_mapping/feature_key_list.py index b52ca84..3f06024 100644 --- a/physics_atv_visual_mapping/feature_key_list.py +++ b/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 diff --git a/physics_atv_visual_mapping/localmapping/voxel/raytracing.py b/physics_atv_visual_mapping/localmapping/voxel/raytracing.py index bedc0b4..9c599b8 100644 --- a/physics_atv_visual_mapping/localmapping/voxel/raytracing.py +++ b/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"]) diff --git a/physics_atv_visual_mapping/localmapping/voxel/voxel_localmapper.py b/physics_atv_visual_mapping/localmapping/voxel/voxel_localmapper.py index f5927a7..c5fa667 100644 --- a/physics_atv_visual_mapping/localmapping/voxel/voxel_localmapper.py +++ b/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,7 +100,7 @@ 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: @@ -103,7 +108,9 @@ def add_feature_pc(self, pos: torch.Tensor, feat_pc: FeaturePointCloudTorch, do_ 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)