New sanity check function: warn_on_field_padding_mismatch()

jlblancoc · jlblancoc · commit 59fd8edfa6de · 2026-04-09T12:46:27.000+02:00
diff --git a/mp2p_icp_filters/src/FilterBoundingBox.cpp b/mp2p_icp_filters/src/FilterBoundingBox.cpp
@@ -18,6 +18,7 @@
  * @date   Sep 10, 2021
  */
 
+#include <mp2p_icp/pointcloud_field_utils.h>
 #include <mp2p_icp_filters/FilterBoundingBox.h>
 #include <mp2p_icp_filters/GetOrCreatePointLayer.h>
 #include <mrpt/containers/yaml.h>
@@ -111,11 +112,13 @@ void FilterBoundingBox::filter(mp2p_icp::metric_map_t& inOut) const
     {
         insidePc->registerPointFieldsFrom(pc);
         ctxInside = insidePc->prepareForInsertPointsFrom(pc);
+        mp2p_icp::warn_on_field_padding_mismatch(pc, *insidePc, *this);
     }
     if (outsidePc)
     {
         outsidePc->registerPointFieldsFrom(pc);
         ctxOutside = outsidePc->prepareForInsertPointsFrom(pc);
+        mp2p_icp::warn_on_field_padding_mismatch(pc, *outsidePc, *this);
     }
 
     const auto& xs = pc.getPointsBufferRef_x();
diff --git a/mp2p_icp_filters/src/FilterByRange.cpp b/mp2p_icp_filters/src/FilterByRange.cpp
@@ -18,6 +18,7 @@
  * @date   Nov 14, 2023
  */
 
+#include <mp2p_icp/pointcloud_field_utils.h>
 #include <mp2p_icp/pointcloud_sanity_check.h>
 #include <mp2p_icp_filters/FilterByRange.h>
 #include <mp2p_icp_filters/GetOrCreatePointLayer.h>
@@ -122,11 +123,13 @@ void FilterByRange::filter(mp2p_icp::metric_map_t& inOut) const
     {
         outBetween->registerPointFieldsFrom(pc);
         ctxBetween = outBetween->prepareForInsertPointsFrom(pc);
+        mp2p_icp::warn_on_field_padding_mismatch(pc, *outBetween, *this);
     }
     if (outOutside)
     {
         outOutside->registerPointFieldsFrom(pc);
         ctxOutside = outOutside->prepareForInsertPointsFrom(pc);
+        mp2p_icp::warn_on_field_padding_mismatch(pc, *outOutside, *this);
     }
 
     for (size_t i = 0; i < xs.size(); i++)
diff --git a/mp2p_icp_filters/src/FilterDecimate.cpp b/mp2p_icp_filters/src/FilterDecimate.cpp
@@ -18,6 +18,7 @@
  * @date   Jan 12, 2026
  */
 
+#include <mp2p_icp/pointcloud_field_utils.h>
 #include <mp2p_icp_filters/FilterDecimate.h>
 #include <mp2p_icp_filters/GetOrCreatePointLayer.h>
 #include <mrpt/containers/yaml.h>
@@ -82,21 +83,14 @@ void FilterDecimate::filter(mp2p_icp::metric_map_t& inOut) const
         pcIn->GetRuntimeClass()->className);
 
     // Efficiently copy points including all extra fields
-#if MRPT_VERSION >= 0x020f00
     outPc->registerPointFieldsFrom(*pcIn);
     auto ctx = outPc->prepareForInsertPointsFrom(*pcIn);
-#endif
+    mp2p_icp::warn_on_field_padding_mismatch(*pcIn, *outPc, *this);
 
     outPc->reserve(outPc->size() + (nIn / N));
 
     for (size_t i = 0; i < nIn; i += N)
     {
-#if MRPT_VERSION >= 0x020f03
         outPc->insertPointFrom(i, ctx);
-#elif MRPT_VERSION >= 0x020f00
-        outPc->insertPointFrom(*pcIn, i, ctx);
-#else
-        outPc->insertPointFrom(*pcIn, i);
-#endif
     }
 }
