diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index f22772908c411fba8a096fb764af40fe6b39316e..a1317fd2936dedf39d79301526fa3d0829b509a5 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -52,7 +52,9 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b
StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
{
// TODO: remove these lines after issue #381 is addressed
- assert(isValid(1e-5) && "Quaternion unit norm is worse than 1e-5 tolerance!");
+ if(not isValid(1e-5))
+ throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
+
state_.normalize();
}
@@ -62,7 +64,9 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fix
assert(state_.size() == 4 && "The quaternion state vector must be of size 4");
// TODO: remove these lines after issue #381 is addressed
- assert(isValid(1e-5) && "Quaternion unit norm is worse than 1e-5 tolerance!");
+ if(not isValid(1e-5))
+ throw std::runtime_error("Quaternion unit norm is worse than 1e-5 tolerance!");
+
state_.normalize();
}
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index 130a43859d5ca944c899f9193f5f6a7398c08207..28dd039c0c2d2fa075aa5d5e366442687a119a4b 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -85,16 +85,16 @@ void Prior::check() const
throw std::runtime_error("Prior " + std::string(1, key_) + " state is not valid (local param violation)");
// check state size
- if (mode_ != "nothing" and state_.size() != sb->getLocalSize())
- throw std::runtime_error("Prior " + std::string(1, key_) + " state size different of local size");
+ if (mode_ != "nothing" and state_.size() != sb->getSize())
+ throw std::runtime_error("Prior " + std::string(1, key_) + " state size different of state size");
// check sigma size
if (mode_ == "factor" and sigma_.size() != sb->getLocalSize())
- throw std::runtime_error("Prior " + std::string(1, key_) + " sigma size different of local size");
+ throw std::runtime_error("Prior " + std::string(1, key_) + " sigma size different of state local size");
// check sigma size
if (dynamic_ and sigma_drift_.size() != sb->getLocalSize())
- throw std::runtime_error("Prior " + std::string(1, key_) + " sigma_drift size different of local size");
+ throw std::runtime_error("Prior " + std::string(1, key_) + " sigma_drift size different of state local size");
}
std::string Prior::print() const
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index de541a5a83b4935e2a52970bb569d8905788585e..42d1a4d476b848a94ddb6659e03fbc0e51b52aab 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -92,17 +92,17 @@ StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
return nullptr;
}
-StateBlockPtr create_position(const Eigen::VectorXd& _state, bool _fixed)
+StateBlockPtr create_pos_vel(const Eigen::VectorXd& _state, bool _fixed)
{
if (_state.size() != 2 and _state.size() != 3)
- throw std::runtime_error("Wrong vector size for position. Must be 2 or 3 (2D or 3D).");
+ throw std::runtime_error("Wrong vector size for position/velocity. Must be 2 or 3 (2D or 3D).");
return StateBlock::create(_state, _fixed);
}
-//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
namespace{
const bool __attribute__((used)) create_orientationRegisteredWithO = FactoryStateBlock::registerCreator("O", wolf::create_orientation);
- const bool __attribute__((used)) create_positionRegisteredWithO = FactoryStateBlock::registerCreator("P", wolf::create_position);
+ const bool __attribute__((used)) create_positionRegisteredWithO = FactoryStateBlock::registerCreator("P", wolf::create_pos_vel);
+ const bool __attribute__((used)) create_velocityRegisteredWithO = FactoryStateBlock::registerCreator("V", wolf::create_pos_vel);
}
}
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
index 4a2f6a1d5217064d14dca6a870db485d3ff8f3d2..c73b78d198a6b6a95e15b03e965a7ceb5a1e065f 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior.cpp
@@ -27,7 +27,7 @@
using namespace wolf;
using namespace Eigen;
-struct PriorTestSetup
+struct PriorAsStruct
{
char key;
std::string mode;
@@ -38,31 +38,26 @@ struct PriorTestSetup
};
void testPrior(const Prior& P,
- char _key,
- const std::string& _mode,
- const VectorXd& _state,
- const VectorXd& _sigma,
- bool _dynamic,
- const VectorXd& _sigma_drift)
+ const PriorAsStruct& pstruct)
{
- ASSERT_EQ(_key, P.getKey());
- ASSERT_EQ(_mode, P.getMode());
- ASSERT_MATRIX_APPROX(_state, P.getState(), 1e-8);
+ ASSERT_EQ(pstruct.key, P.getKey());
+ ASSERT_EQ(pstruct.mode, P.getMode());
+ ASSERT_MATRIX_APPROX(pstruct.state, P.getState(), 1e-8);
- if (_mode == "factor")
+ if (pstruct.mode == "factor")
{
- ASSERT_MATRIX_APPROX(_sigma, P.getSigma(), 1e-8);
+ ASSERT_MATRIX_APPROX(pstruct.sigma, P.getSigma(), 1e-8);
}
else
{
ASSERT_EQ(0,P.getSigma().size());
}
- ASSERT_EQ(_dynamic, P.isDynamic());
+ ASSERT_EQ(pstruct.dynamic, P.isDynamic());
- if (_dynamic)
+ if (pstruct.dynamic)
{
- ASSERT_MATRIX_APPROX(_sigma_drift, P.getSigmaDrift(), 1e-8);
+ ASSERT_MATRIX_APPROX(pstruct.sigma_drift, P.getSigmaDrift(), 1e-8);
}
else
{
@@ -70,87 +65,169 @@ void testPrior(const Prior& P,
}
};
-void createAndTestPrior(char _key,
- const std::string& _mode,
- const VectorXd& _state,
- const VectorXd& _sigma,
- bool _dynamic,
- const VectorXd& _sigma_drift)
+void testPriors(const std::vector<PriorAsStruct>& setups, bool should_work)
{
- // check constructor with default values
- if (not _dynamic)
+ WOLF_INFO("Testing setups that should ", (should_work ? "" : "NOT"), " work...");
+ for (auto setup : setups)
{
- auto P = Prior(_key, _mode, _state, _sigma);
- testPrior(P, _key, _mode, _state, _sigma, _dynamic, _sigma_drift);
+ WOLF_DEBUG("prior setup: ",
+ "\n\tkey: ", std::string(1,setup.key),
+ "\n\tmode: ", setup.mode,
+ "\n\tstate: ", setup.state.transpose(),
+ "\n\tsigma: ", setup.sigma.transpose(),
+ "\n\tdynamic: ", setup.dynamic,
+ "\n\tsigma_drift: ", setup.sigma_drift.transpose());
+ if (should_work)
+ testPrior(Prior(setup.key, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup);
+ else
+ {
+ ASSERT_THROW(testPrior(Prior(setup.key, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error);
+ }
}
+}
- auto P = Prior(_key, _mode, _state, _sigma, _dynamic, _sigma_drift);
- testPrior(P, _key, _mode, _state, _sigma, _dynamic, _sigma_drift);
-};
+TEST(Prior, P)
+{
+ std::vector<PriorAsStruct> setups_ok, setups_death;
+
+ // SHOULD FAIL: Nothing - not dynamic (not allowed for P and O)
+ setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),false}));
+
+ // SHOULD FAIL: Nothing - dynamic (not implemented)
+ setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),true}));
+
+ // Initial guess - not dynamic
+ setups_ok .push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(2),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size
+
+ // SHOULD FAIL: Initial guess - dynamic (not implemented)
+ setups_death.push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(2),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true}));
+
+ // Fix - not dynamic
+ setups_ok .push_back(PriorAsStruct({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorAsStruct({'P',"fix",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size
+
+ // SHOULD FAIL: Fix - dynamic (not implemented)
+ setups_death.push_back(PriorAsStruct({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorAsStruct({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),true}));
+
+ // Factor - not dynamic
+ setups_ok .push_back(PriorAsStruct({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),false}));
+ setups_ok .push_back(PriorAsStruct({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),false}));
+ setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(4),VectorXd::Random(4),false})); // wrong size
+ setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(2),VectorXd::Random(3),false})); // inconsistent size
+
+ // SHOULD FAIL: Factor - dynamic (not implemented)
+ setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),true}));
+ setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),true}));
+
+ // TEST SETUPS
+ testPriors(setups_ok, true);
+ testPriors(setups_death, false);
+}
-TEST(Prior, PFix)
+TEST(Prior, O)
{
- std::vector<PriorTestSetup> setups_ok, setups_death;
+ std::vector<PriorAsStruct> setups_ok, setups_death;
// SHOULD FAIL: Nothing - not dynamic (not allowed for P and O)
- setups_death.push_back(PriorTestSetup({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),false}));
- setups_death.push_back(PriorTestSetup({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(1),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(4).normalized(),VectorXd::Random(0),false}));
// SHOULD FAIL: Nothing - dynamic (not implemented)
- setups_death.push_back(PriorTestSetup({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),true}));
- setups_death.push_back(PriorTestSetup({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(1),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(4).normalized(),VectorXd::Random(0),true}));
// Initial guess - not dynamic
- setups_ok. push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(2),VectorXd::Random(0),false}));
- setups_ok. push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false}));
- setups_death.push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size
+ setups_ok .push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(4).normalized(),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false})); // wrong size
+ setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(4),VectorXd::Random(0),false})); // not normalized
// SHOULD FAIL: Initial guess - dynamic (not implemented)
- setups_death.push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(2),VectorXd::Random(0),true}));
- setups_death.push_back(PriorTestSetup({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(4).normalized(),VectorXd::Random(0),true}));
// Fix - not dynamic
- setups_ok. push_back(PriorTestSetup({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),false}));
- setups_ok. push_back(PriorTestSetup({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),false}));
- setups_death.push_back(PriorTestSetup({'P',"fix",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size
+ setups_ok .push_back(PriorAsStruct({'O',"fix",VectorXd::Random(1),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'O',"fix",VectorXd::Random(4).normalized(),VectorXd::Random(0),false}));
+ setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(3),VectorXd::Random(0),false})); // wrong size
+ setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(4),VectorXd::Random(0),false})); // not normalized
// SHOULD FAIL: Fix - dynamic (not implemented)
- setups_death.push_back(PriorTestSetup({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),true}));
- setups_death.push_back(PriorTestSetup({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(1),VectorXd::Random(0),true}));
+ setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(4).normalized(),VectorXd::Random(0),true}));
// Factor - not dynamic
- setups_ok. push_back(PriorTestSetup({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),false}));
- setups_ok. push_back(PriorTestSetup({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),false}));
- setups_death.push_back(PriorTestSetup({'P',"factor",VectorXd::Random(4),VectorXd::Random(4),false})); // wrong size
- setups_death.push_back(PriorTestSetup({'P',"factor",VectorXd::Random(2),VectorXd::Random(3),false})); // inconsistent size
+ setups_ok .push_back(PriorAsStruct({'O',"factor",VectorXd::Random(1),VectorXd::Random(1),false}));
+ setups_ok .push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4).normalized(),VectorXd::Random(3),false}));
+ setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(3).normalized(),VectorXd::Random(3),false})); // wrong state size
+ setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4).normalized(),VectorXd::Random(4),false})); // wrong sigma size
+ setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4),VectorXd::Random(3),false})); // not normalized
// SHOULD FAIL: Factor - dynamic (not implemented)
- setups_death.push_back(PriorTestSetup({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),true}));
- setups_death.push_back(PriorTestSetup({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),true}));
+ setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(1),VectorXd::Random(2),true}));
+ setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4).normalized(),VectorXd::Random(3),true}));
- for (auto setup : setups_ok)
- {
- WOLF_INFO("Testing setup_ok: ",
- std::string(1,setup.key), " ",
- setup.mode, " ",
- setup.state.transpose(), " ",
- setup.sigma.transpose(), " ",
- setup.dynamic, " ",
- setup.sigma_drift.transpose());
- createAndTestPrior(setup.key, setup.mode, setup.state,setup.sigma,setup.dynamic,setup.sigma_drift);
- }
+ // TEST SETUPS
+ testPriors(setups_ok, true);
+ testPriors(setups_death, false);
+}
- for (auto setup : setups_death)
- {
- WOLF_INFO("Testing setup_death: ",
- std::string(1,setup.key), " ",
- setup.mode, " ",
- setup.state.transpose(), " ",
- setup.sigma.transpose(), " ",
- setup.dynamic, " ",
- setup.sigma_drift.transpose());
- ASSERT_THROW(createAndTestPrior(setup.key, setup.mode, setup.state,setup.sigma,setup.dynamic,setup.sigma_drift),std::runtime_error);
- }
+TEST(Prior, K)
+{
+ std::vector<PriorAsStruct> setups_ok, setups_death;
+
+ // Nothing - not dynamic
+ setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(1),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(3),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(6),VectorXd::Random(0),false}));
+
+ // Nothing - dynamic
+ setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(1),VectorXd::Random(0),true,VectorXd::Random(1)}));
+ setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(3)}));
+ setups_death.push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(2)})); // wrong size
+
+ // Initial guess - not dynamic
+ setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(6),VectorXd::Random(0),false}));
+
+ // Initial guess - dynamic
+ setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),true,VectorXd::Random(1)}));
+ setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(3)}));
+ setups_death.push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(2)})); // wrong size
+
+ // Fix - not dynamic
+ setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(1),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(3),VectorXd::Random(0),false}));
+ setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(6),VectorXd::Random(0),false}));
+
+ // Fix - dynamic
+ setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(1),VectorXd::Random(0),true,VectorXd::Random(1)}));
+ setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(3)}));
+ setups_death.push_back(PriorAsStruct({'K',"fix",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(2)})); // wrong size
+
+ // Factor - not dynamic
+ setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(1),VectorXd::Random(1),false}));
+ setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(3),false}));
+ setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(6),VectorXd::Random(6),false}));
+ setups_death.push_back(PriorAsStruct({'K',"factor",VectorXd::Random(4),VectorXd::Random(3),false})); // wrong sigma size
+
+ // Factor - dynamic
+ setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(1),VectorXd::Random(1),true,VectorXd::Random(1)}));
+ setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(3),true,VectorXd::Random(3)}));
+ setups_death.push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(3),true,VectorXd::Random(2)})); // wrong sigma_drift size
+ setups_death.push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(2),true,VectorXd::Random(3)})); // wrong sigma size
+
+ // TEST SETUPS
+ testPriors(setups_ok, true);
+ testPriors(setups_death, false);
}
int main(int argc, char **argv)