Commit 2b3b7a8c authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[C++] Rename special orthogonal and euclidean class

* SE(n) -> SpecialEuclideanOperation<N>
* SO(n) -> SpecialOrthogonalOperation<N>
parent 947686dc
......@@ -39,16 +39,16 @@ namespace se3 {
template<> struct LieGroupTpl::operation <JointModelComposite> {};
template<> struct LieGroupTpl::operation <JointModelSpherical> {
typedef SpecialOrthogonalOperation type;
typedef SpecialOrthogonalOperation<3> type;
};
template<> struct LieGroupTpl::operation <JointModelFreeFlyer> {
typedef SpecialEuclideanOperation type;
typedef SpecialEuclideanOperation<3> type;
};
template<> struct LieGroupTpl::operation <JointModelPlanar> {
typedef SpecialEuclidean1Operation type;
typedef SpecialEuclideanOperation<2> type;
};
template<int Axis> struct LieGroupTpl::operation <JointModelRevoluteUnbounded<Axis> > {
typedef SpecialOrthogonal1Operation type;
typedef SpecialOrthogonalOperation<2> type;
};
// TODO REMOVE: For testing purposes only
......
......@@ -30,115 +30,34 @@
namespace se3
{
struct SpecialEuclideanOperation;
template <> struct traits<SpecialEuclideanOperation> {
template<int N> struct SpecialEuclideanOperation {};
template<int N> struct traits<SpecialEuclideanOperation<N> > {};
template<> struct traits<SpecialEuclideanOperation<2> > {
typedef double Scalar;
enum {
NQ = 7,
NV = 6
NQ = 4,
NV = 3
};
typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
};
struct SpecialEuclideanOperation : public LieGroupOperationBase <SpecialEuclideanOperation>
{
typedef CartesianProductOperation <VectorSpaceOperation<3>, SpecialOrthogonalOperation> R3crossSO3_t;
typedef SpecialEuclideanOperation LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
typedef Eigen::Quaternion<Scalar> Quaternion_t;
typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
typedef SE3 Transformation_t;
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d)
{
ConstQuaternionMap_t p0 (q0.derived().template tail<4>().data());
ConstQuaternionMap_t p1 (q1.derived().template tail<4>().data());
const_cast < Eigen::MatrixBase<Tangent_t>& > (d)
= log6( SE3(p0.matrix(), q0.derived().template head<3>()).inverse()
* SE3(p1.matrix(), q1.derived().template head<3>())).toVector();
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConfigOut_t& out = (const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived();
ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
QuaternionMap_t res_quat (out.template tail<4>().data());
SE3 M0 (quat.matrix(), q.derived().template head<3>());
SE3 M1 (M0 * exp6(Motion(v)));
out.template head<3>() = M1.translation();
res_quat = M1.rotation();
// Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix.
// It is then safer to re-normalized after converting M1.rotation to quaternion.
firstOrderNormalize(res_quat);
}
// interpolate_impl use default implementation.
// template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
// static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
// const Eigen::MatrixBase<ConfigR_t> & q1,
// const Scalar& u,
// const Eigen::MatrixBase<ConfigOut_t>& qout)
// {
// }
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t t;
difference_impl(q0, q1, t);
return t.squaredNorm();
}
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
{
R3crossSO3_t::random(qout);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
R3crossSO3_t::randomConfiguration(lower, upper, qout);
}
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
return R3crossSO3_t::isSameConfiguration(q0, q1, prec);
}
}; // struct SpecialEuclideanOperation
struct SpecialEuclidean1Operation;
template <> struct traits<SpecialEuclidean1Operation> {
template<> struct traits<SpecialEuclideanOperation<3> > {
typedef double Scalar;
enum {
NQ = 4,
NV = 3
NQ = 7,
NV = 6
};
typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
};
struct SpecialEuclidean1Operation : public LieGroupOperationBase <SpecialEuclidean1Operation>
template<>
struct SpecialEuclideanOperation<2> : public LieGroupOperationBase <SpecialEuclideanOperation<2> >
{
typedef CartesianProductOperation <VectorSpaceOperation<2>, SpecialOrthogonal1Operation> R2crossSO1_t;
typedef SpecialEuclidean1Operation LieGroupDerived;
typedef CartesianProductOperation <VectorSpaceOperation<2>, SpecialOrthogonalOperation<2> > R2crossSO2_t;
typedef SpecialEuclideanOperation LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
......@@ -228,7 +147,7 @@ namespace se3
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
{
R2crossSO1_t::random(qout);
R2crossSO2_t::random(qout);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
......@@ -236,7 +155,7 @@ namespace se3
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
R2crossSO1_t::randomConfiguration(lower, upper, qout);
R2crossSO2_t::randomConfiguration(lower, upper, qout);
}
template <class ConfigL_t, class ConfigR_t>
......@@ -244,7 +163,7 @@ namespace se3
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
return R2crossSO1_t::isSameConfiguration(q0, q1, prec);
return R2crossSO2_t::isSameConfiguration(q0, q1, prec);
}
private:
......@@ -259,7 +178,91 @@ namespace se3
M.rotation().topLeftCorner<2,2>() << c_theta, -s_theta, s_theta, c_theta;
M.translation().head<2>() = q.template head<2>();
}
}; // struct SpecialEuclidean1Operation
}; // struct SpecialEuclideanOperation<2>
template<>
struct SpecialEuclideanOperation<3> : public LieGroupOperationBase <SpecialEuclideanOperation<3> >
{
typedef CartesianProductOperation <VectorSpaceOperation<3>, SpecialOrthogonalOperation<3> > R3crossSO3_t;
typedef SpecialEuclideanOperation LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
typedef Eigen::Quaternion<Scalar> Quaternion_t;
typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
typedef SE3 Transformation_t;
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d)
{
ConstQuaternionMap_t p0 (q0.derived().template tail<4>().data());
ConstQuaternionMap_t p1 (q1.derived().template tail<4>().data());
const_cast < Eigen::MatrixBase<Tangent_t>& > (d)
= log6( SE3(p0.matrix(), q0.derived().template head<3>()).inverse()
* SE3(p1.matrix(), q1.derived().template head<3>())).toVector();
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConfigOut_t& out = (const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived();
ConstQuaternionMap_t quat(q.derived().template tail<4>().data());
QuaternionMap_t res_quat (out.template tail<4>().data());
SE3 M0 (quat.matrix(), q.derived().template head<3>());
SE3 M1 (M0 * exp6(Motion(v)));
out.template head<3>() = M1.translation();
res_quat = M1.rotation();
// Norm of qs might be epsilon-different to 1, so M1.rotation might be epsilon-different to a rotation matrix.
// It is then safer to re-normalized after converting M1.rotation to quaternion.
firstOrderNormalize(res_quat);
}
// interpolate_impl use default implementation.
// template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
// static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
// const Eigen::MatrixBase<ConfigR_t> & q1,
// const Scalar& u,
// const Eigen::MatrixBase<ConfigOut_t>& qout)
// {
// }
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t t;
difference_impl(q0, q1, t);
return t.squaredNorm();
}
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
{
R3crossSO3_t::random(qout);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
R3crossSO3_t::randomConfiguration(lower, upper, qout);
}
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
return R3crossSO3_t::isSameConfiguration(q0, q1, prec);
}
}; // struct SpecialEuclideanOperation<3>
} // namespace se3
......
......@@ -26,119 +26,33 @@
namespace se3
{
struct SpecialOrthogonalOperation;
template <> struct traits<SpecialOrthogonalOperation> {
template<int N> struct SpecialOrthogonalOperation {};
template<int N> struct traits<SpecialOrthogonalOperation<N> > {};
template <> struct traits<SpecialOrthogonalOperation<2> > {
typedef double Scalar;
enum {
NQ = 4,
NV = 3
NQ = 2,
NV = 1
};
typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
};
struct SpecialOrthogonalOperation : public LieGroupOperationBase <SpecialOrthogonalOperation>
{
typedef SpecialOrthogonalOperation LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
typedef Eigen::Quaternion<Scalar> Quaternion_t;
typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d)
{
ConstQuaternionMap_t p0 (q0.derived().data());
ConstQuaternionMap_t p1 (q1.derived().data());
const_cast < Eigen::MatrixBase<Tangent_t>& > (d)
= log3((p0.matrix().transpose() * p1.matrix()).eval());
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConstQuaternionMap_t quat(q.derived().data());
QuaternionMap_t quaternion_result (
(const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived().data()
);
Quaternion_t pOmega(exp3(v));
quaternion_result = quat * pOmega;
firstOrderNormalize(quaternion_result);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
ConstQuaternionMap_t p0 (q0.derived().data());
ConstQuaternionMap_t p1 (q1.derived().data());
QuaternionMap_t quaternion_result (
(const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived().data()
);
quaternion_result = p0.slerp(u, p1);
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t t;
difference_impl(q0, q1, t);
return t.squaredNorm();
}
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
{
QuaternionMap_t out (
(const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived().data()
);
uniformRandom(out);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
random_impl(qout);
}
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
ConstQuaternionMap_t quat1(q0.derived().data());
ConstQuaternionMap_t quat2(q1.derived().data());
return defineSameRotation(quat1,quat2,prec);
}
}; // struct SpecialOrthogonalOperation
struct SpecialOrthogonal1Operation;
template <> struct traits<SpecialOrthogonal1Operation> {
template <> struct traits<SpecialOrthogonalOperation<3> > {
typedef double Scalar;
enum {
NQ = 2,
NV = 1
NQ = 4,
NV = 3
};
typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
};
struct SpecialOrthogonal1Operation : public LieGroupOperationBase <SpecialOrthogonal1Operation>
template<>
struct SpecialOrthogonalOperation<2> : public LieGroupOperationBase <SpecialOrthogonalOperation<2> >
{
typedef SpecialOrthogonal1Operation LieGroupDerived;
typedef SpecialOrthogonalOperation LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
......@@ -221,7 +135,96 @@ namespace se3
{
random_impl(qout);
}
}; // struct SpecialOrthogonal1Operation
}; // struct SpecialOrthogonalOperation<2>
template<>
struct SpecialOrthogonalOperation<3> : public LieGroupOperationBase <SpecialOrthogonalOperation<3> >
{
typedef SpecialOrthogonalOperation LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
typedef Eigen::Quaternion<Scalar> Quaternion_t;
typedef Eigen::Map< Quaternion_t> QuaternionMap_t;
typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t;
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d)
{
ConstQuaternionMap_t p0 (q0.derived().data());
ConstQuaternionMap_t p1 (q1.derived().data());
const_cast < Eigen::MatrixBase<Tangent_t>& > (d)
= log3((p0.matrix().transpose() * p1.matrix()).eval());
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConstQuaternionMap_t quat(q.derived().data());
QuaternionMap_t quaternion_result (
(const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived().data()
);
Quaternion_t pOmega(exp3(v));
quaternion_result = quat * pOmega;
firstOrderNormalize(quaternion_result);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
ConstQuaternionMap_t p0 (q0.derived().data());
ConstQuaternionMap_t p1 (q1.derived().data());
QuaternionMap_t quaternion_result (
(const_cast< Eigen::MatrixBase<ConfigOut_t>& >(qout)).derived().data()
);
quaternion_result = p0.slerp(u, p1);
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t t;
difference_impl(q0, q1, t);
return t.squaredNorm();
}
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
{
QuaternionMap_t out (
(const_cast< Eigen::MatrixBase<Config_t>& >(qout)).derived().data()
);
uniformRandom(out);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
random_impl(qout);
}
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
ConstQuaternionMap_t quat1(q0.derived().data());
ConstQuaternionMap_t quat2(q1.derived().data());
return defineSameRotation(quat1,quat2,prec);
}
}; // struct SpecialOrthogonalOperation<3>
} // namespace se3
#endif // ifndef __se3_special_orthogonal_operation_hpp__
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