From 66ee16ce452412ae8256c4bbbf4ec7648a6b506f Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Fri, 6 May 2022 14:04:52 +0200
Subject: [PATCH] [skip ci] wip
---
include/core/state_block/prior.h | 4 +-
include/core/utils/params_server.h | 22 +++++-----
src/utils/params_server.cpp | 6 +++
test/CMakeLists.txt | 8 ++--
test/gtest_prior.cpp | 70 ++++++++++++++++++++++++++++++
test/gtest_sensor_base.cpp | 29 +++++++++----
6 files changed, 114 insertions(+), 25 deletions(-)
create mode 100644 test/gtest_prior.cpp
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index 14aba5e5f..cf36d9fc3 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -48,8 +48,8 @@ class Prior
const std::string& _mode,
const Eigen::VectorXd& _state,
const Eigen::VectorXd& _sigma,
- bool _dynamic,
- const Eigen::VectorXd& _sigma_drift);
+ bool _dynamic = false,
+ const Eigen::VectorXd& _sigma_drift = Eigen::VectorXd(0));
Prior(const std::string& _prefix, char _key, const ParamsServer& _server);
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
index 661957f02..23e2d82be 100644
--- a/include/core/utils/params_server.h
+++ b/include/core/utils/params_server.h
@@ -71,16 +71,18 @@ public:
};
-// template<typename Derived>
-// std::string to_string(const Eigen::DenseBase<Derived>& mat)
-// {
-// std::stringstream ss;
-// if (mat.cols() == 1)
-// ss << mat.transpose();
-// else
-// ss << std::endl << mat;
-// return ss.str();
-// }
+template<typename Derived>
+std::string to_string(const Eigen::DenseBase<Derived>& mat)
+{
+ std::stringstream ss;
+ if (mat.cols() == 1)
+ ss << mat.transpose();
+ else
+ ss << std::endl << mat;
+ return ss.str();
+}
+
+std::string to_string(bool _arg);
}
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
index 7b2aea7e3..ac0296370 100644
--- a/src/utils/params_server.cpp
+++ b/src/utils/params_server.cpp
@@ -48,4 +48,10 @@ void ParamsServer::addParams(std::map<std::string, std::string> _params)
{
params_.insert(_params.begin(), _params.end());
}
+
+std::string to_string(bool _arg)
+{
+ return (_arg ? "true" : "false");
+}
+
}
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4bf4cd05e..f913970e6 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -62,8 +62,8 @@ wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
target_link_libraries(gtest_factor_base ${PLUGIN_NAME})
# FactorAutodiff class test
-wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
-target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
+# wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
+# target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
# Factory test
wolf_add_gtest(gtest_factory gtest_factory.cpp)
@@ -94,8 +94,8 @@ wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
# IsMotion classes test
-wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
-target_link_libraries(gtest_motion_provider ${PLUGIN_NAME})
+# wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
+# target_link_libraries(gtest_motion_provider ${PLUGIN_NAME})
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
new file mode 100644
index 000000000..6b7015a1e
--- /dev/null
+++ b/test/gtest_prior.cpp
@@ -0,0 +1,70 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#include "core/utils/utils_gtest.h"
+#include "core/state_block/prior.h"
+
+using namespace wolf;
+
+testPrior(char _key,
+ const std::string& _mode,
+ const Eigen::VectorXd& _state,
+ const Eigen::VectorXd& _sigma,
+ bool _dynamic = false,
+ const Eigen::VectorXd& _sigma_drift = Eigen::VectorXd(0))
+{
+ auto P = Prior(key,
+ mode,
+ state,
+ sigma,
+ dynamic,
+ sigma_drift);
+
+ ASSERT_EQ(key, P.getKey());
+ ASSERT_EQ(mode, P.getMode());
+ ASSERT_MATRIX_APPROX(state, P.getState(), 1e-8);
+ if (mode == "factor")
+ ASSERT_MATRIX_APPROX(sigma, P.getSigma(), 1e-8);
+ ASSERT_EQ(dynamic, P.isDynamic());
+ if (dynamic)
+ ASSERT_MATRIX_APPROX(state, P.getState(), 1e-8);
+}
+
+TEST(Prior, Pfix)
+{
+ dim=2;
+ auto key = 'P';
+ auto mode = "fix";
+ Eigen::VectorXd state = Eigen::VectorXd::Zero(dim);
+ Eigen::VectorXd sigma = Eigen::VectorXd(0);
+ bool dynamic = false;
+ Eigen::VectorXd sigma_drift = Eigen::VectorXd(0);
+
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index ac13388e2..513036095 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -32,17 +32,28 @@
using namespace wolf;
-TEST(SensorBase, setNoiseStd)
+TEST(SensorBase, POfix)
{
- SensorBasePtr S (std::make_shared<SensorBase>("SensorBase", nullptr, nullptr, nullptr, 2)); // noise size 2
+ auto S = std::make_shared<SensorBase>("SensorBase",
+ "sensor1",
+ 2,
+ Priors({{'P',Prior('P',
+ "fix",
+ Eigen::Vector2d::Zero(),
+ Eigen::VectorXd(0),
+ false,
+ Eigen::VectorXd(0))},
+ {'O',Prior('O',
+ "fix",
+ Eigen::Vector1d::Zero(),
+ Eigen::VectorXd(0),
+ false,
+ Eigen::VectorXd(0))},
+ }),
+ std::make_shared<ParamsSensorBase>({Eigen::Vector2d::Constant(0.1)}));
- Eigen::Vector2d noise_std = Eigen::Vector2d::Ones() * 0.1;
- Eigen::Matrix2d noise_cov = Eigen::Matrix2d::Identity() * 0.01;
-
- S->setNoiseStd(noise_std);
-
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
+ ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
+ ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
}
int main(int argc, char **argv)
--
GitLab