Fix BCPD convergence issues and align with the original C implementation

[tatsuya-ogawa]

Summary

This PR fixes significant convergence issues in the BCPD (Bayesian Coherent Point Drift) implementation. By comparing the logic with the original C implementation (ohirose/bcpd), several mathematical and implementation-level discrepancies were identified and corrected. These fixes enable BCPD to achieve the same level of accuracy and stability as the original implementation, especially when using extreme regularization parameters (e.g., "rigid-hack" settings).

Key Bug Fixes and Improvements

1. Corrected Frobenius Inner Product for Scale Update

• In the M-step, the scale update logic previously used a simplified trace calculation that was mathematically inconsistent with the Procrustes analysis. It has been corrected to use the Frobenius inner product (np.sum(rot * s_xu)), matching exactly with the index-summation in the C code L365-367.

2. Updated Transformation Usage in Variance Calculation

• The residual variance ($\sigma^2$) was previously being updated using the transformation parameters from the previous iteration. This led to stalled convergence. The logic now re-calculates y_hat using the latest R, s, t before updating $\sigma^2$, as seen in the original implementation's update order L373-382.

3. Outlier Probability Normalization

• The outlier term has been changed from w / N to w / volume (where volume is the bounding box of the target). This aligns with the probability density of a uniform distribution over the target space, preventing bias across different data scales. L265.

4. Initialization and Scaling Fixes

• Corrected the initial $\sigma^2$ calculation to use $\gamma^2$ scaling, ensuring consistency with the standard deviation definition in the original paper and C implementation L251.
• Fixed a bug where sigma2 was squared again in the s2s2 ratio calculation, which severely decoupled $\lambda$ and $\sigma^2$ as they approached zero.

5. Optionalized Debias Terms

• Certain debias/correction terms were being applied constantly. In the original C implementation, these are only enabled with the -a flag (aliased as db in the source). They are now disabled by default to match the standard behavior, significantly improving initial convergence speed. L179, L331, L364, L377.

Verification

Tested using the Stanford Bunny dataset (bunny.pcd) with 500 outliers and a 30-degree rotation.

Metric	Previous implementation	This PR	Original C Implementation
RMSE (Final)	0.012900	0.003600	0.003603
Convergence	Stalled / Poor	Success	Success

The results show that the Python implementation now matches the original C implementation's performance within 0.1% accuracy.

def compute_mse(source_file, target_file):
    source = np.loadtxt(source_file)
    target = np.loadtxt(target_file)
    tree = cKDTree(target)
    dist, _ = tree.query(source)
    return np.mean(dist**2)

print("\n--- Test : C code params (lmd=1e10, k=1e80, gamma=0.5) ---")
tf_result = bcpd.registration_bcpd(source, target, w=0.1, lmd=1e10, k=1e80, gamma=0.5, maxiter=100, tol=1e-9, beta=1.0, kernel='imq')
target_estimated = tf_result.transform(source)
rmse = compute_rmse(target_estimated, target)
print(f"RMSE (C params, IMQ): {rmse:.6f}")
print(f"  scale={tf_result.rigid_trans.scale:.6f}")

[neka-nat]

Thanks!