diff --git a/mp2p_icp_filters/src/FilterDecimateAdaptive.cpp b/mp2p_icp_filters/src/FilterDecimateAdaptive.cpp
@@ -18,6 +18,7 @@
  * @date   Nov 24, 2023
  */
 
+#include <mp2p_icp/pointcloud_field_utils.h>
 #include <mp2p_icp_filters/FilterDecimateAdaptive.h>
 #include <mp2p_icp_filters/GetOrCreatePointLayer.h>
 #include <mrpt/containers/yaml.h>
@@ -183,6 +184,7 @@ void FilterDecimateAdaptive::filter(mp2p_icp::metric_map_t& inOut) const
 
     outPc->registerPointFieldsFrom(pc);
     mrpt::maps::CPointsMap::InsertCtx ctx = outPc->prepareForInsertPointsFrom(pc);
+    mp2p_icp::warn_on_field_padding_mismatch(pc, *outPc, *this);
 
     // Perform resampling:
     // -------------------
diff --git a/mp2p_icp_filters/src/FilterDecimateVoxels.cpp b/mp2p_icp_filters/src/FilterDecimateVoxels.cpp
@@ -18,6 +18,7 @@
  * @date   Sep 10, 2021
  */
 
+#include <mp2p_icp/pointcloud_field_utils.h>
 #include <mp2p_icp/pointcloud_sanity_check.h>
 #include <mp2p_icp_filters/FilterDecimateVoxels.h>
 #include <mp2p_icp_filters/GetOrCreatePointLayer.h>
@@ -172,6 +173,7 @@ void FilterDecimateVoxels::filter(mp2p_icp::metric_map_t& inOut) const
             outPc->registerPointFieldsFrom(*pcPtrs[mapIdx]);
             mrpt::maps::CPointsMap::InsertCtx ctxOut =
                 outPc->prepareForInsertPointsFrom(*pcPtrs[mapIdx]);
+            mp2p_icp::warn_on_field_padding_mismatch(*pcPtrs[mapIdx], *outPc, *this);
 
             for (size_t i = 0; i < xs.size(); i++)
             {
diff --git a/mp2p_icp_filters/src/FilterDeskew.cpp b/mp2p_icp_filters/src/FilterDeskew.cpp
@@ -24,6 +24,7 @@
 #include <mola_imu_preintegration/trajectory_from_buffer.h>
 #endif
 
+#include <mp2p_icp/pointcloud_field_utils.h>
 #include <mp2p_icp_filters/FilterDeskew.h>
 #include <mp2p_icp_filters/GetOrCreatePointLayer.h>
 #include <mrpt/containers/yaml.h>
@@ -430,6 +431,7 @@ void FilterDeskew::filter(mp2p_icp::metric_map_t& inOut) const
 
         // and then, prepare structures for fast copying:
         insert_ctx = outPc->prepareForInsertPointsFrom(*inPc);
+        mp2p_icp::warn_on_field_padding_mismatch(*inPc, *outPc, *this);
     }
 
     // If the input is empty, just move on:
diff --git a/mp2p_icp_filters/src/FilterMLS.cpp b/mp2p_icp_filters/src/FilterMLS.cpp
@@ -19,6 +19,8 @@
  * @date   Oct 26, 2025
  */
 
+#include <mp2p_icp/pointcloud_field_utils.h>
+#include <mp2p_icp/pointcloud_sanity_check.h>
 #include <mp2p_icp_filters/FilterMLS.h>
 #include <mp2p_icp_filters/GetOrCreatePointLayer.h>
 #include <mrpt/containers/NonCopiableData.h>
@@ -630,6 +632,7 @@ void FilterMLS::filter(mp2p_icp::metric_map_t& inOut) const
     ASSERT_(normals_z);
 
     const auto ctx = outPc->prepareForInsertPointsFrom(*query_pc);
