Add degeneracy detection and solution remapping in align()

.github/workflows/python-app.yml

@@ -27,7 +27,7 @@ jobs:
       run: |
         sudo apt-get install freeglut3-dev
         python -m pip install --upgrade pip
-        pip install point-cloud-registration
+        pip install -e . pykdtree
         pip install q3dviewer==1.1.6
         pip install pytest
     - name: Test with pytest

README.md

@@ -71,6 +71,7 @@ print("Estimated Transform matrix:\n", T_new)
 - [x] **Point-to-Plane ICP** – Improved accuracy using normal constraints  
 - [ ] **Generalized ICP (GICP)** – Handles anisotropic noise and improves robustness  
 - [x] **Normal Distributions Transform (NDT)** – Grid-based registration for high-noise environments  
+- [x] **Degeneracy detection + solution remapping** – Hold unobservable DOFs instead of drifting along them  
 - [ ] **Further optimizations** while staying pure Python  
 ### Demo
 
@@ -106,6 +107,38 @@ python3 demo_matching.py
 
 ![demo](imgs/demo.png)
 
+#### Degeneracy Detection & Solution Remapping
+
+Real-world geometry often under-constrains registration — a featureless
+seafloor, a straight corridor, a staircase. On such scenes the
+unconstrained Gauss-Newton solve slides the estimate along the
+unobservable directions, driven by nothing but noise. `align()` can
+instead eigendecompose its 6×6 Hessian, flag the degenerate directions,
+and hold them at the initial guess while the constrained ones converge
+(solution remapping: Zhang, Kaess & Singh, ICRA 2016; Hinduja, Ho &
+Kaess, IROS 2019):
+
+```python
+icp = PlaneICP(max_iter=50, max_dist=1.0)
+icp.set_target(target)
+T = icp.align(scan, use_solution_remapping=True, lm_damping=True)
+print(icp.last_hessian)  # inspect the observability yourself
+```
+
+```bash
+python3 demo_degeneracy.py --save   # writes imgs/degeneracy_*.png (needs matplotlib)
+```
+
+![Degeneracy demo](imgs/degeneracy_stair.png)
+
+On the synthetic staircase in `data/` only cross-step translation is
+unobservable: plain plane-ICP drifts centimetres along it, solution
+remapping holds it near the initial value while the other five DOFs
+still converge. On a flat plane three DOFs (tx, ty, yaw) are degenerate
+and the effect is an order of magnitude larger. matplotlib is needed
+only by this demo — the feature itself adds no dependencies to the
+library.
+
 ### Comparison of Registration Methods
 
 | Method                        | Objective Function*                                         | Data Representation   | Speed         | Precision    |

data/README.md

@@ -1,4 +1,6 @@
-# License Information for B-01.pcd
+# Data
+
+## B-01.pcd
 
 The file `B-01.pcd` is used to test our algorithms and is sourced from the following dataset:
 
