More unit tests

README.md

@@ -40,6 +40,7 @@ And these applications:
  * [mm-filter](apps/mm-filter): CLI tool to apply a pipeline to an input metric map (`*.mm`), saving the result as another metric map file.
  * [mm-info](apps/mm-info): CLI tool to read a metric map (`*.mm`) and describe its contents.
  * [mm-viewer](apps/mm-viewer): GUI tool to visualize .mm (metric map) files.
+ * [mm2ply](apps/mm2ply): CLI tool to export the layers of a metric map (`*.mm`) in PLY format.
  * [mm2txt](apps/mm2txt): CLI tool to export the layers of a metric map (`*.mm`) as CSV/TXT.
  * [icp-log-viewer](apps/icp-log-viewer): GUI to inspect results from ICP runs.
  * [icp-run](apps/icp-run): Standalone program to run ICP pipelines.

mp2p_icp/include/mp2p_icp/Parameters.h

@@ -84,8 +84,7 @@ struct Parameters : public mrpt::serialization::CSerializable
 
     /** Generated files format, if generateDebugFiles is true. */
     std::string debugFileNameFormat =
-        "icp-run-$UNIQUE_ID-local-$LOCAL_ID$LOCAL_LABEL-"
-        "global-$GLOBAL_ID$GLOBAL_LABEL.icplog";
+        "icp-run-$UNIQUE_ID-local-$LOCAL_ID$LOCAL_LABEL-global-$GLOBAL_ID$GLOBAL_LABEL.icplog";
 
     /** Function to apply to the local and global maps before saving the map to
      * a log file. Useful to apply deletion filters to save space and time.

mp2p_icp/src/icp_pipeline_from_yaml.cpp

@@ -55,10 +55,16 @@ std::tuple<mp2p_icp::ICP::Ptr, mp2p_icp::Parameters> mp2p_icp::icp_pipeline_from
     }
 
     // ICP solver class:
-    if (icpParams.has("solvers")) icp->initialize_solvers(icpParams["solvers"]);
+    if (icpParams.has("solvers"))
+    {
+        icp->initialize_solvers(icpParams["solvers"]);
+    }
 
     // ICP matchers class:
-    if (icpParams.has("matchers")) icp->initialize_matchers(icpParams["matchers"]);
+    if (icpParams.has("matchers"))
+    {
+        icp->initialize_matchers(icpParams["matchers"]);
+    }
 
     // ICP quality class:
     ASSERT_(icpParams.has("quality"));

tests/CMakeLists.txt

@@ -64,6 +64,7 @@ mp2p_add_test(mp2p_FilterDecimateVoxels EXTRA_LIBS mp2p_icp_filters)
 mp2p_add_test(mp2p_Filters EXTRA_LIBS mp2p_icp_filters)
 mp2p_add_test(mp2p_FilterByExpression EXTRA_LIBS mp2p_icp_filters)
 mp2p_add_test(mp2p_FilterSOR EXTRA_LIBS mp2p_icp_filters)
+mp2p_add_test(mp2p_generator EXTRA_LIBS mp2p_icp_filters)
 
 if (mola_test_datasets_FOUND)
   mp2p_add_test(mp2p_quality_voxels)

tests/test-mp2p_generator.cpp

@@ -0,0 +1,112 @@
+/*               _
+ _ __ ___   ___ | | __ _
+| '_ ` _ \ / _ \| |/ _` | 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-2025 Jose Luis Blanco, University of Almeria,
+                         and individual contributors.
+ SPDX-License-Identifier: BSD-3-Clause
+*/
+
+/**
+ * @file   test-mp2p_generator.cpp
+ * @brief  Unit tests for Generator
+ * @author Jose Luis Blanco Claraco
+ * @date   Dec 28, 2025
+ */
+
+#include <mp2p_icp_filters/Generator.h>
+#include <mrpt/core/exceptions.h>
+#include <mrpt/maps/CSimplePointsMap.h>
+#include <mrpt/maps/CVoxelMap.h>
+#include <mrpt/obs/CObservationPointCloud.h>
+
+#include <iostream>  // cerr
+
+namespace
+{
+void test_generator_create_map()
+{
+    constexpr const char* yaml_content = R"(
+# --------------------------------------------------------
+# 1) Generator (observation -> local frame metric maps)
+# --------------------------------------------------------
+generators:
+  # This first generator is used to just create the metric map "gridmap" once:
+  - class_name: mp2p_icp_filters::Generator
+    params:
+      target_layer: "voxelmap"
+      # The default '.*' is replaced by '' (none): do not insert directly any observation,
+      # since we want to insert them after decimation
+      process_class_names_regex: ""
+      metric_map_definition:
+        # Any class derived from mrpt::maps::CMetricMap https://docs.mrpt.org/reference/latest/group_mrpt_maps_grp.html
+        class: mrpt::maps::CVoxelMap
+        #plugin: 'libmola_metric_maps.so' # Import additional custom user-defined map classes (search in LD_LIBRARY_PATH)
+        creationOpts:
+          resolution: 0.20 # [m]
+          #resolution: $f{0.05*MAX_SENSOR_RANGE} # [m]  # You can also use formulas in any numeric field
+        #insertionOpts:
+        # none required for this class
+        likelihoodOpts:
+          decimation: 1
+          occupiedThreshold: 0.51
+        renderOpts:
+          occupiedThreshold: 0.51
+          freeThreshold: 0.40
+          generateFreeVoxels: false
+
+  # Default generator: convert all observations into a point cloud layer "raw":
+  - class_name: mp2p_icp_filters::Generator
+    params:
+      target_layer: "raw"
+      throw_on_unhandled_observation_class: true
+      process_class_names_regex: ".*"
+      #process_sensor_labels_regex: '.*'
+
+    )";
+
+    const auto generators = mp2p_icp_filters::generators_from_yaml(
+        mrpt::containers::yaml::FromText(yaml_content)["generators"]);
+
+    ASSERT_EQUAL_(generators.size(), 2UL);
+
+    mrpt::obs::CObservationPointCloud obs;
+    {
+        auto pts = std::make_shared<mrpt::maps::CSimplePointsMap>();
+        pts->insertPoint(1.0f, 2.0f, 3.0f);
+        pts->insertPoint(4.0f, 5.0f, 6.0f);
+        pts->insertPoint(7.0f, 8.0f, 9.0f);
+        obs.pointcloud = pts;
+    }
+
+    const auto map = mp2p_icp_filters::apply_generators(generators, obs);
+
+    ASSERT_EQUAL_(map.layers.size(), 2UL);
+
+    auto layerMap = map.layer<mrpt::maps::CPointsMap>("raw");
+    ASSERT_(layerMap);
+    ASSERT_EQUAL_(layerMap->size(), 3UL);
+
+    auto voxelMap = map.layer<mrpt::maps::CVoxelMap>("voxelmap");
+    ASSERT_(voxelMap);
+}
+}  // namespace
+
+int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
+{
+    try
+    {
+        test_generator_create_map();
+    }
+    catch (std::exception& e)
+    {
+        std::cerr << mrpt::exception_to_str(e) << "\n";
+        return 1;
+    }
+    return 0;
+}

