Feat: Censi3D covariance method

[jlblancoc]

Summary by CodeRabbit

• New Features

  • Added configurable covariance estimation methods for ICP algorithms with support for multiple computation backends.
  • Added configuration parameters for covariance estimation, including point sigma and diagonal variance floors for axes and angles.
  • Added YAML serialization support for covariance configuration parameters.

• Tests

  • Added comprehensive test coverage for covariance estimation functionality.
  • Added test coverage for YAML-driven ICP pipeline parsing with covariance configuration.

[coderabbitai]

Warning

Rate limit exceeded

@jlblancoc has exceeded the limit for the number of commits that can be reviewed per hour. Please wait 19 minutes and 12 seconds before requesting another review.

To keep reviews running without waiting, you can enable usage-based add-on for your organization. This allows additional reviews beyond the hourly cap. Account admins can enable it under billing.

⌛ How to resolve this issue?

After the wait time has elapsed, a review can be triggered using the @coderabbitai review command as a PR comment. Alternatively, push new commits to this PR.

We recommend that you space out your commits to avoid hitting the rate limit.

🚦 How do rate limits work?

CodeRabbit enforces hourly rate limits for each developer per organization.

Our paid plans have higher rate limits than the trial, open-source and free plans. In all cases, we re-allow further reviews after a brief timeout.

Please see our FAQ for further information.

ℹ️ Review info

⚙️ Run configuration

Configuration used: defaults

Review profile: CHILL

Plan: Pro

Run ID: 3a050137-ad01-41b4-a807-466d6ee524a4

📥 Commits

Reviewing files that changed from the base of the PR and between 8fa4642 and 0258aaa.

📒 Files selected for processing (8)

• mp2p_icp/include/mp2p_icp/Parameters.h
• mp2p_icp/include/mp2p_icp/covariance.h
• mp2p_icp/src/ICP.cpp
• mp2p_icp/src/Parameters.cpp
• mp2p_icp/src/covariance.cpp
• tests/CMakeLists.txt
• tests/test-mp2p_censi3d_covariance.cpp
• tests/test-mp2p_pipeline_from_yaml.cpp

📝 Walkthrough

Walkthrough

This PR introduces a configurable covariance estimation system for the ICP algorithm, enabling selection between InverseHessian and Censi3D methods via a Method enum in CovarianceParameters. The new parameters are integrated into the main ICP Parameters struct with YAML serialization support, refactors the covariance computation to dispatch on the selected method, and adds comprehensive test coverage for both the new Censi3D implementation and YAML configuration parsing.

Changes

Cohort / File(s)	Summary
Configuration & Types mp2p_icp/include/mp2p_icp/covariance.h, mp2p_icp/include/mp2p_icp/Parameters.h	Added CovarianceParameters::Method enum (InverseHessian, Censi3D), numeric parameters for sigma defaults and floor stabilization, and YAML load/save method declarations. Integrated covariance_params member into main Parameters struct with corresponding import.
Core Covariance Implementation mp2p_icp/src/covariance.cpp	Refactored covariance computation to dispatch based on param.method, removed chi²/dof residual-variance scaling from inverse-hessian path, introduced per-pair Cholesky factorization (removed caching), added new covariance_censi3d function, and implemented diagonal variance floor application. Updated YAML serialization to handle method enum and degree-to-radian conversion for floor_sigma_angles_deg.
ICP Integration & Serialization mp2p_icp/src/ICP.cpp, mp2p_icp/src/Parameters.cpp	Modified ICP::align to use configured covariance_params directly with timing instrumentation. Enhanced Parameters::load_from and save_to to conditionally parse/serialize "covariance" YAML block.
Test Infrastructure & Coverage tests/CMakeLists.txt, tests/test-mp2p_censi3d_covariance.cpp, tests/test-mp2p_pipeline_from_yaml.cpp	Added two test targets and corresponding test files validating Censi3D covariance computation correctness (isotropic/anisotropic scenes, floor enforcement, pt2pt paths) and YAML-driven ICP pipeline parsing with covariance configuration scenarios.

Sequence Diagram(s)

sequenceDiagram
    participant ICP as ICP::align
    participant Cov as covariance()
    participant InvH as covariance_inverse_hessian()
    participant Censi as covariance_censi3d()
    participant Floor as apply_floor()

    ICP->>Cov: Call with p.covariance_params
    alt No pairings
        Cov-->>ICP: Return diagonal fallback
    else Pairings exist
        Cov->>Cov: Check param.method
        alt method == InverseHessian
            Cov->>InvH: Numeric Jacobian approach
            InvH->>InvH: Per-pair Cholesky whitening
            InvH->>Floor: Result matrix
        else method == Censi3D
            Cov->>Censi: Analytic sandwich approach
            Censi->>Censi: Per-pair covariance inverse
            Censi->>Floor: Result matrix
        end
        Floor->>Floor: Apply diagonal variance floor
        Floor-->>Cov: Stabilized covariance matrix
        Cov-->>ICP: Return computed covariance
    end

Loading

Estimated Code Review Effort

🎯 3 (Moderate) | ⏱️ ~25 minutes

Possibly Related PRs

• Fix cov2cov whitening and add residual-variance scaling in covariance() #59: Directly related—PR #59 introduced Cholesky whitening plus chi²/(m-6) residual scaling that this PR partially removes and refactors as method-specific behavior.
• Fix bugs in covariance estimation for some cases #29: Related through covariance.cpp modifications including cov2cov handling and algorithmic changes to residual whitening.

Poem

🐰 A covariance leap with methods two,
Censi3D arrives to join Hessian's crew,
YAML configs dance, tests bloom anew,
Floor parameters keep variance true! ✨

🚥 Pre-merge checks | ✅ 4 | ❌ 1

❌ Failed checks (1 warning)

Check name	Status	Explanation	Resolution
Docstring Coverage	⚠️ Warning	Docstring coverage is 59.09% which is insufficient. The required threshold is 80.00%.	Write docstrings for the functions missing them to satisfy the coverage threshold.

✅ Passed checks (4 passed)

Check name	Status	Explanation
Description Check	✅ Passed	Check skipped - CodeRabbit’s high-level summary is enabled.
Title check	✅ Passed	The title 'Feat: Censi3D covariance method' directly and clearly identifies the main feature being added—a new Censi3D method for covariance estimation in the ICP pipeline.
Linked Issues check	✅ Passed	Check skipped because no linked issues were found for this pull request.
Out of Scope Changes check	✅ Passed	Check skipped because no linked issues were found for this pull request.

_{✏️ Tip: You can configure your own custom pre-merge checks in the settings.}

✨ Finishing Touches

🧪 Generate unit tests (beta)

• Create PR with unit tests
• Commit unit tests in branch feat/censi3d-covariance

---

Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out.

❤️ Share

• X
• Mastodon
• Reddit
• LinkedIn

_{Review rate limit: 0/1 reviews remaining, refill in 19 minutes and 12 seconds.}

_{Comment @coderabbitai help to get the list of available commands and usage tips.}

[coderabbitai]

Actionable comments posted: 2

🤖 Prompt for all review comments with AI agents

Verify each finding against the current code and only fix it if needed.

Inline comments:
In `@mp2p_icp/src/covariance.cpp`:
- Around line 264-267: In covariance_censi3d(), guard the unconditional
Eigen::LDLT<M6> Hldlt(H) and subsequent solves (Hldlt.solve(M),
Hldlt.solve(...)) by checking the decomposition succeeded and that H is positive
definite (e.g., verify Hldlt.info()==Eigen::Success and Hldlt.isPositive()); if
the check fails (singular/near-singular H) return the same conservative fallback
covariance (1e6 on diagonal) used for empty pairings instead of calling
Hldlt.solve on a bad factorization; ensure the early-return prevents computing
Hinv_M and cov_eigen when the LDLT is invalid.

In `@mp2p_icp/src/Parameters.cpp`:
- Around line 95-98: The binary CSerializable path currently omits
covariance_params causing YAML-loaded covariance to be lost; update
serializeGetVersion(), serializeTo(CArchive&), and serializeFrom(CArchive&, int)
to include all six CovarianceParameters fields (method, defaultPointSigma,
floor_sigma_xyz, floor_sigma_angles, finDif_xyz, finDif_angles) in the binary
stream alongside covariance_params (or by serializing covariance_params as a
struct), so that serializeTo writes and serializeFrom reads these fields and
serializeGetVersion increments/reflects any version change; reference
covariance_params, CovarianceParameters, serializeGetVersion, serializeTo, and
serializeFrom when making the changes.

🪄 Autofix (Beta)

Fix all unresolved CodeRabbit comments on this PR:

• Push a commit to this branch (recommended)
• Create a new PR with the fixes

---

ℹ️ Review info

⚙️ Run configuration

Configuration used: defaults

Review profile: CHILL

Plan: Pro

Run ID: 4bf361e4-c6bf-4cbc-9ef6-efaf8e72e165

📥 Commits

Reviewing files that changed from the base of the PR and between 691506c and 8fa4642.

📒 Files selected for processing (8)

• mp2p_icp/include/mp2p_icp/Parameters.h
• mp2p_icp/include/mp2p_icp/covariance.h
• mp2p_icp/src/ICP.cpp
• mp2p_icp/src/Parameters.cpp
• mp2p_icp/src/covariance.cpp
• tests/CMakeLists.txt
• tests/test-mp2p_censi3d_covariance.cpp
• tests/test-mp2p_pipeline_from_yaml.cpp