Commit 8a7403c9 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Add neutral element in OperationBase and derived classes.

parent 2e5c0c00
......@@ -50,12 +50,21 @@ namespace se3
/// 4 (quaternion) while the dimension of the tangent space is 3.
Index nq () const
{
return NQ;
return lg1_.nq () + lg2_.nq ();
}
/// Get dimension of Lie Group tangent space
Index nv () const
{
return NV;
return lg1_.nv () + lg2_.nv ();
}
ConfigVector_t neutral () const
{
ConfigVector_t n;
n.resize (nq ());
n.head (lg1_.nq ()) = lg1_.neutral ();
n.tail (lg2_.nq ()) = lg2_.neutral ();
return n;
}
std::string name () const
......
......@@ -243,6 +243,8 @@ namespace se3
Index nq () const;
/// Get dimension of Lie Group tangent space
Index nv () const;
/// Get neutral element as a vector
ConfigVector_t neutral () const;
/// Get name of instance
std::string name () const;
......
......@@ -243,6 +243,13 @@ namespace se3 {
return Derived::nv ();
}
template <class Derived>
typename LieGroupOperationBase <Derived>::ConfigVector_t
LieGroupOperationBase <Derived>::neutral () const
{
return Derived::neutral ();
}
template <class Derived>
std::string LieGroupOperationBase <Derived>::name () const
{
......
......@@ -75,6 +75,12 @@ namespace se3
return NV;
}
ConfigVector_t neutral () const
{
ConfigVector_t n; n.setZero (); n [2] = 1;
return n;
}
std::string name () const
{
return std::string ("SE(2)");
......@@ -228,6 +234,12 @@ namespace se3
return NV;
}
ConfigVector_t neutral () const
{
ConfigVector_t n; n.setZero (); n [6] = 1;
return n;
}
std::string name () const
{
return std::string ("SE(3)");
......
......@@ -69,6 +69,12 @@ namespace se3
return NV;
}
ConfigVector_t neutral () const
{
ConfigVector_t n; n.setZero (); n [0] = 1;
return n;
}
std::string name () const
{
return std::string ("SO(2)");
......@@ -181,6 +187,12 @@ namespace se3
return NV;
}
ConfigVector_t neutral () const
{
ConfigVector_t n; n.setZero (); n [3] = 1;
return n;
}
std::string name () const
{
return std::string ("SO(3)");
......
......@@ -69,6 +69,15 @@ namespace se3
return size_;
}
ConfigVector_t neutral () const
{
ConfigVector_t n;
if (Size == Eigen::Dynamic)
n.resize (size_);
n.setZero ();
return n;
}
std::string name () const
{
std::ostringstream oss; oss << "R^" << size_;
......
......@@ -130,4 +130,81 @@ BOOST_AUTO_TEST_CASE ( test_vector_space )
BOOST_CHECK_MESSAGE(error, "Random configuration between infinite bounds should return an error");
}
BOOST_AUTO_TEST_CASE ( test_size )
{
// R^1: neutral = [0]
VectorSpaceOperation <1> vs1;
Eigen::VectorXd neutral;
neutral.resize (1);
neutral.setZero ();
BOOST_CHECK (vs1.nq () == 1);
BOOST_CHECK (vs1.nv () == 1);
BOOST_CHECK (vs1.name () == "R^1");
BOOST_CHECK (vs1.neutral () == neutral);
// R^2: neutral = [0, 0]
VectorSpaceOperation <2> vs2;
neutral.resize (2);
neutral.setZero ();
BOOST_CHECK (vs2.nq () == 2);
BOOST_CHECK (vs2.nv () == 2);
BOOST_CHECK (vs2.name () == "R^2");
BOOST_CHECK (vs2.neutral () == neutral);
// R^3: neutral = [0, 0, 0]
VectorSpaceOperation <3> vs3;
neutral.resize (3);
neutral.setZero ();
BOOST_CHECK (vs3.nq () == 3);
BOOST_CHECK (vs3.nv () == 3);
BOOST_CHECK (vs3.name () == "R^3");
BOOST_CHECK (vs3.neutral () == neutral);
// SO(2): neutral = [1, 0]
SpecialOrthogonalOperation <2> so2;
neutral.resize (2); neutral [0] = 1; neutral [1] = 0;
BOOST_CHECK (so2.nq () == 2);
BOOST_CHECK (so2.nv () == 1);
BOOST_CHECK (so2.name () == "SO(2)");
BOOST_CHECK (so2.neutral () == neutral);
// SO(3): neutral = [0, 0, 0, 1]
SpecialOrthogonalOperation <3> so3;
neutral.resize (4); neutral.setZero ();
neutral [3] = 1;
BOOST_CHECK (so3.nq () == 4);
BOOST_CHECK (so3.nv () == 3);
BOOST_CHECK (so3.name () == "SO(3)");
BOOST_CHECK (so3.neutral () == neutral);
// SE(2): neutral = [0, 0, 1, 0]
SpecialEuclideanOperation <2> se2;
neutral.resize (4); neutral.setZero ();
neutral [2] = 1;
BOOST_CHECK (se2.nq () == 4);
BOOST_CHECK (se2.nv () == 3);
BOOST_CHECK (se2.name () == "SE(2)");
BOOST_CHECK (se2.neutral () == neutral);
// SE(3): neutral = [0, 0, 0, 0, 0, 0, 1]
SpecialEuclideanOperation <3> se3;
neutral.resize (7); neutral.setZero ();
neutral [6] = 1;
BOOST_CHECK (se3.nq () == 7);
BOOST_CHECK (se3.nv () == 6);
BOOST_CHECK (se3.name () == "SE(3)");
BOOST_CHECK (se3.neutral () == neutral);
// R^2 x SO(2): neutral = [0, 0, 1, 0]
CartesianProductOperation <VectorSpaceOperation <2>,
SpecialOrthogonalOperation <2> > r2xso2;
neutral.resize (4); neutral.setZero ();
neutral [2] = 1;
BOOST_CHECK (r2xso2.nq () == 4);
BOOST_CHECK (r2xso2.nv () == 3);
BOOST_CHECK (r2xso2.name () == "R^2*SO(2)");
BOOST_CHECK (r2xso2.neutral () == neutral);
// R^3 x SO(3): neutral = [0, 0, 0, 0, 0, 0, 1]
CartesianProductOperation <VectorSpaceOperation <3>,
SpecialOrthogonalOperation <3> > r3xso3;
neutral.resize (7); neutral.setZero ();
neutral [6] = 1;
BOOST_CHECK (r3xso3.nq () == 7);
BOOST_CHECK (r3xso3.nv () == 6);
BOOST_CHECK (r3xso3.name () == "R^3*SO(3)");
BOOST_CHECK (r3xso3.neutral () == neutral);
}
BOOST_AUTO_TEST_SUITE_END ()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment