Skip to content
Snippets Groups Projects
Commit 66ee16ce authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

[skip ci] wip

parent 2ed9fb69
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #10465 skipped
This commit is part of merge request !448. Comments created here will be created in the context of that merge request.
......@@ -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);
......
......@@ -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);
}
......
......@@ -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
......@@ -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)
......
//--------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();
}
......@@ -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)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment