wip

test/CMakeLists.txt

@@ -27,4 +27,5 @@ endif(falkolib_FOUND)
 
 if(PCL_FOUND)
 	wolf_add_gtest(gtest_laser3d_tools gtest_laser3d_tools.cpp)
+	wolf_add_gtest(gtest_processor_odom_icp_3d gtest_processor_odom_icp_3d.cpp)
 endif(PCL_FOUND)
\ No newline at end of file

test/gtest_processor_odom_icp_3d.cpp

 // wolf
 #include "laser/processor/processor_odom_icp_3d.h"
 #include "core/processor/factory_processor.h"
+#include "core/common/wolf.h"
 
 // wolf yaml
 #include <core/utils/params_server.h>
@@ -16,16 +17,16 @@ using namespace laser;
 
 WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
 
-class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
+class Test_ProcessorOdomIcp3D_yaml : public testing::Test
 {
   public:
     ProblemPtr                           P;
     SensorLaser3dPtr                     S;
     ProcessorOdomIcp3dPtr                p;
     FrameBasePtr                         F0, F1, F;
-    CaptureLaser3dPtr                     C0, C1, C;
-    FeatureBasePtr                        f1;
-    FactorBasePtr c1;
+    CaptureLaser3dPtr                    C0, C1, C2;
+    FeatureBasePtr                       f1, f2, f3;
+    FactorBasePtr                        c1, c2;
 
     VectorXd              data;
     MatrixXd              data_cov;
@@ -37,14 +38,18 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
   protected:
     void SetUp() override
     {
-        std::string  wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
-        ParserYaml   parser    = ParserYaml("processor_odom_icp_3d.yaml", wolf_root + "/test/yaml/");
+        std::string laser_root_dir = _WOLF_LASER_ROOT_DIR;
+
+        ParserYaml   parser    = ParserYaml("processor_odom_icp_3d.yaml", laser_root_dir + "/test/yaml/");
         ParamsServer server    = ParamsServer(parser.getParams());
         P                      = Problem::autoSetup(server);
 
-        S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
-        p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+        S = std::static_pointer_cast<SensorLaser3d>(P->getHardware()->getSensorList().front());
+        p = std::static_pointer_cast<ProcessorOdomIcp3d>(S->getProcessorList().front());
+
 
+        P->print(4,1,1,1);
+/* 
         data     = VectorXd::Zero(12);  // [ a, w, f, t ]
         data_cov = MatrixXd::Identity(12, 12);
         C        = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
@@ -83,10 +88,11 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
         F1->getStateBlock('V')->setState(Vector3d(0, 0, 0));
         F1->getStateBlock('O')->setState(Vector4d(0, 0, 0, 1));
         F1->getStateBlock('L')->setState(Vector3d(0, 0, 0));
+        */
     }
 };
 
-class Test_FactorForceTorqueInertialDynamics : public testing::Test
+class Test_ProcessorOdomIcp3D : public testing::Test
 {
   public:
     ProblemPtr                                    P;

test/yaml/processor_odom_icp_3d.yaml

+config:
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      time_tolerance: 0.05
+    tree_manager: 
+      type: "None"
+  map:
+    type: "MapBase"
+    plugin: "core"
+  sensors:
+   -
+    name: "Lidar"
+    type: "SensorLaser3d"   # No
+    plugin: "laser"
+    extrinsic:
+      pose: [0.5,0,0, 0,0,0,1]
+
+  processors:
+   -
+    name: "Odometry ICP 3d"
+    type: "ProcessorOdomIcp3d"
+    sensor_name: "Lidar"
+    plugin: "laser"
+    time_tolerance: 0.05
+    icp_max_iterations:  20
+    icp_transformation_rotation_epsilon:  0.99
+    icp_transformation_translation_epsilon: 1e-6
+    icp_max_correspondence_distance:  0.5
\ No newline at end of file