diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp index 162d0aacfa86fdc28127da284e4ba3b5132984ac..c5bc71bbc0252beb8c951a6bc37d5f5a66884ea0 100644 --- a/src/state_block/state_block_derived.cpp +++ b/src/state_block/state_block_derived.cpp @@ -27,51 +27,58 @@ namespace wolf { StateBlockPtr StatePoint2d::create(const Eigen::VectorXd& _state, bool _fixed) { - MatrixSizeCheck<2, 1>::check(_state); - return std::make_shared<StatePoint2d>(_state, _fixed); + if (_state.size() == 2) return std::make_shared<StatePoint2d>(_state, _fixed); + + throw std::length_error("Wrong vector size for Point2d."); } StateBlockPtr StatePoint3d::create(const Eigen::VectorXd& _state, bool _fixed) { - MatrixSizeCheck<3, 1>::check(_state); - return std::make_shared<StatePoint3d>(_state, _fixed); + if (_state.size() == 3) return std::make_shared<StatePoint3d>(_state, _fixed); + + throw std::length_error("Wrong vector size for Point3d."); } StateBlockPtr StateVector2d::create(const Eigen::VectorXd& _state, bool _fixed) { - MatrixSizeCheck<2, 1>::check(_state); - return std::make_shared<StateVector2d>(_state, _fixed); + if (_state.size() == 2) return std::make_shared<StateVector2d>(_state, _fixed); + + throw std::length_error("Wrong vector size for Vector2d."); } StateBlockPtr StateVector3d::create(const Eigen::VectorXd& _state, bool _fixed) { - MatrixSizeCheck<3, 1>::check(_state); - return std::make_shared<StateVector3d>(_state, _fixed); + if (_state.size() == 3) return std::make_shared<StateVector3d>(_state, _fixed); + + throw std::length_error("Wrong vector size for Vector3d."); } StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed) { - assert((_state.size() == 2 or _state.size() == 3) && "Provided _state must be size 2 or 3!"); if (_state.size() == 2) return std::make_shared<StatePoint2d>(_state, _fixed); - else + else if (_state.size() == 3) return std::make_shared<StatePoint3d>(_state, _fixed); + + throw std::length_error("Wrong vector size for Point."); } StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed) { - assert((_state.size() == 2 or _state.size() == 3) && "Provided _state must be size 2 or 3!"); if (_state.size() == 2) return std::make_shared<StateVector2d>(_state, _fixed); - else + else if (_state.size() == 3) return std::make_shared<StateVector3d>(_state, _fixed); + + throw std::length_error("Wrong vector size for Vector."); } template <size_t size> StateBlockPtr StateParams<size>::create(const Eigen::VectorXd& _state, bool _fixed) { - MatrixSizeCheck<size, 1>::check(_state); - return std::make_shared<StateParams<size>>(_state, _fixed); + if (_state.size() == size) return std::make_shared<StateParams<size>>(_state, _fixed); + + throw std::length_error("Wrong vector size for Params."); } namespace diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp index fd90e69a52587d4f0acc6434350715e0d8405a47..a7bfa9c963ad52a55d89d6f713c35c2223c69c66 100644 --- a/test/gtest_factory_state_block.cpp +++ b/test/gtest_factory_state_block.cpp @@ -180,7 +180,7 @@ TEST(FactoryStateBlock, creator_Point) ASSERT_FALSE(sba->hasLocalParametrization()); // fails - // ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error); + ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error); } TEST(FactoryStateBlock, creator_P) @@ -198,7 +198,7 @@ TEST(FactoryStateBlock, creator_P) ASSERT_FALSE(sba->hasLocalParametrization()); // fails - // ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::length_error); + ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::length_error); } TEST(FactoryStateBlock, creator_Vector) @@ -216,7 +216,7 @@ TEST(FactoryStateBlock, creator_Vector) ASSERT_FALSE(sba->hasLocalParametrization()); // fails - // ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error); + ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error); } TEST(FactoryStateBlock, creator_V) @@ -234,7 +234,7 @@ TEST(FactoryStateBlock, creator_V) ASSERT_FALSE(sba->hasLocalParametrization()); // fails - // ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::length_error); + ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::length_error); } TEST(FactoryStateBlock, creator_Params) @@ -264,8 +264,8 @@ TEST(FactoryStateBlock, creator_Params) ASSERT_EQ(sb1->getLocalSize(), 1); ASSERT_FALSE(sb1->hasLocalParametrization()); - // fails to compile (static asserts) - // ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::length_error); + // fails + ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::length_error); } int main(int argc, char **argv)