start deskew refactoring for point fields

.github/workflows/ros-build.yml

@@ -38,9 +38,9 @@ jobs:
             use_ros2-testing: true
 
           # Rolling Ridley (No End-Of-Life)
-          - docker_image: ubuntu:noble
-            ros_distribution: rolling
-            use_ros2-testing: false
+          #- docker_image: ubuntu:noble
+          #  ros_distribution: rolling
+          #  use_ros2-testing: false
 
           - docker_image: ubuntu:noble
             ros_distribution: rolling
@@ -52,10 +52,10 @@ jobs:
             use_ros2-testing: true
             disable_tbb: true
 
-          - docker_image: ubuntu:noble
-            ros_distribution: rolling
-            use_ros2-testing: true
-            coverage_run: true
+          #- docker_image: ubuntu:noble
+          #  ros_distribution: rolling
+          #  use_ros2-testing: true
+          #  coverage_run: true
 
     container:
       image: ${{ matrix.docker_image }}

.vscode/settings.json

@@ -46,5 +46,8 @@
         "voxelizes",
         "voxelmaps",
         "XYZIRT"
+    ],
+    "cSpell.ignoreWords": [
+        "Bonxai"
     ]
 }

mp2p_icp/src/QualityEvaluator_Voxels.cpp

@@ -65,30 +65,38 @@ QualityEvaluator::Result QualityEvaluator_Voxels::evaluate(
     // Take both voxel maps from each map:
     // ------------------------------------
     if (pcGlobal.layers.count(voxel_layer_name) == 0)
+    {
         THROW_EXCEPTION_FMT(
             "Input global map was expected to contain a layer named '%s'",
             voxel_layer_name.c_str());
+    }
 
     if (pcLocal.layers.count(voxel_layer_name) == 0)
+    {
         THROW_EXCEPTION_FMT(
             "Input local map was expected to contain a layer named '%s'", voxel_layer_name.c_str());
+    }
 
     const mrpt::maps::CVoxelMap::Ptr globalVoxels =
         std::dynamic_pointer_cast<mrpt::maps::CVoxelMap>(pcGlobal.layers.at(voxel_layer_name));
     if (!globalVoxels)
+    {
         THROW_EXCEPTION_FMT(
             "Input global map was expected to contain a layer named '%s' of "
             "type 'CVoxelMap'",
             voxel_layer_name.c_str());
+    }
 
     const mrpt::maps::CVoxelMap::Ptr localVoxels =
         std::dynamic_pointer_cast<mrpt::maps::CVoxelMap>(pcLocal.layers.at(voxel_layer_name));
 
     if (!localVoxels)
+    {
         THROW_EXCEPTION_FMT(
             "Input local map was expected to contain a layer named '%s' of "
             "type 'CVoxelMap'",
             voxel_layer_name.c_str());
+    }
 
     // Compare them:
     // ----------------------------------
@@ -115,13 +123,19 @@ QualityEvaluator::Result QualityEvaluator_Voxels::evaluate(
 
         auto* cell = gAccessor.value(
             Bonxai::PosToCoord({ptGlobal.x, ptGlobal.y, ptGlobal.z}, g.inv_resolution));
-        if (!cell) return;  // cell not observed in global grid
+        if (!cell)
+        {
+            return;  // cell not observed in global grid
+        }
 
         const float localOcc  = localVoxels->l2p(data.occupancy);
         const float globalOcc = globalVoxels->l2p(cell->occupancy);
 
         // barely observed cells?
-        if (std::abs(globalOcc - 0.5f) < 0.01f || std::abs(localOcc - 0.5f) < 0.01f) return;
+        if (std::abs(globalOcc - 0.5f) < 0.01f || std::abs(localOcc - 0.5f) < 0.01f)
+        {
+            return;
+        }
 
         const double d = loss(localOcc, globalOcc);
         dist += d;
@@ -140,13 +154,19 @@ QualityEvaluator::Result QualityEvaluator_Voxels::evaluate(
 
         auto* cell = lAccessor.value(
             Bonxai::PosToCoord({ptLocal.x, ptLocal.y, ptLocal.z}, l.inv_resolution));
-        if (!cell) return;  // cell not observed in global grid
+        if (!cell)
+        {
+            return;  // cell not observed in global grid
+        }
 
         const float localOcc  = localVoxels->l2p(cell->occupancy);
         const float globalOcc = globalVoxels->l2p(data.occupancy);
 
         // barely observed cells?
-        if (std::abs(globalOcc - 0.5f) < 0.01f || std::abs(localOcc - 0.5f) < 0.01f) return;
+        if (std::abs(globalOcc - 0.5f) < 0.01f || std::abs(localOcc - 0.5f) < 0.01f)
+        {
+            return;
+        }
 
         const double d = loss(localOcc, globalOcc);
         dist += d;
@@ -160,7 +180,7 @@ QualityEvaluator::Result QualityEvaluator_Voxels::evaluate(
     r.quality = 0;
     if (dist_cells)
     {
-        dist /= dist_cells;
+        dist /= static_cast<double>(dist_cells);
         r.quality = 1.0 / (1.0 + std::exp(-dist2quality_scale * dist));
     }
     MRPT_LOG_DEBUG_STREAM(

mp2p_icp_filters/include/mp2p_icp_filters/FilterDeskew.h

@@ -57,14 +57,14 @@ enum class MotionCompensationMethod : uint8_t
  *  input pointcloud from a moving LIDAR, where points are time-stamped.
  *
  * Important notes:
- * - The `time` field of each point is assumed to be in seconds since the
+ * - The relative timestamp `t` field of each point is assumed to be in seconds since the
  *   reference time point, which can be the start or middle point of the scan. This can be
  *   controlled by adding a FilterAdjustTimestamps before this block.
  *
  * - The input layer must contain a point cloud in the format
- *   mrpt::maps::CPointsMapXYZIRT so timestamps are present.
+ *   mrpt::maps::CPointsMapXYZIRT or mrpt::maps::CGenericPointsMap so timestamps are present.
  *
- * - If the input layer is of a different type, or the `time` field is missing,
+ * - If the input layer is of a different type, or the `t` field is missing,
  *   an exception will be thrown by default, unless the option
  *   `silently_ignore_no_timestamps` is set to `true`, in which case the input
  *   cloud will be just moved forward to the output.

mp2p_icp_filters/src/FilterBoundingBox.cpp

@@ -132,7 +132,9 @@ void FilterBoundingBox::filter(mp2p_icp::metric_map_t& inOut) const
 #endif
         if (targetPc)
         {
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+            targetPc->insertPointFrom(i, *ctx);
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
             targetPc->insertPointFrom(pc, i, *ctx);
 #else
             targetPc->insertPointFrom(pc, i);

mp2p_icp_filters/src/FilterByIntensity.cpp

@@ -179,7 +179,9 @@ void FilterByIntensity::filter(mp2p_icp::metric_map_t& inOut) const
 
         if (trg)
         {
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+            trg->insertPointFrom(i, *ctx);
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
             trg->insertPointFrom(pc, i, *ctx);
 #else
             trg->insertPointFrom(pc, i);

mp2p_icp_filters/src/FilterByRange.cpp

@@ -149,7 +149,10 @@ void FilterByRange::filter(mp2p_icp::metric_map_t& inOut) const
 
         if (targetPc)
         {
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+            const auto& ctx = isInside ? ctxBetween : ctxOutside;
+            targetPc->insertPointFrom(i, ctx.value());
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
             const auto& ctx = isInside ? ctxBetween : ctxOutside;
             targetPc->insertPointFrom(pc, i, ctx.value());
 #else

mp2p_icp_filters/src/FilterByRing.cpp

@@ -166,7 +166,9 @@ void FilterByRing::filter(mp2p_icp::metric_map_t& inOut) const
 
         if (trg)
         {
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+            trg->insertPointFrom(i, *ctx);
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
             trg->insertPointFrom(pc, i, *ctx);
 #else
             trg->insertPointFrom(pc, i);

mp2p_icp_filters/src/FilterCurvature.cpp

@@ -227,7 +227,9 @@ void FilterCurvature::filter(mp2p_icp::metric_map_t& inOut) const
                 counterLarger++;
                 if (outPcLarger)
                 {
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+                    outPcLarger->insertPointFrom(i, ctxLarger);
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
                     outPcLarger->insertPointFrom(pc, i, ctxLarger);
 #else
                     outPcLarger->insertPointFrom(pc, i);
@@ -258,7 +260,9 @@ void FilterCurvature::filter(mp2p_icp::metric_map_t& inOut) const
                     counterLarger++;
                     if (outPcLarger)
                     {
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+                        outPcLarger->insertPointFrom(i, ctxLarger);
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
                         outPcLarger->insertPointFrom(pc, i, ctxLarger);
 #else
                         outPcLarger->insertPointFrom(pc, i);
@@ -269,7 +273,9 @@ void FilterCurvature::filter(mp2p_icp::metric_map_t& inOut) const
                 {
                     if (outPcOther)
                     {
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+                        outPcOther->insertPointFrom(i, ctxOther);
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
                         outPcOther->insertPointFrom(pc, i, ctxOther);
 #else
                         outPcOther->insertPointFrom(pc, i);
@@ -291,7 +297,9 @@ void FilterCurvature::filter(mp2p_icp::metric_map_t& inOut) const
                 counterLarger++;
                 if (outPcLarger)
                 {
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+                    outPcLarger->insertPointFrom(i, ctxLarger);
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
                     outPcLarger->insertPointFrom(pc, i, ctxLarger);
 #else
                     outPcLarger->insertPointFrom(pc, i);
@@ -303,7 +311,9 @@ void FilterCurvature::filter(mp2p_icp::metric_map_t& inOut) const
                 counterLess++;
                 if (outPcSmaller)
                 {
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+                    outPcSmaller->insertPointFrom(i, ctxSmaller);
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
                     outPcSmaller->insertPointFrom(pc, i, ctxSmaller);
 #else
                     outPcSmaller->insertPointFrom(pc, i);

mp2p_icp_filters/src/FilterDecimateAdaptive.cpp

@@ -221,7 +221,10 @@ void FilterDecimateAdaptive::filter(mp2p_icp::metric_map_t& inOut) const
         if (!ith.exhausted)
         {
             auto ptIdx = ith.voxel->indices[ith.nextIdx++];
-#if MRPT_VERSION >= 0x020f00  // 2.15.0
+
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+            outPc->insertPointFrom(ptIdx, ctx);
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
             outPc->insertPointFrom(pc, ptIdx, ctx);
 #else
             outPc->insertPointFrom(pc, ptIdx);