Skip to content

Fix cov2cov whitening and add residual-variance scaling in covariance()#59

Merged
jlblancoc merged 1 commit intodevelopfrom
fix/cov2cov-covariance-whitening
Apr 28, 2026
Merged

Fix cov2cov whitening and add residual-variance scaling in covariance()#59
jlblancoc merged 1 commit intodevelopfrom
fix/cov2cov-covariance-whitening

Conversation

@jlblancoc
Copy link
Copy Markdown
Member

@jlblancoc jlblancoc commented Apr 28, 2026

The cov2cov branch in covariance.cpp whitened residuals with the full information matrix (cov_inv * e), so the assembled Hessian became J^T * cov_inv^2 * J instead of J^T * cov_inv * J. Combined with hundreds of pairings this drove |cov| down to ~1e-20 and made the estimate unusable.

  • Use the Cholesky factor L^T (with L L^T = cov_inv) to whiten the cov2cov residual, matching what optimal_tf_gauss_newton accumulates.
  • Multiply the inverse-Hessian by chi^2 / (m - 6), the standard a-posteriori unit-weight variance, to rescale the (otherwise optimistic) result by the empirical residual level.

Summary by CodeRabbit

Release Notes

  • Bug Fixes
    • Enhanced covariance calculation methodology: replaced inverse-covariance weighting with Cholesky decomposition-based residual whitening and added empirical variance rescaling for improved uncertainty quantification accuracy.

@coderabbitai
Copy link
Copy Markdown

coderabbitai Bot commented Apr 28, 2026

Warning

Rate limit exceeded

@jlblancoc has exceeded the limit for the number of commits that can be reviewed per hour. Please wait 48 minutes and 33 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: e8b8e053-d9d6-4594-b26d-9a0f3f62ba69

📥 Commits

Reviewing files that changed from the base of the PR and between 564970d and 1bdc476.

📒 Files selected for processing (1)
  • mp2p_icp/src/covariance.cpp
📝 Walkthrough

Walkthrough

The cov-to-cov error term computation has been refactored to use Cholesky decomposition-based whitening of residuals instead of raw inverse-covariance weighting, and now includes a-posteriori chi2-based covariance rescaling for improved statistical alignment.

Changes

Cohort / File(s) Summary
Covariance error term refinement
mp2p_icp/src/covariance.cpp
Replaced raw inverse-covariance weighting with Cholesky factor-based residual whitening; added empirical chi2-based covariance rescaling computed from initial estimate residuals and degrees of freedom.

Estimated code review effort

🎯 4 (Complex) | ⏱️ ~45 minutes

Possibly related PRs

Poem

🐰 A Cholesky hop through covariance's hall,
Whitening residuals, standing tall!
Chi-squared whispers guide the way,
Empirical truths save the day!

🚥 Pre-merge checks | ✅ 4 | ❌ 1

❌ Failed checks (1 warning)

Check name Status Explanation Resolution
Docstring Coverage ⚠️ Warning Docstring coverage is 0.00% 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 'Fix cov2cov whitening and add residual-variance scaling in covariance()' accurately summarizes the main changes: it fixes the whitening approach in cov2cov and adds residual-variance scaling, which are the two key improvements described in the PR objectives.
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 fix/cov2cov-covariance-whitening

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

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

Copy link
Copy Markdown

@coderabbitai coderabbitai Bot left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actionable comments posted: 1

🧹 Nitpick comments (1)
mp2p_icp/src/covariance.cpp (1)

146-150: Cache the whitening factor outside errorLambda.

p.cov_inv is pose-invariant, so this LLT now runs once per cov2cov pairing for every finite-difference Jacobian sample. Precomputing one factor per pairing before estimateJacobian() would keep the math fix and avoid multiplying that cost by the 6-DoF sweep.

🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.