tests/test-mp2p_icp_algos.cpp

@@ -25,6 +25,7 @@
 #include <mp2p_icp/Solver_GaussNewton.h>
 #include <mp2p_icp/Solver_Horn.h>
 #include <mp2p_icp/Solver_OLAE.h>
+#include <mp2p_icp/icp_pipeline_from_yaml.h>
 #include <mp2p_icp/load_xyz_file.h>
 #include <mrpt/core/exceptions.h>
 #include <mrpt/core/get_env.h>
@@ -34,6 +35,7 @@
 #include <mrpt/poses/Lie/SE.h>
 #include <mrpt/poses/Lie/SO.h>
 #include <mrpt/random.h>
+#include <mrpt/system/CDirectoryExplorer.h>
 #include <mrpt/system/CTimeLogger.h>
 #include <mrpt/system/filesystem.h>
 
@@ -49,6 +51,61 @@ bool DO_PRINT_ALL       = mrpt::get_env<bool>("DO_PRINT_ALL", false);
 
 const std::string datasetDir = MP2P_DATASET_DIR;
 
+void test_load_icp_from_yaml()
+{
+    constexpr const char* yaml_content = R"(
+# YAML configuration file for use with the CLI tool icp-run or
+# programmatically from function mp2p_icp::icp_pipeline_from_yaml()
+#
+class_name: mp2p_icp::ICP
+
+# See: mp2p_icp::Parameter
+params:
+  maxIterations: 100
+  minAbsStep_trans: 1e-4
+  minAbsStep_rot: 1e-4
+
+  debugPrintIterationProgress: true  # Print progress
+  #generateDebugFiles: true
+  #debugFileNameFormat: "icp-run-$LOCAL_ID$LOCAL_LABEL-to-$GLOBAL_ID$GLOBAL_LABEL.icplog"
+  #saveIterationDetails: true
+  #decimationIterationDetails: 10
+
+solvers:
+  - class: mp2p_icp::Solver_Horn
+    params:
+      ~
+
+# Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher
+# instances to pair geometric entities between pointclouds.
+matchers:
+  - class: mp2p_icp::Matcher_Points_DistanceThreshold
+    params:
+      threshold: 0.15
+      thresholdAngularDeg: 0
+      #pairingsPerPoint: 1
+      #
+      # If "pointLayerMatches" is not defined, layers will be matched against
+      # those with the same name in both maps:
+      #pointLayerMatches:
+      #  - {global: "2d_lidar", local: "2d_lidar", weight: 1.0}
+
+quality:
+  - class: mp2p_icp::QualityEvaluator_PairedRatio
+    params:
+      ~  # none required
+    )";
+
+    auto [icp, icpParams] =
+        mp2p_icp::icp_pipeline_from_yaml(mrpt::containers::yaml::FromText(yaml_content));
+
+    ASSERT_EQUAL_(icp->matchers().size(), 1UL);
+    ASSERT_EQUAL_(icp->quality_evaluators().size(), 1UL);
+    ASSERT_EQUAL_(icp->solvers().size(), 1UL);
+
+    ASSERT_EQUAL_(icpParams.maxIterations, 100UL);
+}
+
 void test_icp(
     const std::string& inFile, const std::string& icpClassName, const std::string& solverName,
     const std::string& matcherName, int pointDecimation)
