pointcloud: normalize plane distance by the normal, not the query point

[Nicolas Palpacuer (NickPPC)]

Summary

pointcloud.Plane.Distance normalized the plane equation by the length of the query point instead of the length of the plane's normal. The reported distance therefore shrank the further the point was from the origin, and a point at the origin returned NaN.

Found while auditing pointcloud/ after #6314–#6324.

The bug

return (p.equation[0]*pt.X + p.equation[1]*pt.Y + p.equation[2]*pt.Z + p.equation[3]) / pt.Norm()

For plane ax+by+cz+d = 0 the point distance is (ax+by+cz+d) / sqrt(a²+b²+c²).

Measured against the unpatched code, for the plane z = 0 (unit normal, so the true distance is just z):

point	true distance	reported
(0, 0, 5)	5	1.0
(100, 0, 5)	5	0.0499
(1000, 1000, 5)	5	0.0035
(0, 0, 0)	0	NaN

vision/segmentation/plane_segmentation.go:40 already contained the correct formula, so the codebase held two implementations of the same equation that disagreed.

Impact

• ThresholdPointCloudByPlane compares |dist| <= threshold. A point 500mm from the plane at (1000, 1000, 500) reported 0.33, so with a 10mm threshold it was treated as lying on the plane. Points far from the origin are pulled under any threshold; points near it are pushed out.
• GetResidual (plane-fit quality, drives voxel plane fitting) is scaled per-point by 1/|pt|, so the residual is not a fitting error at all.
• SplitPointCloudByPlane only uses the sign and |pt| is positive, so it is unaffected — except at the origin, where NaN fails both comparisons and the point is silently dropped.

Degenerate planes

NewEmptyPlane produces an all-zero equation, which previously yielded 0/|pt| = 0 — i.e. every point read as lying exactly on a plane that does not exist. It now returns +Inf, so threshold and residual callers treat a non-existent plane as containing nothing. TestEmptyPlane is updated accordingly.

Testing

go test ./... across the whole repo passes. golangci-lint clean.

Added TestPlaneDistanceNormalizesByNormal: distance is invariant to the point's position along the plane, is signed, handles the origin, and gives the same answer for 2x+2y-2z=0 as for the identical plane x+y-z=0. Verified to fail against the unpatched source (Expected '1' to almost equal '5').

Why this survived

The existing TestNewPlane probes plane {1,1,-1,0} at point (-1,-1,1). |pt| = sqrt(3) and |normal| = sqrt(3) — the two divisors coincide, so that assertion passes under either formula. It is the only distance assertion in the package.

Tickets

None

Claude Code Prompts Used

• "Hi Claude, I would like you to take a hard look at this repo. I want you to search for bugs, mistakes, and strange choices..."
• "Can you work on the remaining findings following the priority order you have outlined? ... Then audit spatialmath collision internals and audit pointcloud/"

🤖 Generated with Claude Code

[Dan Gottlieb (dgottlieb)]

I don't understand this -- is this a problem someone ran into? It's not clear to me that an empty plane having an infinite distance from everything is better than containing nothing?

[Nicolas Palpacuer (NickPPC)]

The original intent was just to fix the normalization.

is this a problem someone ran into?
No

I would say this is just a matter of convention. Do we want an empty plane to be everywhere (0 distance) or nowhere (infinite distance)? Alternatively we can refactor to Distance() (float64, error) and return an error on empty planes, it would make the handling of those cases more explicit

[Dan Gottlieb (dgottlieb)]

Ah, I see now. The diff definitely draws the eyes to the wrong part. I guess this is fine.

[Dan Gottlieb (dgottlieb)]

Ah, I see now. The diff definitely draws the eyes to the wrong part. I guess this is fine.

[Nicolas Palpacuer (NickPPC)]

Alternative implementation in #6328, which changes the signature to Distance() (float64, error) and handles the degenerate case explicitly at every call site instead of returning +Inf.

These two PRs are mutually exclusive — both contain the same underlying fix (divide by |normal|, not |pt|), they differ only in how the degenerate plane is expressed.

• This PR: 2 files, no API change, degeneracy encoded as +Inf.
• pointcloud: make Plane.Distance return an error for a degenerate plane #6328: 7 files, breaks Plane.Distance and GetResidual, degeneracy in the type.

#6328's description has a side-by-side trade-off section.