From 4f7882df1d15bc388393c602aab497ffad5ad8ac Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 29 Apr 2020 10:25:29 +0200
Subject: [PATCH] Register creator "H" for homogeneous
---
src/state_block/state_block.cpp | 1 +
test/gtest_factory_state_block.cpp | 9 +++++++++
2 files changed, 10 insertions(+)
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 372be5112..030febd46 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -66,6 +66,7 @@ WOLF_REGISTER_STATEBLOCK(StateBlock);
WOLF_REGISTER_STATEBLOCK(StateQuaternion);
WOLF_REGISTER_STATEBLOCK(StateAngle);
WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d);
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(H, StateHomogeneous3d);
StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
{
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index a915b061e..fbded073c 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -111,6 +111,15 @@ TEST(FactoryStateBlock, creator_Homogeneous3d)
ASSERT_TRUE(sbh->hasLocalParametrization());
}
+TEST(FactoryStateBlock, creator_H)
+{
+ auto sbh = FactoryStateBlock::get().create("H", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbh->getSize() , 4);
+ ASSERT_EQ(sbh->getLocalSize(), 3);
+ ASSERT_TRUE(sbh->hasLocalParametrization());
+}
+
TEST(FactoryStateBlock, creator_O_is_quaternion)
{
auto sbq = FactoryStateBlock::get().create("O", Eigen::Vector4d(1,2,3,4), false);
--
GitLab