feat: Implement Birge-ratio auto-balance for cov2cov and prior weighting

agents.md

@@ -199,6 +199,7 @@ Tests use gtest. Each filter, matcher, solver, and serializer has its own test f
 - Parameters follow the `Parameterizable` interface: `initialize(mrpt::containers::yaml)`
 - Always use braces `{}` for all `if`/`for`/`while` blocks
 - Coordinate frame naming: `T_A_to_B` = pose of {B} as seen from {A}; `composePoint` transforms FROM the local (B) frame TO the reference (A) frame
+- Don't use long hyphens. Use American spelling.
 
 ---
 

mp2p_icp/include/mp2p_icp/optimal_tf_gauss_newton.h

@@ -58,6 +58,22 @@ struct OptimalTF_GN_Parameters
     RobustKernel kernel      = RobustKernel::None;
     double       kernelParam = 1.0;
 
+    /** Generalized (tempered) Bayesian scaling for the cov-to-cov data block:
+     *  the cov2cov contribution to H and g is multiplied by `cov2cov_alpha`.
+     *  α=1 keeps the standard MAP cost; α<1 down-weights the data likelihood
+     *  (e.g. α=1/N turns it into a "mean residual" form). Useful when many
+     *  cov-to-cov pairings carry correlated information that the per-pair
+     *  modeled covariances do not capture, and the prior is otherwise drowned. */
+    double cov2cov_alpha = 1.0;
+
+    /** If true (default) and a prior is provided, automatically balance the
+     *  cov2cov data block against the prior using a Birge-ratio-style global
+     *  scale of the modeled per-pair covariances:
+     *    κ = max(1, χ²_cc / (3·N_cc − 6))
+     *  evaluated at each iteration's current residuals; the cov2cov block of
+     *  H and g is then scaled by 1/κ. Has no effect when the prior is absent. */
+    bool cov2cov_auto_balance_with_prior = true;
+
     bool verbose = false;
 };
 

mp2p_icp/src/optimal_tf_gauss_newton.cpp