+    mp2p_icp::warn_on_field_padding_mismatch(*query_pc, *outPc, *this);
 
     const size_t firstIdx = outPc->size();
     outPc->reserve(firstIdx + nNewPoints);
diff --git a/mp2p_icp_filters/src/FilterSOR.cpp b/mp2p_icp_filters/src/FilterSOR.cpp
@@ -19,6 +19,8 @@
  * @date   Dec 28, 2025
  */
 
+#include <mp2p_icp/pointcloud_field_utils.h>
+#include <mp2p_icp/pointcloud_sanity_check.h>
 #include <mp2p_icp_filters/FilterSOR.h>
 #include <mp2p_icp_filters/GetOrCreatePointLayer.h>
 #include <mrpt/containers/yaml.h>
@@ -87,11 +89,13 @@ void FilterSOR::filter(mp2p_icp::metric_map_t& inOut) const
     {
         outInliers->registerPointFieldsFrom(pc);
         ctxI = outInliers->prepareForInsertPointsFrom(pc);
+        mp2p_icp::warn_on_field_padding_mismatch(pc, *outInliers, *this);
     }
     if (outOutliers)
     {
         outOutliers->registerPointFieldsFrom(pc);
         ctxO = outOutliers->prepareForInsertPointsFrom(pc);
+        mp2p_icp::warn_on_field_padding_mismatch(pc, *outOutliers, *this);
     }
 
     if (pcPtr->empty())
diff --git a/mp2p_icp_filters/src/FilterVoxelSOR.cpp b/mp2p_icp_filters/src/FilterVoxelSOR.cpp
@@ -19,6 +19,8 @@
  * @date   Jan 21, 2026
  */
 
+#include <mp2p_icp/pointcloud_field_utils.h>
+#include <mp2p_icp/pointcloud_sanity_check.h>
 #include <mp2p_icp_filters/FilterVoxelSOR.h>
 #include <mp2p_icp_filters/GetOrCreatePointLayer.h>
 #include <mp2p_icp_filters/PointCloudToVoxelGrid.h>
@@ -92,16 +94,17 @@ void FilterVoxelSOR::filter(mp2p_icp::metric_map_t& inOut) const
         outOutliers->clear();
     }
 
-#if MRPT_VERSION >= 0x020f00
     outInliers->registerPointFieldsFrom(*pcIn);
     auto ctxI = outInliers->prepareForInsertPointsFrom(*pcIn);
+    mp2p_icp::warn_on_field_padding_mismatch(*pcIn, *outInliers, *this);
+
     std::optional<mrpt::maps::CPointsMap::InsertCtx> ctxO;
     if (outOutliers)
     {
         outOutliers->registerPointFieldsFrom(*pcIn);
         ctxO = outOutliers->prepareForInsertPointsFrom(*pcIn);
+        mp2p_icp::warn_on_field_padding_mismatch(pc, *outOutliers, *this);
     }
-#endif
 
     const auto& xs = pcIn->getPointsBufferRef_x();
     const auto& ys = pcIn->getPointsBufferRef_y();
