Occasionally I get a singular matrix error

[kpwelsh]

After doing some random registrations, I sometimes run into a singular matrix error for H.

Not really sure why, but H is all 0s.

[scomup]

Can you provide the data to reproduce this issue?

[kpwelsh]

I cannot share the exact cloud that was causing issues, unfortunately. I can describe it, though.

It is a single beam of a train rail. Its generated from a CAD model, so its got sharp edges, planar features, and is downsampled to a regular degree.

I took a minute to explore possible issues, and found two cases:

1. If there are no correspondences, it will trivially result in H=0 and raise a singularity error. Thats not that big of a deal, but I would personally prefer a clearer error message.
2. Perhaps more unintuitively, this optimizer can diverge. Classic ICP solves orthogonal procrustes, and if I understand correctly, that doesn't have divergence failure mode.

Im not an expert on this optimization, so I am not sure if there is a sensible way to guard against this in your implementation (maybe optional regularization?) or if the best choice is just to raise a more clear error. I don't see anything clearly wrong with your code, but I wasn't really sure what to do or what went wrong when I got a singularity error.

Heres a repro script that exemplifies the divergence of this optimizer compared to orthogonal procrustes:

import numpy as np
from point_cloud_registration import ICP


def transform_points(T, points):
    return (T[:3, :3] @ points.T).T + T[:3, 3]


def orthogonal_procrustes(source, target):
    source_center = source.mean(axis=0)
    target_center = target.mean(axis=0)

    source_demeaned = source - source_center
    target_demeaned = target - target_center

    U, _, Vt = np.linalg.svd(source_demeaned.T @ target_demeaned)
    R = Vt.T @ U.T

    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1
        R = Vt.T @ U.T

    t = target_center - R @ source_center

    T = np.eye(4, dtype=np.float32)
    T[:3, :3] = R
    T[:3, 3] = t
    return T


def rmse(T, source, target):
    residual = transform_points(T, source) - target
    return np.sqrt(np.mean(np.sum(residual * residual, axis=1)))


rng = np.random.default_rng(0)
np.set_printoptions(precision=6, suppress=True)

source = (
    np.array([10.0, 0.0, 0.0]) +
    rng.normal(scale=0.1, size=(20, 3))
).astype(np.float32)

target = (
    source +
    rng.normal(scale=0.01, size=(20, 3))
).astype(np.float32)

icp = ICP(max_iter=10, max_dist=0.05, tol=1e-12)
icp.set_target(target)
try:
    icp_T = icp.align(source, verbose=True)
except np.linalg.LinAlgError as exc:
    icp_T = None
    print(f"ICP failed: {exc}")

procrustes_T = orthogonal_procrustes(source, target)

if icp_T is not None:
    print("\nICP transform:")
    print(icp_T)
    print("ICP RMSE:", rmse(icp_T, source, target))

print("\nOrthogonal Procrustes transform:")
print(procrustes_T)
print("Orthogonal Procrustes RMSE:", rmse(procrustes_T, source, target))