@@ -197,10 +254,35 @@ void test_icp(
 
         timer.Tic();
 
+        // Stablish a hook to include it in the unit tests:
+        icp->setIterationHook(
+            [](const mp2p_icp::ICP::IterationHook_Input& in)
+            {
+                ASSERT_(in.currentSolution != nullptr);
+                return mp2p_icp::ICP::IterationHook_Output();
+            });
+
+        // To cover log files in unit tests:
+        const auto tmpOutFiles        = mrpt::system::getTempFileName();
+        icp_params.generateDebugFiles = true;
+        icp_params.debugFileNameFormat =
+            tmpOutFiles +
+            "_$UNIQUE_ID-local-$LOCAL_ID$LOCAL_LABEL-global-$GLOBAL_ID$GLOBAL_LABEL.icplog";
+
+        // Run ICP:
         icp->align(pc_mod, pc_ref, init_guess, icp_params, icp_results);
 
         const double dt = timer.Tac();
 
+        // Expect created output debug records:
+        {
+            mrpt::system::CDirectoryExplorer::TFileInfoList icpLogFiles;
+            mrpt::system::CDirectoryExplorer::explore(
+                mrpt::system::extractFileDirectory(tmpOutFiles), FILE_ATTRIB_ARCHIVE, icpLogFiles);
+            mrpt::system::CDirectoryExplorer::filterByExtension(icpLogFiles, "icplog");
+            ASSERT_(!icpLogFiles.empty());
+        }
+
         const auto pos_error = gt_pose - icp_results.optimal_tf.mean;
         const auto err_log_n = SO<3>::log(pos_error.getRotationMatrix()).norm();
         const auto err_xyz   = pos_error.norm();
@@ -281,6 +363,9 @@ int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
                     std::get<3>(algo));
             }
         }
+
+        // test icp from yaml:
+        test_load_icp_from_yaml();
     }
     catch (std::exception& e)
     {