Feat: Censi3D covariance method

mp2p_icp/include/mp2p_icp/Parameters.h

@@ -15,6 +15,7 @@
 #pragma once
 
 #include <mp2p_icp/WeightParameters.h>
+#include <mp2p_icp/covariance.h>
 #include <mrpt/containers/yaml.h>
 #include <mrpt/core/bits_math.h>  // DEG2RAD()
 #include <mrpt/serialization/CSerializable.h>
@@ -105,6 +106,11 @@ struct Parameters : public mrpt::serialization::CSerializable
 
     /** @} */
 
+    /** Parameters for the post-optimization SE(3) covariance estimation
+     *  (see mp2p_icp::covariance). Loadable from the optional YAML map
+     *  block `covariance:` in the ICP pipeline file. */
+    CovarianceParameters covariance_params;
+
     void load_from(const mrpt::containers::yaml& p);
     void save_to(mrpt::containers::yaml& p) const;
 };

mp2p_icp/include/mp2p_icp/covariance.h

@@ -21,14 +21,59 @@
 
 #include <mp2p_icp/Pairings.h>
 #include <mrpt/math/CMatrixFixed.h>
+#include <mrpt/typemeta/TEnumType.h>
+
+#include <cstdint>
 
 namespace mp2p_icp
 {
 struct CovarianceParameters
 {
-    // Finite difference deltas:
+    /** Covariance estimation backend. */
+    enum class Method : uint8_t
+    {
+        /** Inverse of the (whitened) Gauss-Newton Hessian, scaled by the
+         *  a-posteriori unit-weight variance chi^2/(m-n). Cheap but typically
+         *  optimistic, since it ignores correspondence and surface-mismatch
+         *  noise. */
+        InverseHessian,
+
+        /** Censi-style sandwich  H^{-1} M H^{-1}, with H = sum J_i^T J_i and
+         *  M = sum J_i^T Sigma_z_i J_i. For cov2cov pairings the per-pair
+         *  Sigma_z = inv(cov_inv) is used directly; for pt2pt without
+         *  per-pair noise, an isotropic Sigma_z = defaultPointSigma^2 * I
+         *  is assumed. Reference: Censi 2007, Prakhya 2015. */
+        Censi3D,
+    };
+
+    Method method = Method::InverseHessian;
+
+    /** For pt2pt pairings without per-pair Sigma in Censi3D path:
+     *  isotropic point-noise std-dev [m]. */
+    double defaultPointSigma = 0.02;
+
+    /** Per-axis floor added to the resulting covariance diagonal. Stored as
+     *  std-dev for readability; squared internally. Useful to absorb
+     *  unmodelled errors and keep downstream filters numerically stable. */
+    double floor_sigma_xyz    = 0.0;  ///< [m]
+    double floor_sigma_angles = 0.0;  ///< [rad]
+
+    // Finite difference deltas (only used by InverseHessian path):
     double finDif_xyz    = 1e-7;
     double finDif_angles = 1e-7;
+
+    /** Loads the parameters from a YAML map node. All entries are optional;
+     *  unspecified fields keep their default values. Recognized keys:
+     *
+     *    method:               "InverseHessian" | "Censi3D"
+     *    defaultPointSigma:    [m]
+     *    floor_sigma_xyz:      [m]
+     *    floor_sigma_angles:   [rad]   (use floor_sigma_angles_deg for degrees)
+     *    floor_sigma_angles_deg: [deg] (alternative to the radian form)
+     *    finDif_xyz, finDif_angles
+     */
+    void load_from(const mrpt::containers::yaml& p);
+    void save_to(mrpt::containers::yaml& p) const;
 };
 
 /** Covariance estimation methods for an ICP result.
@@ -40,3 +85,10 @@ mrpt::math::CMatrixDouble66 covariance(
     const CovarianceParameters& p);
 
 }  // namespace mp2p_icp
+
+MRPT_ENUM_TYPE_BEGIN_NAMESPACE(mp2p_icp, mp2p_icp::CovarianceParameters::Method)
+MRPT_FILL_ENUM(CovarianceParameters::Method::InverseHessian);
+MRPT_FILL_ENUM_CUSTOM_NAME(CovarianceParameters::Method::InverseHessian, "InverseHessian");
+MRPT_FILL_ENUM(CovarianceParameters::Method::Censi3D);
+MRPT_FILL_ENUM_CUSTOM_NAME(CovarianceParameters::Method::Censi3D, "Censi3D");
+MRPT_ENUM_TYPE_END()

mp2p_icp/src/ICP.cpp

@@ -397,11 +397,13 @@ void ICP::align(
     result.optimalScale    = state.currentSolution.optimalScale;
     result.finalPairings   = std::move(state.currentPairings);
 
-    // Covariance:
-    mp2p_icp::CovarianceParameters covParams;
+    // Covariance of the estimated SE(3) registration:
+    {
+        mrpt::system::CTimeLoggerEntry tleCov(profiler_, "align.5b_covariance");
 
-    result.optimal_tf.cov =
-        mp2p_icp::covariance(result.finalPairings, result.optimal_tf.mean, covParams);
+        result.optimal_tf.cov =
+            mp2p_icp::covariance(result.finalPairings, result.optimal_tf.mean, p.covariance_params);
+    }
 
     // Clean-up global map icp preparation:
     for (const auto& [layer, mmap] : pcGlobal.layers)

mp2p_icp/src/Parameters.cpp

@@ -27,14 +27,18 @@ const bool MP2P_ICP_GENERATE_DEBUG_FILES =
     mrpt::get_env<bool>("MP2P_ICP_GENERATE_DEBUG_FILES", false);
 
 // Implementation of the CSerializable virtual interface:
-uint8_t Parameters::serializeGetVersion() const { return 2; }
+uint8_t Parameters::serializeGetVersion() const { return 3; }
 void    Parameters::serializeTo(mrpt::serialization::CArchive& out) const
 {
     out << maxIterations << minAbsStep_trans << minAbsStep_rot;
     out << generateDebugFiles << debugFileNameFormat;
     out << debugPrintIterationProgress;
     out << decimationDebugFiles;
     out << saveIterationDetails << decimationIterationDetails;  // v2
+    const uint8_t covMethod = static_cast<uint8_t>(covariance_params.method);
+    out << covMethod << covariance_params.defaultPointSigma << covariance_params.floor_sigma_xyz
+        << covariance_params.floor_sigma_angles << covariance_params.finDif_xyz
+        << covariance_params.finDif_angles;  // v3
 }
 void Parameters::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
 {
@@ -45,12 +49,27 @@ void Parameters::serializeFrom(mrpt::serialization::CArchive& in, uint8_t versio
         case 0:
         case 1:
         case 2:
+        case 3:
         {
             in >> maxIterations >> minAbsStep_trans >> minAbsStep_rot;
             in >> generateDebugFiles >> debugFileNameFormat;
             in >> debugPrintIterationProgress;
-            if (version >= 1) in >> decimationDebugFiles;
-            if (version >= 2) in >> saveIterationDetails >> decimationIterationDetails;
+            if (version >= 1)
+            {
+                in >> decimationDebugFiles;
+            }
+            if (version >= 2)
+            {
+                in >> saveIterationDetails >> decimationIterationDetails;
+            }
+            if (version >= 3)
+            {
+                uint8_t covMethod = 0;
+                in >> covMethod >> covariance_params.defaultPointSigma >>
+                    covariance_params.floor_sigma_xyz >> covariance_params.floor_sigma_angles >>
+                    covariance_params.finDif_xyz >> covariance_params.finDif_angles;
+                covariance_params.method = static_cast<CovarianceParameters::Method>(covMethod);
+            }
         }
         break;
         default:
@@ -92,6 +111,11 @@ void Parameters::load_from(const mrpt::containers::yaml& p)
         }
     }
 
+    if (p.has("covariance"))
+    {
+        covariance_params.load_from(p["covariance"]);
+    }
+
     generateDebugFiles = generateDebugFiles || MP2P_ICP_GENERATE_DEBUG_FILES;
 }
 
@@ -106,4 +130,8 @@ void Parameters::save_to(mrpt::containers::yaml& p) const
     MCP_SAVE(p, decimationDebugFiles);
     MCP_SAVE(p, saveIterationDetails);
     MCP_SAVE(p, decimationIterationDetails);
+
+    mrpt::containers::yaml covYaml = mrpt::containers::yaml::Map();
+    covariance_params.save_to(covYaml);
+    p["covariance"] = covYaml;
 }