@@ -21,4 +23,24 @@ Under the terms of the license, you must:
 2. **Provide a link to the license**: [CC BY 4.0](https://opendefinition.org/licenses/cc-by/).
 3. **Indicate changes**: If any modifications were made to the original data.
 
-For more details, visit the [license page](https://opendefinition.org/licenses/cc-by/).
+For more details, visit the [license page](https://opendefinition.org/licenses/cc-by/).
+## Synthetic degeneracy clouds
+
+`synthetic_staircase_{target,source}.pcd` and
+`synthetic_plane_{target,source}.pcd` are generated by
+[`generate_synthetic.py`](generate_synthetic.py) with fixed seeds —
+regeneration is byte-identical:
+
+```bash
+python3 data/generate_synthetic.py
+```
+
+They are used by `demo_degeneracy.py` and by the test suite
+(`tests/test_synthetic_data.py`). The staircase constrains every DOF
+except cross-step translation (one degenerate direction); the flat
+plane leaves tx, ty and yaw unobservable (three degenerate directions).
+Target and source are independent samplings of the same surface, so
+registration sees realistic correspondence noise.
+
+This is original synthetic data, licensed with the repository (MIT).
+No attribution requirements.

data/generate_synthetic.py

@@ -0,0 +1,158 @@
+#!/usr/bin/env python3
+"""
+Deterministic generators for the synthetic degenerate test clouds.
+
+Two scenes, each under-constraining registration in a known way:
+
+* staircase — treads (normals +z) and risers (normals +x) constrain
+  tx, tz and all three rotations, but nothing observes translation
+  across the stair width: exactly one degenerate DOF (ty).
+* plane — a featureless flat seafloor. Only tz, roll and pitch are
+  observable: three degenerate DOFs (tx, ty, yaw).
+
+Regeneration is byte-identical (fixed seeds, fixed format string), so
+the committed .pcd files can always be reproduced with
+
+    python3 data/generate_synthetic.py
+
+Design notes that carry the demo's story:
+
+* Faces keep a margin (~ the k-NN neighborhood radius) away from the
+  concave tread/riser folds. Without it, PCA normal estimation blends
+  the two faces at the fold, the blended normals pick up spurious
+  y-components, and the ty direction stops being flagged as degenerate.
+* Centering is deterministic (never the empirical mean), so clouds
+  sampled with different seeds lie on the *same* surface — required for
+  target/source pairs that emulate two scans of one scene.
+
+NumPy only; no external point-cloud dependencies.
+"""
+import argparse
+import os
+
+import numpy as np
+
+
+def make_staircase(n_steps=6, tread=1.0, riser=0.5, width=6.0,
+                   pts_per_face=220, margin=0.10, noise=0.01, seed=42):
+    """
+    Point cloud of a staircase marching along +x and rising in +z.
+
+    Step k contributes a tread (horizontal, normal +z) at height
+    (k+1)*riser and a riser (vertical, normal +x) at x = k*tread.
+    Returns an (N, 3) float64 array centered deterministically.
+    """
+    rng = np.random.default_rng(seed)
+    faces = []
+    for k in range(n_steps):
+        # Tread: z = (k+1)*riser, x in [k*tread+margin, (k+1)*tread-margin].
+        u = rng.uniform(k * tread + margin, (k + 1) * tread - margin,
+                        pts_per_face)
+        v = rng.uniform(0.0, width, pts_per_face)
+        tread_face = np.column_stack([
+            u, v, np.full(pts_per_face, (k + 1) * riser)])
+        # Riser: x = k*tread, z in [k*riser+margin, (k+1)*riser-margin].
+        w = rng.uniform(k * riser + margin, (k + 1) * riser - margin,
+                        pts_per_face)
+        v2 = rng.uniform(0.0, width, pts_per_face)
+        riser_face = np.column_stack([
+            np.full(pts_per_face, k * tread), v2, w])
+        faces.extend([tread_face, riser_face])
+    points = np.vstack(faces)
+    points += rng.normal(0.0, noise, points.shape)
+    center = np.array([n_steps * tread / 2.0, width / 2.0,
+                       n_steps * riser / 2.0])
+    return points - center
+
+
+def make_plane(half_extent=6.0, num_points=2500, noise=0.01, seed=7):
+    """
+    A featureless flat seafloor: uniform samples of z = 0 over
+    [-half_extent, half_extent]^2 with Gaussian surface noise.
+    """
+    rng = np.random.default_rng(seed)
+    xy = rng.uniform(-half_extent, half_extent, (num_points, 2))
+    z = rng.normal(0.0, noise, num_points)
+    return np.column_stack([xy, z])
+
+
+def save_pcd(path, points):
+    """
+    Write an Nx3 array as an ASCII PCD v0.7 file (x y z).
+
+    Coordinates are stored at float32 / six-decimal precision — the PCD
+    FIELDS declare SIZE 4 — so a save/load roundtrip is exact to ~1e-6,
+    not bit-exact against float64 input. Regeneration from the fixed
+    seeds is byte-identical because the quantization itself is
+    deterministic.
+    """
+    points = np.asarray(points, dtype=np.float32)
+    n = points.shape[0]
+    header = "\n".join([
+        "# .PCD v0.7 - Point Cloud Data file format",
+        "VERSION 0.7",
+        "FIELDS x y z",
+        "SIZE 4 4 4",
+        "TYPE F F F",
+        "COUNT 1 1 1",
+        f"WIDTH {n}",
+        "HEIGHT 1",
+        "VIEWPOINT 0 0 0 1 0 0 0",
+        f"POINTS {n}",
+        "DATA ascii",
+    ])
+    with open(path, "w") as f:
+        f.write(header + "\n")
+        for x, y, z in points:
+            f.write(f"{x:.6f} {y:.6f} {z:.6f}\n")
+
+
+def load_pcd(path):
+    """Read an ASCII PCD file with x y z fields into an (N, 3) array."""
+    points = []
+    with open(path) as f:
+        in_data = False
+        for line in f:
+            if in_data:
+                parts = line.split()
+                if len(parts) >= 3:
+                    points.append([float(parts[0]), float(parts[1]),
+                                   float(parts[2])])
+            elif line.startswith("DATA"):
+                if line.split()[1] != "ascii":
+                    raise ValueError(f"{path}: only ASCII PCD is supported")
+                in_data = True
+    return np.array(points)
+
+
+# (filename, generator, kwargs) — target/source pairs are independent
+# samplings of the same surface, so registration sees realistic
+# correspondence noise instead of a permutation of identical points.
+CLOUDS = [
+    ("synthetic_staircase_target.pcd", make_staircase,
+     dict(seed=42, pts_per_face=220)),
+    ("synthetic_staircase_source.pcd", make_staircase,
+     dict(seed=2024, pts_per_face=220)),
+    ("synthetic_plane_target.pcd", make_plane,
+     dict(seed=7, num_points=2500)),
+    ("synthetic_plane_source.pcd", make_plane,
+     dict(seed=99, num_points=2000)),
+]
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__.splitlines()[1])
+    parser.add_argument("--out-dir",
+                        default=os.path.dirname(os.path.abspath(__file__)),
+                        help="Directory to write the .pcd files into "
+                             "(default: this file's directory).")
+    args = parser.parse_args()
+    for name, generator, kwargs in CLOUDS:
+        path = os.path.join(args.out_dir, name)
+        points = generator(**kwargs)
+        save_pcd(path, points)
+        print(f"wrote {path} ({points.shape[0]} points)")
+
+
+if __name__ == "__main__":
+    main()