[skip ci] wip

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);
 

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);
 
 }
 

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

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)

test/gtest_prior.cpp

+//--------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();
+}
+

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)