In `@mp2p_icp/src/covariance.cpp` around lines 146 - 150, The LLT decomposition of
p.cov_inv (currently done via Eigen::LLT<Eigen::Matrix3d>(cov_inv).matrixU()
assigned to L_T inside the errorLambda) is computed repeatedly for every
finite-difference sample; move that decomposition out of errorLambda and
precompute a single whitening matrix per cov2cov pairing (e.g., compute L_T once
from p.cov_inv before calling estimateJacobian()) and then use that cached L_T
when assigning err.block<3,1>(...) = L_T * ret.asEigen() inside errorLambda so
the expensive Eigen::LLT is not recomputed for each sample.
🤖 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 168-181: The chi² rescaling currently multiplies cov by chi2/(m-6)
unconditionally after calling errorLambda, but chi2 is only meaningful for
fully-whitened residuals; restrict this scaling to the fully-normalized
paired_cov2cov case. In the covariance() block where errorLambda(xInitial,
lmbParams, errAtOpt) is computed, add a guard that checks whether the residuals
are the whitened paired_cov2cov family (e.g., detect the lmbParams/whitening
flag or the active residual type used in paired_cov2cov) and only then compute
sigma2 = chi2/dof and apply cov.asEigen() *= sigma2; otherwise skip the
empirical scaling. Ensure you reference and use the existing symbols
errorLambda, covariance (the enclosing function), and paired_cov2cov to locate
and implement the guard.

---

Nitpick comments:
In `@mp2p_icp/src/covariance.cpp`:
- Around line 146-150: The LLT decomposition of p.cov_inv (currently done via
Eigen::LLT<Eigen::Matrix3d>(cov_inv).matrixU() assigned to L_T inside the
errorLambda) is computed repeatedly for every finite-difference sample; move
that decomposition out of errorLambda and precompute a single whitening matrix
per cov2cov pairing (e.g., compute L_T once from p.cov_inv before calling
estimateJacobian()) and then use that cached L_T when assigning
err.block<3,1>(...) = L_T * ret.asEigen() inside errorLambda so the expensive
Eigen::LLT is not recomputed for each sample.
🪄 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: 7df2d103-27fa-4d43-a23f-646577fa0c79

📥 Commits

Reviewing files that changed from the base of the PR and between 5b4bdea and 564970d.

📒 Files selected for processing (1)
  • mp2p_icp/src/covariance.cpp

Comment thread mp2p_icp/src/covariance.cpp
The cov2cov branch in covariance.cpp whitened residuals with the full
information matrix (cov_inv * e), so the assembled Hessian became
J^T * cov_inv^2 * J instead of J^T * cov_inv * J. Combined with hundreds
of pairings this drove |cov| down to ~1e-20 and made the estimate
unusable.

- Use the Cholesky factor L^T (with L L^T = cov_inv) to whiten the
  cov2cov residual, matching what optimal_tf_gauss_newton accumulates.
- Multiply the inverse-Hessian by chi^2 / (m - 6), the standard
  a-posteriori unit-weight variance, to rescale the (otherwise
  optimistic) result by the empirical residual level.
@jlblancoc jlblancoc force-pushed the fix/cov2cov-covariance-whitening branch from 564970d to 1bdc476 Compare April 28, 2026 18:17
@jlblancoc jlblancoc enabled auto-merge April 28, 2026 18:17
@jlblancoc jlblancoc merged commit 4c3e07d into develop Apr 28, 2026
13 checks passed
@jlblancoc jlblancoc deleted the fix/cov2cov-covariance-whitening branch April 28, 2026 18:31
@codecov
Copy link
Copy Markdown

codecov Bot commented Apr 28, 2026

Codecov Report

✅ All modified and coverable lines are covered by tests.
✅ Project coverage is 78.66%. Comparing base (5b4bdea) to head (1bdc476).
⚠️ Report is 2 commits behind head on develop.

Additional details and impacted files

Impacted file tree graph

@@             Coverage Diff             @@
##           develop      #59      +/-   ##
===========================================
+ Coverage    78.62%   78.66%   +0.03%     
===========================================
  Files          191      191              
  Lines        10641    10657      +16     
  Branches       986      988       +2     
===========================================
+ Hits          8367     8383      +16     
  Misses        2274     2274              
Files with missing lines Coverage Δ
mp2p_icp/src/covariance.cpp 86.20% <100.00%> (+3.10%) ⬆️
🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant