171 lines
6.8 KiB
Python
171 lines
6.8 KiB
Python
"""Keep CMU height hazards, except an observed supported grade.
|
|
|
|
Height above a cell's low quantile is not step height. A smooth 15-degree ramp
|
|
can exceed 10 cm across that cell. A 6 cm voxel mesh also quantizes a continuous
|
|
grade. Admit that surface only with broad support, a bounded plane residual and
|
|
no observed short-range height jump exceeding the qualified 10 cm step. Vertical
|
|
surfaces, excessive roughness and sparse/unknown support retain the CMU cost.
|
|
"""
|
|
|
|
import ctypes
|
|
import hashlib
|
|
import math
|
|
from collections import OrderedDict
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
_NATIVE_PATH = Path("/opt/missioncore/libterrain_connectivity.so")
|
|
_NATIVE = ctypes.CDLL(str(_NATIVE_PATH)) if _NATIVE_PATH.is_file() else None
|
|
if _NATIVE is not None:
|
|
for suffix, dtype in (("f32", ctypes.c_float), ("f64", ctypes.c_double)):
|
|
function = getattr(_NATIVE, "terrain_connected_" + suffix)
|
|
function.argtypes = [ctypes.POINTER(dtype), ctypes.c_int]
|
|
function.restype = ctypes.c_int
|
|
|
|
|
|
def connected_grade(nearby):
|
|
if _NATIVE is not None and nearby.dtype in (np.dtype("float32"), np.dtype("float64")):
|
|
points = np.ascontiguousarray(nearby[:, :3])
|
|
dtype, suffix = (
|
|
(ctypes.c_float, "f32") if points.dtype.itemsize == 4 else (ctypes.c_double, "f64")
|
|
)
|
|
return bool(
|
|
getattr(_NATIVE, "terrain_connected_" + suffix)(
|
|
points.ctypes.data_as(ctypes.POINTER(dtype)), len(points)
|
|
)
|
|
)
|
|
# Reference implementation retained for portable CPU tests and comparison.
|
|
dx = nearby[:, None, 0] - nearby[None, :, 0]
|
|
dy = nearby[:, None, 1] - nearby[None, :, 1]
|
|
separation = np.sqrt(dx * dx + dy * dy)
|
|
jump = np.abs(nearby[:, None, 2] - nearby[None, :, 2])
|
|
if np.any((separation <= 0.12) & (jump > 0.1001)):
|
|
return False
|
|
connected = separation <= 0.12
|
|
reached = connected[np.argmin(np.linalg.norm(nearby[:, :2], axis=1))].copy()
|
|
while True:
|
|
expanded = np.any(connected[reached], axis=0)
|
|
if np.array_equal(expanded, reached):
|
|
return bool(reached.all())
|
|
reached = expanded
|
|
|
|
|
|
def underbody_support_costs(terrain, pose, contact_height_m=0.37):
|
|
"""Reconcile low returns already inside the current chassis footprint.
|
|
|
|
CMU's neighbourhood ground reference can label the supported terrain under
|
|
the chassis as a body collision. Use measured pose and the declared contact
|
|
height only inside the body (with a 5 cm inset), never for terrain ahead.
|
|
Retain drops, high returns and excessive tilt. The 8 cm band has 2 cm reserve
|
|
below the physically qualified 10 cm step; raw terrain memory is unchanged.
|
|
"""
|
|
result = terrain.copy()
|
|
x, y, z, w = pose[3:]
|
|
up = np.array([2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)])
|
|
if up[2] < math.cos(math.radians(25)):
|
|
return result, 0
|
|
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
|
|
axes = np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]])
|
|
local = (terrain[:, :2] - pose[:2]) @ axes
|
|
height = (terrain[:, :3] - pose[:3]) @ up + contact_height_m
|
|
supported = (
|
|
(np.abs(local) < 0.45).all(axis=1) & (np.abs(height) <= 0.08) & (terrain[:, 3] > 0.1)
|
|
)
|
|
result[supported, 3] = np.abs(height[supported])
|
|
return result, int(supported.sum())
|
|
|
|
|
|
def _neighborhoods(terrain, radius=0.4):
|
|
"""Exact radius neighborhoods without scanning the whole accumulated map.
|
|
|
|
Returns in the nine adjacent cells include every possible neighbor. Keep
|
|
source order so fitting and thresholds remain identical to the full scan.
|
|
"""
|
|
cells = np.floor(terrain[:, :2] / radius).astype(np.int64)
|
|
buckets = {}
|
|
for index, (x, y) in enumerate(cells):
|
|
buckets.setdefault((x, y), []).append(index)
|
|
cached = {}
|
|
|
|
def around(index):
|
|
key = tuple(cells[index])
|
|
if key not in cached:
|
|
x, y = key
|
|
cached[key] = np.array(
|
|
sorted(
|
|
i
|
|
for dx in (-1, 0, 1)
|
|
for dy in (-1, 0, 1)
|
|
for i in buckets.get((x + dx, y + dy), ())
|
|
),
|
|
dtype=np.int64,
|
|
)
|
|
delta = terrain[cached[key], :3] - terrain[index, :3]
|
|
return delta[np.linalg.norm(delta[:, :2], axis=1) <= radius]
|
|
|
|
return around
|
|
|
|
|
|
def supported_slope_costs(terrain, cache=None):
|
|
result = terrain.copy()
|
|
if len(terrain) < 8:
|
|
return result, 0
|
|
around = _neighborhoods(terrain)
|
|
corrected = 0
|
|
for index in np.flatnonzero(terrain[:, 3] > 0.1):
|
|
nearby = around(index)
|
|
if len(nearby) < 8:
|
|
continue
|
|
# Any admitted plane spans at most a 0.8 m diameter at 25 degrees,
|
|
# plus the two 7.5 cm residuals. Reject tall foliage/walls before fitting.
|
|
if np.ptp(nearby[:, 2]) > 0.8 * math.tan(math.radians(25 + 1e-4)) + 0.15:
|
|
continue
|
|
key = None
|
|
if cache is not None:
|
|
key = hashlib.blake2b(nearby.tobytes(), digest_size=24).digest()
|
|
if key in cache:
|
|
if cache[key]:
|
|
result[index, 3] = 0.0
|
|
corrected += 1
|
|
cache.move_to_end(key)
|
|
continue
|
|
cache[key] = False
|
|
if len(cache) > 16384:
|
|
cache.popitem(last=False)
|
|
# No collinear strip, hidden region, multiple height layers or vertical
|
|
# surface is admitted as a plane. All observed points must agree.
|
|
covariance = np.cov(nearby[:, :2], rowvar=False)
|
|
if np.linalg.eigvalsh(covariance)[0] < 0.0036:
|
|
continue
|
|
matrix = np.column_stack((nearby[:, :2], np.ones(len(nearby))))
|
|
plane = np.linalg.lstsq(matrix, nearby[:, 2], rcond=None)[0]
|
|
slope = math.degrees(math.atan(np.linalg.norm(plane[:2])))
|
|
if not 2 <= slope <= 25 + 1e-4 or abs(plane[2]) > 0.05:
|
|
continue
|
|
if np.max(np.abs(matrix @ plane - nearby[:, 2])) > 0.075:
|
|
continue
|
|
# A permissive fit alone could erase a real ledge. Test close measured
|
|
# returns explicitly: even a narrow step/drop must retain its hazard.
|
|
if not connected_grade(nearby):
|
|
continue # Do not fit a road across a gap with no returns.
|
|
result[index, 3] = 0.0
|
|
corrected += 1
|
|
if cache is not None:
|
|
cache[key] = True
|
|
return result, corrected
|
|
|
|
|
|
class TerrainCostNormalizer:
|
|
"""Reuse fits only for byte-identical observed neighborhoods, bounded in RAM.
|
|
|
|
New or changed returns always trigger a new fit. This stores no occupancy
|
|
belief and clears with the owning ROS node on every episode/reset.
|
|
"""
|
|
|
|
def __init__(self):
|
|
self.cache = OrderedDict()
|
|
|
|
def __call__(self, terrain):
|
|
return supported_slope_costs(terrain, self.cache)
|