From 185428e41692e8c8d137a9ff12a7455ad487a57e Mon Sep 17 00:00:00 2001
From: vsainz <vsainz@iri.upc.edu>
Date: Fri, 5 Aug 2022 15:04:04 +0200
Subject: [PATCH] wip
---
test/CMakeLists.txt | 1 +
test/gtest_processor_odom_icp_3d.cpp | 24 ++++++++++++--------
test/yaml/processor_odom_icp_3d.yaml | 34 ++++++++++++++++++++++++++++
3 files changed, 50 insertions(+), 9 deletions(-)
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c64c1d390..040d0980f 100644
--- a/test/CMakeLists.txt
+++ b/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
diff --git a/test/gtest_processor_odom_icp_3d.cpp b/test/gtest_processor_odom_icp_3d.cpp
index 0703ed18b..b14d44658 100644
--- a/test/gtest_processor_odom_icp_3d.cpp
+++ b/test/gtest_processor_odom_icp_3d.cpp
@@ -1,6 +1,7 @@
// 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;
diff --git a/test/yaml/processor_odom_icp_3d.yaml b/test/yaml/processor_odom_icp_3d.yaml
index e69de29bb..82386dd0f 100644
--- a/test/yaml/processor_odom_icp_3d.yaml
+++ b/test/yaml/processor_odom_icp_3d.yaml
@@ -0,0 +1,34 @@
+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
--
GitLab