gtest_prior working

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

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

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

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)