diff --git a/mp2p_icp_map/CMakeLists.txt b/mp2p_icp_map/CMakeLists.txt
@@ -11,6 +11,7 @@ set(LIB_SRCS
   src/NearestPlaneCapable.cpp
   src/NearestPointWithCovCapable.cpp
   src/point_with_cov_pair_t.cpp
+  src/pointcloud_field_utils.cpp
   src/pointcloud_sanity_check.cpp
   src/update_velocity_buffer_from_obs.cpp
   #
@@ -30,6 +31,7 @@ set(LIB_PUBLIC_HDRS
   include/mp2p_icp/point_plane_pair_t.h
   include/mp2p_icp/point_with_cov_pair_t.h
   include/mp2p_icp/pointcloud_bitfield.h
+  include/mp2p_icp/pointcloud_field_utils.h
   include/mp2p_icp/pointcloud_sanity_check.h
   include/mp2p_icp/render_params.h
   include/mp2p_icp/update_velocity_buffer_from_obs.h
diff --git a/mp2p_icp_map/include/mp2p_icp/pointcloud_field_utils.h b/mp2p_icp_map/include/mp2p_icp/pointcloud_field_utils.h
@@ -0,0 +1,54 @@
+/*               _
+ _ __ ___   ___ | | __ _
+| '_ ` _ \ / _ \| |/ _` | 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   pointcloud_field_utils.h
+ * @brief  Raises warnings on missing fields that would be filled with zeros.
+ * @author Jose Luis Blanco Claraco
+ * @date   Apr 9, 2026
+ */
+
+#pragma once
+
+#include <mrpt/maps/CPointsMap.h>
+#include <mrpt/system/COutputLogger.h>
+
+#include <string>
+#include <vector>
+
+namespace mp2p_icp
+{
+
+/** Checks whether copying points from \a src into \a dst via
+ *  CPointsMap::prepareForInsertPointsFrom() + insertPointFrom() would result
+ *  in any destination field being zero-padded (i.e. the field exists in \a dst
+ *  but is absent in \a src).
+ *
+ *  This situation is benign on MRPT < 2.15.13 (those fields were simply
+ *  skipped, which caused sanity-check crashes), and is
+ *  handled correctly from MRPT 2.15.13 onwards by writing explicit zeros.
+ *  However, silent zero-padding may still indicate a configuration problem,
+ *  or sensor drops.
+ *
+ *  One WARN-level message is emitted per mismatched field name.
+ *
+ *  \param src  Source point cloud (the one being inserted FROM).
+ *  \param dst  Destination point cloud (the one being inserted INTO).
+ *  \param logger  Logger used for WARN messages.
+ *  \return True if at least one mismatch was found.
+ */
+bool warn_on_field_padding_mismatch(
+    const mrpt::maps::CPointsMap& src, const mrpt::maps::CPointsMap& dst,
+    const mrpt::system::COutputLogger& logger);
+
+}  // namespace mp2p_icp
diff --git a/mp2p_icp_map/src/pointcloud_field_utils.cpp b/mp2p_icp_map/src/pointcloud_field_utils.cpp
@@ -0,0 +1,92 @@
+/*               _
+ _ __ ___   ___ | | __ _
+| '_ ` _ \ / _ \| |/ _` | 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   pointcloud_field_utils.cpp
+ * @brief  Raises warnings on missing fields that would be filled with zeros.
+ * @author Jose Luis Blanco Claraco
+ * @date   Apr 9, 2026
+ */
+
+#include <mp2p_icp/pointcloud_field_utils.h>
+
+bool mp2p_icp::warn_on_field_padding_mismatch(
+    const mrpt::maps::CPointsMap& src, const mrpt::maps::CPointsMap& dst,
+    const mrpt::system::COutputLogger& logger)
+{
+    bool any = false;
+
+    // Helper: check one family of fields.
+    // dst_names  – fields registered in dst for this type
+    // src_has_fn – returns true if src carries a non-empty buffer for the name
+    auto check =
+        [&](const std::vector<std::string>& dst_names, auto src_has_fn, const char* type_label)
+    {
+        for (const auto& name : dst_names)
+        {
+            if (!src_has_fn(name))
+            {
+                logger.logFmt(
+                    mrpt::system::LVL_WARN,
+                    "warn_on_field_padding_mismatch: destination map has field '%s' (%s) but "
+                    "source map does not: points inserted from source will be "
+                    "zero-padded for this field.",
+                    name.c_str(), type_label);
+                any = true;
+            }
+        }
+    };
+
+    // float fields (excluding x/y/z which are always present)
+    check(
+        dst.getPointFieldNames_float_except_xyz(),
+        [&](const std::string& name) -> bool
+        {
+            const auto* v = src.getPointsBufferRef_float_field(name);
+            return v && !v->empty();
+        },
+        "float");
+
+    // double fields
+    check(
+        dst.getPointFieldNames_double(),
+        [&](const std::string& name) -> bool
+        {
+            const auto* v = src.getPointsBufferRef_double_field(name);
+            return v && !v->empty();
+        },
+        "double");
+
+    // uint16 fields
+    check(
+        dst.getPointFieldNames_uint16(),
+        [&](const std::string& name) -> bool
+        {
+            const auto* v = src.getPointsBufferRef_uint16_field(name);
+            return v && !v->empty();
+        },
+        "uint16");
+
+    // uint8 fields
+    check(
+        dst.getPointFieldNames_uint8(),
+        [&](const std::string& name) -> bool
+        {
+            const auto* v = src.getPointsBufferRef_uint8_field(name);
+            return v && !v->empty();
+        },
+        "uint8");
+
+    return any;
+}

Original file line number	Diff line number	Diff line change
`@@ -18,6 +18,7 @@`
`18`	`18`	`* @date Sep 10, 2021`
`19`	`19`	`*/`
`20`	`20`
	`21`	`+#include <mp2p_icp/pointcloud_field_utils.h>`
`21`	`22`	`#include <mp2p_icp_filters/FilterBoundingBox.h>`
`22`	`23`	`#include <mp2p_icp_filters/GetOrCreatePointLayer.h>`
`23`	`24`	`#include <mrpt/containers/yaml.h>`
`@@ -111,11 +112,13 @@ void FilterBoundingBox::filter(mp2p_icp::metric_map_t& inOut) const`
`111`	`112`	`{`
`112`	`113`	`insidePc->registerPointFieldsFrom(pc);`
`113`	`114`	`ctxInside = insidePc->prepareForInsertPointsFrom(pc);`
	`115`	`+ mp2p_icp::warn_on_field_padding_mismatch(pc, insidePc, this);`
`114`	`116`	`}`
`115`	`117`	`if (outsidePc)`
`116`	`118`	`{`
`117`	`119`	`outsidePc->registerPointFieldsFrom(pc);`
`118`	`120`	`ctxOutside = outsidePc->prepareForInsertPointsFrom(pc);`
	`121`	`+ mp2p_icp::warn_on_field_padding_mismatch(pc, outsidePc, this);`
`119`	`122`	`}`
`120`	`123`
`121`	`124`	`const auto& xs = pc.getPointsBufferRef_x();`
Original file line number	Diff line number	Diff line change
`@@ -19,6 +19,8 @@`
`19`	`19`	`* @date Dec 28, 2025`
`20`	`20`	`*/`
`21`	`21`
	`22`	`+#include <mp2p_icp/pointcloud_field_utils.h>`
	`23`	`+#include <mp2p_icp/pointcloud_sanity_check.h>`
`22`	`24`	`#include <mp2p_icp_filters/FilterSOR.h>`
`23`	`25`	`#include <mp2p_icp_filters/GetOrCreatePointLayer.h>`
`24`	`26`	`#include <mrpt/containers/yaml.h>`
`@@ -87,11 +89,13 @@ void FilterSOR::filter(mp2p_icp::metric_map_t& inOut) const`
`87`	`89`	`{`
`88`	`90`	`outInliers->registerPointFieldsFrom(pc);`
`89`	`91`	`ctxI = outInliers->prepareForInsertPointsFrom(pc);`
	`92`	`+ mp2p_icp::warn_on_field_padding_mismatch(pc, outInliers, this);`
`90`	`93`	`}`
`91`	`94`	`if (outOutliers)`
`92`	`95`	`{`
`93`	`96`	`outOutliers->registerPointFieldsFrom(pc);`
`94`	`97`	`ctxO = outOutliers->prepareForInsertPointsFrom(pc);`
	`98`	`+ mp2p_icp::warn_on_field_padding_mismatch(pc, outOutliers, this);`
`95`	`99`	`}`
`96`	`100`
`97`	`101`	`if (pcPtr->empty())`