YAML parser for processor odom ICP

CMakeLists.txt

@@ -216,6 +216,7 @@ SET(SRCS_DTASSC
 SET(SRCS_SOLVER
   )
 SET(SRCS_YAML
+  src/yaml/processor_odom_ICP_yaml.cpp
   src/yaml/sensor_laser_2D_yaml.cpp
   )
 #OPTIONALS

src/yaml/processor_odom_ICP_yaml.cpp

+/**
+ * \file processor_odom_ICP_yaml.cpp
+ *
+ *  Created on: Aug 6, 2019
+ *      \author: jsola
+ */
+
+
+// wolf
+#include "laser/processor/processor_odom_icp.h"
+#include "core/common/factory.h"
+
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// yaml library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+namespace {
+// intrinsics creator
+ProcessorParamsBasePtr createProcessorParamsOdomICP(const std::string& _filename_dot_yaml)
+{
+    WOLF_INFO("ProcessorParamsOdomICP: Parsing file: ", _filename_dot_yaml);
+
+    YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+    if (config["type"].as<std::string>() == "ODOM ICP")
+    {
+        ProcessorParamsOdomICPPtr params = std::make_shared<ProcessorParamsOdomICP>();
+
+        params->voting_active               = config["voting_active"]               .as<bool>();
+        params->voting_aux_active           = config["voting_aux_active"]           .as<bool>();
+        params->time_tolerance              = config["time_tolerance"]              .as<Scalar>();
+
+        params->min_features_for_keyframe   = config["min_features_for_keyframe"]   .as<Scalar>();
+        params->max_new_features            = config["max_new_features"]            .as<SizeEigen  >();
+
+        params->use_point_to_line_distance  = config["use_point_to_line_distance"]  .as<int>();
+        params->max_correspondence_dist     = config["max_correspondence_dist"]     .as<int>();
+        params->max_iterations              = config["max_iterations"]              .as<int>();
+        params->use_corr_tricks             = config["use_corr_tricks"]             .as<int>();
+        params->outliers_maxPerc            = config["outliers_maxPerc"]            .as<Scalar>();
+        params->outliers_adaptive_order     = config["outliers_adaptive_order"]     .as<Scalar>();
+        params->outliers_adaptive_mult      = config["outliers_adaptive_mult"]      .as<Scalar>();
+        params->vfk_min_dist                = config["vfk_min_dist"]                .as<Scalar>();
+        params->vfk_min_angle               = config["vfk_min_angle"]               .as<Scalar>();
+        params->vfk_min_time                = config["vfk_min_time"]                .as<Scalar>();
+        params->vfk_min_error               = config["vfk_min_error"]               .as<Scalar>();
+        params->vfk_max_points              = config["vfk_max_points"]              .as<int>();
+
+        return params;
+    }
+
+    std::cout << "Bad configuration file. No processor type found." << std::endl;
+    return nullptr;
+}
+
+// register into factory
+const bool WOLF_UNUSED registered_odom_ICP_params = ProcessorParamsFactory::get().registerCreator("ODOM ICP", createProcessorParamsOdomICP);
+
+} // namespace [void]
+} // namespace wolf

test/gtest_processor_odom_icp.cpp

@@ -6,7 +6,7 @@
  */
 
 #include "laser/processor/processor_odom_icp.h"
-
+#include "laser/internal/config.h"
 #include "core/utils/utils_gtest.h"
 
 using namespace wolf;
@@ -33,6 +33,19 @@ TEST(ProcessorOdomIcp, Constructor)
     ASSERT_EQ(prc->getType(), "ODOM ICP");
 }
 
+TEST(ProcessorOdomIcp, Constructor_yaml)
+{
+    std::string wolf_root = _WOLF_LASER_ROOT_DIR;
+
+    auto params = std::static_pointer_cast<ProcessorParamsOdomICP>(ProcessorParamsFactory::get().create("ODOM ICP", wolf_root + "/test/yaml/processor_odom_ICP.yaml"));
+    auto prc    = std::make_shared<ProcessorOdomICP>(params);
+
+    ASSERT_TRUE(prc); // not nullptr
+    ASSERT_EQ(prc->getType(), "ODOM ICP");
+}
+
+
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);

demos/processor_odom_ICP.yaml → test/yaml/processor_odom_ICP.yaml

@@ -10,15 +10,15 @@ min_features_for_keyframe   : 0
 max_new_features            : 0
 
 # from processor odom ICP
-use_point_to_line_distance  : true
-max_correspondence_dist     : 1
-max_iterations              : 5
-use_corr_tricks             : 1
-outliers_maxPerc            : 1
-outliers_adaptive_order     : 1
-outliers_adaptive_mult      : 1
-vfk_min_dist                : 1
-vfk_min_angle               : 1
-vfk_min_time                : 1
-vfk_min_error               : 1
-vfk_max_points              : 1
+use_point_to_line_distance  : 1
+max_correspondence_dist     : 2
+max_iterations              : 3
+use_corr_tricks             : 4
+outliers_maxPerc            : 5
+outliers_adaptive_order     : 6
+outliers_adaptive_mult      : 7
+vfk_min_dist                : 8
+vfk_min_angle               : 9
+vfk_min_time                : 10
+vfk_min_error               : 11
+vfk_max_points              : 12