@@ -198,6 +198,11 @@ bool mp2p_icp::optimal_tf_gauss_newton(
         //
         // ============== cov-to-cov ===============
         //
+        // Accumulate the cov2cov block separately so we can apply a global
+        // scale (generalized-Bayes α and/or Birge-ratio κ) before adding to H/g.
+        Eigen::Matrix<double, 6, 6> H_cc    = Eigen::Matrix<double, 6, 6>::Zero();
+        Eigen::Matrix<double, 6, 1> g_cc    = Eigen::Matrix<double, 6, 1>::Zero();
+        double                      chi2_cc = 0;  // sum of Mahalanobis squared norms
 #if defined(MP2P_HAS_TBB)
         const auto [H_tbb_cov2cov, g_tbb_cov2cov, errNormSqr_tbb_cov2cov] = tbb::parallel_reduce(
             // Range
@@ -243,9 +248,9 @@ bool mp2p_icp::optimal_tf_gauss_newton(
             // 2nd lambda: Parallel reduction
             [](const Result& a, const Result& b) -> Result { return a + b; });
 
-        H.noalias() += H_tbb_cov2cov;
-        g.noalias() += g_tbb_cov2cov;
-        errNormSqr += errNormSqr_tbb_cov2cov;
+        H_cc    = H_tbb_cov2cov;
+        g_cc    = g_tbb_cov2cov;
+        chi2_cc = errNormSqr_tbb_cov2cov;
 #else
         // Cov-to-cov:
         for (size_t idx_pairing = 0; idx_pairing < nCov2Cov; idx_pairing++)
@@ -270,15 +275,32 @@ bool mp2p_icp::optimal_tf_gauss_newton(
 
             // Error and Jacobian:
             const Eigen::Vector3d err_i = ret.asEigen();
-            errNormSqr += weight * retSqrNorm;
+            chi2_cc += weight * retSqrNorm;
 
             const Eigen::Matrix<double, 3, 6> Ji = J1.asEigen() * dDexpe_de.asEigen();
 
             // Whitening: multiply by Σ^{-1/2}
-            g.noalias() += weight * Ji.transpose() * cov_inv * err_i;
-            H.noalias() += weight * Ji.transpose() * cov_inv * Ji;
+            g_cc.noalias() += weight * Ji.transpose() * cov_inv * err_i;
+            H_cc.noalias() += weight * Ji.transpose() * cov_inv * Ji;
         }
 #endif
+        // Generalized-Bayes / Birge-ratio scaling of the cov2cov data block,
+        // to keep a meaningful balance against the prior when many cov2cov
+        // pairings carry correlated information not captured by their per-pair
+        // modeled covariances. See OptimalTF_GN_Parameters docs.
+        {
+            double cc_scale = gnParams.cov2cov_alpha;
+            if (gnParams.cov2cov_auto_balance_with_prior && gnParams.prior.has_value() &&
+                nCov2Cov >= 3)
+            {
+                const double dof   = std::max(1.0, 3.0 * static_cast<double>(nCov2Cov) - 6.0);
+                const double kappa = std::max(1.0, chi2_cc / dof);
+                cc_scale /= kappa;
+            }
+            H.noalias() += cc_scale * H_cc;
+            g.noalias() += cc_scale * g_cc;
+            errNormSqr += cc_scale * chi2_cc;
+        }
         //
         // ============== Point-to-line ===============
         //

tests/CMakeLists.txt

@@ -60,6 +60,7 @@ mp2p_add_test(mp2p_matcher_pt2pt_parameterizable)
 mp2p_add_test(mp2p_matcher_pt2pt)
 mp2p_add_test(mp2p_optimal_tf_algos)
 mp2p_add_test(mp2p_optimize_cov2cov)
+mp2p_add_test(mp2p_optimize_cov2cov_with_prior)
 mp2p_add_test(mp2p_censi3d_covariance)
 mp2p_add_test(mp2p_pipeline_from_yaml)
 mp2p_add_test(mp2p_optimize_pt2ln)

tests/test-mp2p_optimize_cov2cov_with_prior.cpp

@@ -0,0 +1,174 @@
+/*               _
+ _ __ ___   ___ | | __ _
+| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for
+| | | | | | (_) | | (_| | Localization and mApping (MOLA)
+|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola
+
+ A repertory of multi primitive-to-primitive (MP2P) ICP algorithms
+ and map building tools. mp2p_icp is part of MOLA.
+
+ Copyright (C) 2018-2026 Jose Luis Blanco, University of Almeria,
+                         and individual contributors.
+ SPDX-License-Identifier: BSD-3-Clause
+*/
+
+/**
+ * @file   test-mp2p_optimize_cov2cov_with_prior.cpp
+ * @brief  Verifies that, when many cov2cov pairings are combined with a
+ *         pose prior, the prior is not drowned by the data block thanks to
+ *         the generalized-Bayes alpha and the Birge-ratio auto-balance.
+ * @author Jose Luis Blanco Claraco
+ */
+
+#include <mp2p_icp/Pairings.h>
+#include <mp2p_icp/Results.h>
+#include <mp2p_icp/optimal_tf_gauss_newton.h>
+#include <mrpt/poses/Lie/SO.h>
+
+#include <random>
+
+namespace
+{
+// Build a set of cov2cov pairings consistent (up to noise) with `gtPose`,
+// scattered around the origin. Each pairing has a slightly inflated, isotropic
+// inverse covariance so that no single pairing fixes a degenerate direction.
+mp2p_icp::Pairings makeCov2CovPairings(const mrpt::poses::CPose3D& gtPose, std::size_t N)
+{
+    std::mt19937                          rng(42);
+    std::uniform_real_distribution<float> u(-5.0f, 5.0f);
+    // Actual point-pair noise is large (σ ≈ 0.3 m), but the *modeled*
+    // per-pair information below pretends σ ≈ 1 cm — i.e. the per-pair
+    // covariances are heavily overconfident, mimicking the realistic case
+    // where neighbouring cov2cov pairings see correlated surface noise that
+    // their independent Gaussians cannot capture.
+    std::normal_distribution<float> noise(0.0f, 0.3f);
+
+    mp2p_icp::Pairings out;
+    out.paired_cov2cov.reserve(N);
+    for (std::size_t i = 0; i < N; i++)
+    {
+        auto& p       = out.paired_cov2cov.emplace_back();
+        p.local       = {u(rng), u(rng), u(rng)};
+        const auto pg = gtPose.composePoint(
+            {p.local.x + noise(rng), p.local.y + noise(rng), p.local.z + noise(rng)});
+        p.global = {static_cast<float>(pg.x), static_cast<float>(pg.y), static_cast<float>(pg.z)};
+        // Overconfident modeled per-pair information: σ ≈ 1 cm isotropic.
+        p.cov_inv.setDiagonal(std::vector<float>({1e4f, 1e4f, 1e4f}));
+    }
+    return out;
+}
+}  // namespace
+
+int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
+{
+    using mrpt::literals::operator""_deg;
+
+    try
+    {
+        // Ground-truth pose, and a *biased* prior shifted from it. The data
+        // covers the GT, so an unbalanced solver will ignore the prior; a
+        // properly balanced one will pull the estimate towards the prior.
+        const auto gtPose =
+            mrpt::poses::CPose3D::FromXYZYawPitchRoll(1.0, 0.0, 0.0, 0.0_deg, 0.0_deg, 0.0_deg);
+        const auto priorMean =
+            mrpt::poses::CPose3D::FromXYZYawPitchRoll(1.5, 0.0, 0.0, 0.0_deg, 0.0_deg, 0.0_deg);
+
+        // Many pairings, so the data block is O(N) larger than the prior.
+        const std::size_t N        = 400;
+        const auto        pairings = makeCov2CovPairings(gtPose, N);
+
+        // Reasonably tight prior (info = 1e4 → σ ≈ 1 cm).
+        mrpt::poses::CPose3DPDFGaussianInf prior;
+        prior.mean = priorMean;
+        prior.cov_inv.setZero();
+        for (int i = 0; i < 6; i++) prior.cov_inv(i, i) = 1e4;
+
+        const auto initPose = priorMean;
+
+        // Case A: balancing DISABLED (alpha=1, auto-balance off) → data wins.
+        mrpt::poses::CPose3D poseNoBalance;
+        {
+            mp2p_icp::OptimalTF_GN_Parameters gnParams;
+            gnParams.linearizationPoint              = initPose;
+            gnParams.prior                           = prior;
+            gnParams.cov2cov_alpha                   = 1.0;
+            gnParams.cov2cov_auto_balance_with_prior = false;
+            gnParams.maxInnerLoopIterations          = 20;
+
+            mp2p_icp::OptimalTF_Result result;
+            ASSERT_(mp2p_icp::optimal_tf_gauss_newton(pairings, result, gnParams));
+            poseNoBalance = result.optimalPose;
+            std::cout << "[no-balance] pose: " << poseNoBalance << "\n";
+        }
+
+        // Case B: auto-balance ENABLED (default) → prior is respected.
+        mrpt::poses::CPose3D poseBalanced;
+        {
+            mp2p_icp::OptimalTF_GN_Parameters gnParams;  // defaults: balance ON
+            gnParams.linearizationPoint     = initPose;
+            gnParams.prior                  = prior;
+            gnParams.maxInnerLoopIterations = 20;
+
+            mp2p_icp::OptimalTF_Result result;
+            ASSERT_(mp2p_icp::optimal_tf_gauss_newton(pairings, result, gnParams));
+            poseBalanced = result.optimalPose;
+            std::cout << "[balanced ] pose: " << poseBalanced << "\n";
+        }
+
+        const double dxNoBalance = std::abs(poseNoBalance.x() - priorMean.x());
+        const double dxBalanced  = std::abs(poseBalanced.x() - priorMean.x());
+        std::cout << "|x - x_prior|  no-balance=" << dxNoBalance << "  balanced=" << dxBalanced
+                  << "\n";
+
+        // Without balancing, the solver should snap to the data (≈ gtPose).
+        ASSERT_LT_(std::abs(poseNoBalance.x() - gtPose.x()), 0.05);
+
+        // With balancing, the result must be measurably pulled back toward
+        // the prior compared with the unbalanced case.
+        ASSERT_LT_(dxBalanced, dxNoBalance - 0.05);
+
+        // And it must remain a sensible compromise between prior and data,
+        // i.e. located between them along x (within tolerances).
+        ASSERT_GT_(poseBalanced.x(), gtPose.x() - 0.05);
+        ASSERT_LT_(poseBalanced.x(), priorMean.x() + 0.05);
+
+        // Sanity: with α=1 and balancing off, alpha=1/N should also recover a
+        // prior-respecting result (manual generalized-Bayes path).
+        {
+            mp2p_icp::OptimalTF_GN_Parameters gnParams;
+            gnParams.linearizationPoint              = initPose;
+            gnParams.prior                           = prior;
+            gnParams.cov2cov_alpha                   = 1.0 / static_cast<double>(N);
+            gnParams.cov2cov_auto_balance_with_prior = false;
+            gnParams.maxInnerLoopIterations          = 20;
+
+            mp2p_icp::OptimalTF_Result result;
+            ASSERT_(mp2p_icp::optimal_tf_gauss_newton(pairings, result, gnParams));
+            std::cout << "[alpha=1/N] pose: " << result.optimalPose << "\n";
+            ASSERT_LT_(std::abs(result.optimalPose.x() - priorMean.x()), dxNoBalance - 0.05);
+        }
+
+        // No-prior regression: with no prior, balancing must be inert (κ
+        // multiplier disabled), so the original cov2cov-only test setup
+        // still converges to GT.
+        {
+            const auto initBad = mrpt::poses::CPose3D::FromXYZYawPitchRoll(
+                0.0, 0.2, 0.1, 2.0_deg, -2.0_deg, -3.0_deg);
+
+            mp2p_icp::OptimalTF_GN_Parameters gnParams;  // default balance ON
+            gnParams.linearizationPoint     = initBad;
+            gnParams.maxInnerLoopIterations = 20;
+
+            mp2p_icp::OptimalTF_Result result;
+            ASSERT_(mp2p_icp::optimal_tf_gauss_newton(pairings, result, gnParams));
+            const auto poseError = gtPose - result.optimalPose;
+            ASSERT_LT_(poseError.translation().norm(), 0.05);
+            ASSERT_LT_(mrpt::poses::Lie::SO<3>::log(poseError.getRotationMatrix()).norm(), 0.05);
+        }
+    }
+    catch (std::exception& e)
+    {
+        std::cerr << mrpt::exception_to_str(e) << "\n";
+        return 1;
+    }
+}