chore: checkpoint ground-plane calibration refinement work
This commit is contained in:
@@ -43,8 +43,11 @@ class GroundPlaneConfig:
|
||||
max_rotation_deg: float = 5.0
|
||||
max_translation_m: float = 0.1
|
||||
min_inliers: int = 500
|
||||
min_inlier_ratio: float = 0.0
|
||||
min_inlier_ratio: float = 0.15
|
||||
min_valid_cameras: int = 2
|
||||
normal_vertical_thresh: float = 0.9
|
||||
max_consensus_deviation_deg: float = 10.0
|
||||
max_consensus_deviation_m: float = 0.5
|
||||
seed: Optional[int] = None
|
||||
|
||||
|
||||
@@ -160,6 +163,7 @@ def compute_consensus_plane(
|
||||
) -> FloorPlane:
|
||||
"""
|
||||
Compute a consensus plane from multiple plane detections.
|
||||
Uses a robust median-like approach to reject outliers.
|
||||
"""
|
||||
if not planes:
|
||||
raise ValueError("No planes provided for consensus.")
|
||||
@@ -173,30 +177,65 @@ def compute_consensus_plane(
|
||||
f"Weights length {len(weights)} must match planes length {n_planes}"
|
||||
)
|
||||
|
||||
# Use the first plane as reference for orientation
|
||||
ref_normal = planes[0].normal
|
||||
# 1. Align all normals to be in the upper hemisphere (y > 0)
|
||||
# This simplifies averaging
|
||||
aligned_planes = []
|
||||
for p in planes:
|
||||
normal = p.normal.copy()
|
||||
d = p.d
|
||||
if normal[1] < 0:
|
||||
normal = -normal
|
||||
d = -d
|
||||
aligned_planes.append(FloorPlane(normal=normal, d=d, num_inliers=p.num_inliers))
|
||||
|
||||
# 2. Compute median normal and d to be robust against outliers
|
||||
normals = np.array([p.normal for p in aligned_planes])
|
||||
ds = np.array([p.d for p in aligned_planes])
|
||||
|
||||
# Median of each component for normal (approximate robust mean)
|
||||
median_normal = np.median(normals, axis=0)
|
||||
norm = np.linalg.norm(median_normal)
|
||||
if norm > 1e-6:
|
||||
median_normal /= norm
|
||||
else:
|
||||
median_normal = np.array([0.0, 1.0, 0.0])
|
||||
|
||||
median_d = float(np.median(ds))
|
||||
|
||||
# 3. Filter outliers based on deviation from median
|
||||
# Angle deviation
|
||||
valid_indices = []
|
||||
for i, p in enumerate(aligned_planes):
|
||||
# Angle between normal and median normal
|
||||
dot = np.clip(np.dot(p.normal, median_normal), -1.0, 1.0)
|
||||
angle_deg = np.rad2deg(np.arccos(dot))
|
||||
|
||||
# Distance deviation
|
||||
dist_diff = abs(p.d - median_d)
|
||||
|
||||
# Thresholds for outlier rejection (hardcoded for now, could be config)
|
||||
if angle_deg < 15.0 and dist_diff < 0.5:
|
||||
valid_indices.append(i)
|
||||
|
||||
if not valid_indices:
|
||||
# Fallback to all if everything is rejected (should be rare)
|
||||
valid_indices = list(range(n_planes))
|
||||
|
||||
# 4. Weighted average of valid planes
|
||||
accum_normal = np.zeros(3, dtype=np.float64)
|
||||
accum_d = 0.0
|
||||
total_weight = 0.0
|
||||
|
||||
for i, plane in enumerate(planes):
|
||||
for i in valid_indices:
|
||||
w = weights[i]
|
||||
normal = plane.normal
|
||||
d = plane.d
|
||||
|
||||
# Check orientation against reference
|
||||
if np.dot(normal, ref_normal) < 0:
|
||||
# Flip normal and d to align with reference
|
||||
normal = -normal
|
||||
d = -d
|
||||
|
||||
accum_normal += normal * w
|
||||
accum_d += d * w
|
||||
p = aligned_planes[i]
|
||||
accum_normal += p.normal * w
|
||||
accum_d += p.d * w
|
||||
total_weight += w
|
||||
|
||||
if total_weight <= 0:
|
||||
raise ValueError("Total weight must be positive.")
|
||||
# Should not happen given checks above
|
||||
return FloorPlane(normal=median_normal, d=median_d)
|
||||
|
||||
avg_normal = accum_normal / total_weight
|
||||
avg_d = accum_d / total_weight
|
||||
@@ -205,10 +244,8 @@ def compute_consensus_plane(
|
||||
norm = np.linalg.norm(avg_normal)
|
||||
if norm > 1e-6:
|
||||
avg_normal /= norm
|
||||
# Scale d by 1/norm to maintain plane equation consistency
|
||||
avg_d /= norm
|
||||
else:
|
||||
# Fallback (should be rare if inputs are valid)
|
||||
avg_normal = np.array([0.0, 1.0, 0.0])
|
||||
avg_d = 0.0
|
||||
|
||||
@@ -223,10 +260,14 @@ def compute_floor_correction(
|
||||
target_floor_y: float = 0.0,
|
||||
max_rotation_deg: float = 5.0,
|
||||
max_translation_m: float = 0.1,
|
||||
target_plane: Optional[FloorPlane] = None,
|
||||
) -> FloorCorrection:
|
||||
"""
|
||||
Compute the correction transform to align the current floor plane to the target floor height.
|
||||
Constrains correction to pitch/roll and vertical translation only.
|
||||
|
||||
If target_plane is provided, aligns current plane to target_plane (relative correction).
|
||||
Otherwise, aligns to absolute Y=target_floor_y (absolute correction).
|
||||
"""
|
||||
current_normal = current_floor_plane.normal
|
||||
current_d = current_floor_plane.d
|
||||
@@ -234,9 +275,19 @@ def compute_floor_correction(
|
||||
# Target normal is always [0, 1, 0] (Y-up)
|
||||
target_normal = np.array([0.0, 1.0, 0.0])
|
||||
|
||||
if target_plane is not None:
|
||||
# Use target_plane.normal as the target normal
|
||||
align_target_normal = target_plane.normal
|
||||
|
||||
# Ensure it points roughly up
|
||||
if align_target_normal[1] < 0:
|
||||
align_target_normal = -align_target_normal
|
||||
else:
|
||||
align_target_normal = target_normal
|
||||
|
||||
# 1. Compute rotation to align normals
|
||||
try:
|
||||
R_align = rotation_align_vectors(current_normal, target_normal)
|
||||
R_align = rotation_align_vectors(current_normal, align_target_normal)
|
||||
except ValueError as e:
|
||||
return FloorCorrection(
|
||||
transform=np.eye(4), valid=False, reason=f"Rotation alignment failed: {e}"
|
||||
@@ -258,27 +309,48 @@ def compute_floor_correction(
|
||||
)
|
||||
|
||||
# 2. Compute translation
|
||||
# We want to move points such that the floor is at y = target_floor_y
|
||||
# Plane equation: n . p + d = 0
|
||||
# Current floor at y = -current_d (if n=[0,1,0])
|
||||
# We want new y = target_floor_y
|
||||
# So shift = target_floor_y - (-current_d) = target_floor_y + current_d
|
||||
if target_plane is not None:
|
||||
# Relative correction: align d to target_plane.d
|
||||
# Shift = current_d - target_plane.d (assuming normals aligned)
|
||||
# We use absolute values of d to handle potential sign flips in plane detection
|
||||
# But wait, d sign matters for plane side.
|
||||
# If normals are aligned (which we ensured with R_align and align_target_normal),
|
||||
# then d should be comparable directly.
|
||||
# However, target_plane.d might be negative if normal was flipped.
|
||||
# Let's use the d corresponding to align_target_normal.
|
||||
|
||||
t_y = target_floor_y + current_d
|
||||
target_d = target_plane.d
|
||||
if np.dot(target_plane.normal, align_target_normal) < 0:
|
||||
target_d = -target_d
|
||||
|
||||
# Current d needs to be relative to current normal?
|
||||
# No, current_d is relative to current_normal.
|
||||
# After rotation R_align, current_normal becomes align_target_normal.
|
||||
# So current_d is preserved (distance to origin doesn't change with rotation around origin).
|
||||
# So we just compare d values.
|
||||
|
||||
t_mag = current_d - target_d
|
||||
trans_dir = align_target_normal
|
||||
else:
|
||||
# Absolute correction to target_y
|
||||
# We want new y = target_floor_y
|
||||
# So shift = target_floor_y + current_d
|
||||
t_mag = target_floor_y + current_d
|
||||
trans_dir = target_normal
|
||||
|
||||
# Check translation magnitude
|
||||
if abs(t_y) > max_translation_m:
|
||||
if abs(t_mag) > max_translation_m:
|
||||
return FloorCorrection(
|
||||
transform=np.eye(4),
|
||||
valid=False,
|
||||
reason=f"Translation {t_y:.3f} m exceeds limit {max_translation_m:.3f} m",
|
||||
reason=f"Translation {t_mag:.3f} m exceeds limit {max_translation_m:.3f} m",
|
||||
)
|
||||
|
||||
# Construct T
|
||||
T = np.eye(4)
|
||||
T[:3, :3] = R_align
|
||||
# Translation is applied in the rotated frame (aligned to target normal)
|
||||
T[:3, 3] = target_normal * t_y
|
||||
T[:3, 3] = trans_dir * t_mag
|
||||
|
||||
return FloorCorrection(transform=T.astype(np.float64), valid=True)
|
||||
|
||||
@@ -360,6 +432,11 @@ def refine_ground_from_depth(
|
||||
if ratio < config.min_inlier_ratio:
|
||||
continue
|
||||
|
||||
# Check normal orientation (must be roughly vertical)
|
||||
# We expect floor normal to be roughly [0, 1, 0] or [0, -1, 0]
|
||||
if abs(plane.normal[1]) < config.normal_vertical_thresh:
|
||||
continue
|
||||
|
||||
metrics.camera_planes[serial] = plane
|
||||
valid_planes.append(plane)
|
||||
valid_serials.append(serial)
|
||||
@@ -400,12 +477,42 @@ def refine_ground_from_depth(
|
||||
target_floor_y=config.target_y,
|
||||
max_rotation_deg=config.max_rotation_deg,
|
||||
max_translation_m=config.max_translation_m,
|
||||
target_plane=metrics.consensus_plane,
|
||||
)
|
||||
|
||||
if not correction.valid:
|
||||
metrics.skipped_cameras.append(serial)
|
||||
continue
|
||||
|
||||
# Validate against consensus if available
|
||||
if metrics.consensus_plane:
|
||||
# Check if this camera's plane is too far from consensus
|
||||
# This prevents a single bad camera from getting a huge correction
|
||||
# even if it passed individual checks (e.g. it found a wall instead of floor)
|
||||
|
||||
# Angle check
|
||||
dot = np.clip(
|
||||
np.dot(plane.normal, metrics.consensus_plane.normal), -1.0, 1.0
|
||||
)
|
||||
# Handle flipped normals
|
||||
if dot < 0:
|
||||
dot = -dot
|
||||
angle_deg = np.rad2deg(np.arccos(dot))
|
||||
|
||||
if angle_deg > config.max_consensus_deviation_deg:
|
||||
metrics.skipped_cameras.append(serial)
|
||||
continue
|
||||
|
||||
# Distance check (project consensus origin onto this plane)
|
||||
# Consensus plane: n_c . p + d_c = 0
|
||||
# This plane: n . p + d = 0
|
||||
# Compare d values (assuming normals aligned)
|
||||
d_diff = abs(abs(plane.d) - abs(metrics.consensus_plane.d))
|
||||
|
||||
if d_diff > config.max_consensus_deviation_m:
|
||||
metrics.skipped_cameras.append(serial)
|
||||
continue
|
||||
|
||||
T_corr = correction.transform
|
||||
metrics.camera_corrections[serial] = T_corr
|
||||
|
||||
|
||||
Reference in New Issue
Block a user