Commit df32e8f0 authored by jcarpent's avatar jcarpent
Browse files

[LieGroup] Make LieGroupOperation compliant with non-static method

Some methods must be changed from static to non-static due to JointModelComposite
parent 298af434
......@@ -89,83 +89,83 @@ namespace se3
}
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d)
const Eigen::MatrixBase<Tangent_t> & d) const
{
Tangent_t& out = const_cast< Eigen::MatrixBase<Tangent_t>& > (d).derived();
LieGroup1::difference(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), out.template head<LieGroup1::NV>());
LieGroup2::difference(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), out.template tail<LieGroup2::NV>());
lg1_.difference(q0.head(lg1_.nq()), q1.head(lg1_.nq()), out.head(lg1_.nv()));
lg2_.difference(q0.tail(lg2_.nq()), q1.tail(lg2_.nq()), out.tail(lg2_.nv()));
}
template <class ConfigL_t, class ConfigR_t, class JacobianLOut_t, class JacobianROut_t>
static void Jdifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
void Jdifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianLOut_t>& J0,
const Eigen::MatrixBase<JacobianROut_t>& J1)
const Eigen::MatrixBase<JacobianROut_t>& J1) const
{
JacobianLOut_t& J0out = const_cast< JacobianLOut_t& >(J0.derived());
J0out.template topRightCorner<LieGroup1::NV,LieGroup2::NV>().setZero();
J0out.template bottomLeftCorner<LieGroup2::NV,LieGroup1::NV>().setZero();
J0out.topRightCorner(lg1_.nv(),lg2_.nv()).setZero();
J0out.bottomLeftCorner(lg2_.nv(),lg1_.nv()).setZero();
JacobianROut_t& J1out = const_cast< JacobianROut_t& >(J1.derived());
J1out.template topRightCorner<LieGroup1::NV,LieGroup2::NV>().setZero();
J1out.template bottomLeftCorner<LieGroup2::NV,LieGroup1::NV>().setZero();
LieGroup1::Jdifference(
q0.template head<LieGroup1::NQ>(),
q1.template tail<LieGroup1::NQ>(),
J0out.template topLeftCorner<LieGroup1::NV,LieGroup1::NV>(),
J1out.template topLeftCorner<LieGroup1::NV,LieGroup1::NV>());
LieGroup2::Jdifference(
q0.template tail<LieGroup2::NQ>(),
q1.template tail<LieGroup2::NQ>(),
J0out.template bottomRightCorner<LieGroup2::NV,LieGroup2::NV>(),
J1out.template bottomRightCorner<LieGroup2::NV,LieGroup2::NV>());
J1out.topRightCorner(lg1_.nv(),lg2_.nv()).setZero();
J1out.bottomLeftCorner(lg2_.nv(),lg1_.nv()).setZero();
lg1_.Jdifference(
q0.head(lg1_.nq()),
q1.head(lg1_.nq()),
J0out.topLeftCorner(lg1_.nv(),lg1_.nv()),
J1out.topLeftCorner(lg1_.nv(),lg1_.nv()));
lg2_.Jdifference(
q0.tail(lg2_.nq()),
q1.tail(lg2_.nq()),
J0out.bottomRightCorner(lg2_.nv(),lg2_.nv()),
J1out.bottomRightCorner(lg2_.nv(),lg2_.nv()));
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
LieGroup1::integrate(q.template head<LieGroup1::NQ>(), v.template head<LieGroup1::NV>(), out.template head<LieGroup1::NQ>());
LieGroup2::integrate(q.template tail<LieGroup2::NQ>(), v.template tail<LieGroup2::NV>(), out.template tail<LieGroup2::NQ>());
LieGroup1().integrate(q.head(lg1_.nq()), v.head(lg1_.nv()), out.head(lg1_.nq()));
LieGroup2().integrate(q.tail(lg2_.nq()), v.tail(lg2_.nv()), out.tail(lg2_.nq()));
}
template <class Tangent_t, class JacobianOut_t>
static void Jintegrate_impl(const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J)
void Jintegrate_impl(const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
JacobianOut_t& Jout = const_cast< JacobianOut_t& >(J.derived());
Jout.template topRightCorner<LieGroup1::NV,LieGroup2::NV>().setZero();
Jout.template bottomLeftCorner<LieGroup2::NV,LieGroup1::NV>().setZero();
LieGroup1::Jintegrate(v.template head<LieGroup1::NV>(), Jout.template topLeftCorner<LieGroup1::NV,LieGroup1::NV>());
LieGroup2::Jintegrate(v.template tail<LieGroup2::NV>(), Jout.template bottomRightCorner<LieGroup2::NV,LieGroup2::NV>());
Jout.topRightCorner(lg1_.nv(),lg2_.nv()).setZero();
Jout.bottomLeftCorner(lg2_.nv(),lg1_.nv()).setZero();
lg1_.Jintegrate(v.head(lg1_.nv()), Jout.topLeftCorner(lg1_.nv(),lg1_.nv()));
lg2_.Jintegrate(v.tail(lg2_.nv()), Jout.bottomRightCorner(lg2_.nv(),lg2_.nv()));
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const
{
return LieGroup1::squaredDistance(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>())
+ LieGroup2::squaredDistance(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>());
return lg1_.squaredDistance(q0.head(lg1_.nq()), q1.head(lg1_.nq()))
+ lg2_.squaredDistance(q0.tail(lg2_.nq()), q1.tail(lg2_.nq()));
}
template <class Config_t>
static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
LieGroup1::normalize(qout.derived().template head<LieGroup1::NQ>());
LieGroup2::normalize(qout.derived().template tail<LieGroup2::NQ>());
lg1_.normalize(qout.derived().head(lg1_.nq()));
lg2_.normalize(qout.derived().tail(lg2_.nq()));
}
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
{
Config_t& out = const_cast< Eigen::MatrixBase<Config_t>& > (qout).derived();
LieGroup1 ().random(out.template head<LieGroup1::NQ>());
LieGroup2 ().random(out.template tail<LieGroup2::NQ>());
lg1_.random(out.head(lg1_.nq()));
lg2_.random(out.tail(lg2_.nq()));
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
......@@ -175,21 +175,21 @@ namespace se3
const
{
ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
LieGroup1 ().randomConfiguration(lower.template head<LieGroup1::NQ>(),
upper.template head<LieGroup1::NQ>(),
out.template head<LieGroup1::NQ>());
LieGroup2 ().randomConfiguration(lower.template tail<LieGroup2::NQ>(),
upper.template tail<LieGroup2::NQ>(),
out.template tail<LieGroup2::NQ>());
lg1_.randomConfiguration(lower.head(lg1_.nq()),
upper.head(lg1_.nq()),
out.head(lg1_.nq()));
lg2_.randomConfiguration(lower.tail(lg2_.nq()),
upper.tail(lg2_.nq()),
out.tail(lg2_.nq()));
}
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)
bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
return LieGroup1::isSameConfiguration(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), prec)
+ LieGroup2::isSameConfiguration(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), prec);
return LieGroup1().isSameConfiguration(q0.head(lg1_.nq()), q1.head(lg1_.nq()), prec)
+ LieGroup2().isSameConfiguration(q0.tail(lg2_.nq()), q1.tail(lg2_.nq()), prec);
}
private:
LieGroup1 lg1_;
......
......@@ -72,9 +72,9 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @return The configuration integrated
*/
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
static void integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout);
void integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout) const;
/**
* @brief Computes the Jacobian of the Integrate operation.
......@@ -84,8 +84,8 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @return The Jacobian
*/
template <class Tangent_t, class JacobianOut_t>
static void Jintegrate(const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J);
void Jintegrate(const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const;
/**
* @brief Interpolation between two joint's configurations
......@@ -97,10 +97,10 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @return The interpolated configuration (q0 if u = 0, q1 if u = 1)
*/
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout);
void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout) const;
/**
* @brief Normalize the joint configuration given as input. For instance, the quaternion must unitary.
......@@ -108,7 +108,7 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @return The normalized joint configuration.
*/
template <class Config_t>
static void normalize (const Eigen::MatrixBase<Config_t>& qout);
void normalize (const Eigen::MatrixBase<Config_t>& qout) const;
/**
* @brief Generate a random joint configuration, normalizing quaternions when necessary.
......@@ -145,9 +145,9 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @return The corresponding velocity
*/
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
static void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t>& d);
void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t>& d) const;
/**
* @brief Computes the Jacobian of the difference operation
......@@ -158,10 +158,10 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @return The Jacobian
*/
template <class ConfigL_t, class ConfigR_t, class JacobianLOut_t, class JacobianROut_t>
static void Jdifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianLOut_t>& J0,
const Eigen::MatrixBase<JacobianROut_t>& J1);
void Jdifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianLOut_t>& J0,
const Eigen::MatrixBase<JacobianROut_t>& J1) const;
/**
* @brief Squared distance between two configurations of the joint
......@@ -172,8 +172,8 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @return The corresponding distance
*/
template <class ConfigL_t, class ConfigR_t>
static Scalar squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1);
Scalar squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const;
/**
* @brief Distance between two configurations of the joint
......@@ -184,8 +184,8 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @return The corresponding distance
*/
template <class ConfigL_t, class ConfigR_t>
static Scalar distance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1);
Scalar distance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const;
/**
* @brief Check if two configurations are equivalent within the given precision
......@@ -194,22 +194,22 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @param[in] q1 Configuration 1
*/
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;
/// \}
/// \name API that allocates memory
/// \{
template <class Config_t, class Tangent_t>
static ConfigVector_t integrate(const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Tangent_t> & v);
ConfigVector_t integrate(const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Tangent_t> & v) const ;
template <class ConfigL_t, class ConfigR_t>
static ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u);
ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u) const;
ConfigVector_t random() const;
......@@ -219,8 +219,8 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;
template <class ConfigL_t, class ConfigR_t>
static TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1);
TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const;
/// \}
......@@ -228,22 +228,22 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
/// \{
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);
void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout) const;
template <class Config_t>
static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout);
void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;
template <class ConfigL_t, class ConfigR_t>
static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1);
Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const;
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);
bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec) const;
/// Get dimension of Lie Group vector representation
///
/// For instance, for SO(3), the dimension of the vector representation is
......
......@@ -29,23 +29,23 @@ namespace se3 {
void LieGroupOperationBase<Derived>::integrate(
const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout)
const Eigen::MatrixBase<ConfigOut_t>& qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
Derived::integrate_impl(q, v, qout);
derived().integrate_impl(q, v, qout);
}
template <class Derived>
template <class Tangent_t, class JacobianOut_t>
void LieGroupOperationBase<Derived>::Jintegrate(
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J)
const Eigen::MatrixBase<JacobianOut_t>& J) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
Derived::Jintegrate_impl(v, J);
derived().Jintegrate_impl(v, J);
}
/**
......@@ -63,21 +63,21 @@ namespace se3 {
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
const Eigen::MatrixBase<ConfigOut_t>& qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
Derived::interpolate_impl(q0, q1, u, qout);
Derived().interpolate_impl(q0, q1, u, qout);
}
template <class Derived>
template <class Config_t>
void LieGroupOperationBase<Derived>::normalize
(const Eigen::MatrixBase<Config_t>& qout)
(const Eigen::MatrixBase<Config_t>& qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
return Derived::normalize_impl (qout);
return derived().normalize_impl (qout);
}
/**
......@@ -115,12 +115,12 @@ namespace se3 {
void LieGroupOperationBase<Derived>::difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t>& d)
const Eigen::MatrixBase<Tangent_t>& d) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
Derived::difference_impl(q0, q1, d);
derived().difference_impl(q0, q1, d);
}
template <class Derived>
......@@ -129,13 +129,13 @@ namespace se3 {
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianLOut_t>& J0,
const Eigen::MatrixBase<JacobianROut_t>& J1)
const Eigen::MatrixBase<JacobianROut_t>& J1) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianLOut_t, JacobianMatrix_t);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianROut_t, JacobianMatrix_t);
Derived::Jdifference_impl (q0, q1, J0, J1);
derived().Jdifference_impl (q0, q1, J0, J1);
}
template <class Derived>
......@@ -143,11 +143,11 @@ namespace se3 {
typename LieGroupOperationBase<Derived>::Scalar
LieGroupOperationBase<Derived>::squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
const Eigen::MatrixBase<ConfigR_t> & q1) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
return Derived::squaredDistance_impl(q0, q1);
return derived().squaredDistance_impl(q0, q1);
}
template <class Derived>
......@@ -155,7 +155,7 @@ namespace se3 {
typename LieGroupOperationBase<Derived>::Scalar
LieGroupOperationBase<Derived>::distance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
const Eigen::MatrixBase<ConfigR_t> & q1) const
{
return sqrt(squaredDistance(q0, q1));
}
......@@ -165,11 +165,11 @@ namespace se3 {
bool LieGroupOperationBase<Derived>::isSameConfiguration(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
const Scalar & prec) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
return Derived::isSameConfiguration_impl(q0, q1, prec);
return derived().isSameConfiguration_impl(q0, q1, prec);
}
// ----------------- API that allocates memory ---------------------------- //
......@@ -177,9 +177,9 @@ namespace se3 {
template <class Derived>
template <class Config_t, class Tangent_t>
typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::integrate(
const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Tangent_t> & v)
typename LieGroupOperationBase<Derived>::ConfigVector_t
LieGroupOperationBase<Derived>::integrate(const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Tangent_t> & v) const
{
ConfigVector_t qout;
integrate(q, v, qout);
......@@ -191,7 +191,7 @@ namespace se3 {
typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u)
const Scalar& u) const
{
ConfigVector_t qout;
interpolate(q0, q1, u, qout);
......@@ -223,7 +223,7 @@ namespace se3 {
template <class ConfigL_t, class ConfigR_t>
typename LieGroupOperationBase<Derived>::TangentVector_t LieGroupOperationBase<Derived>::difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
const Eigen::MatrixBase<ConfigR_t> & q1) const
{
TangentVector_t diff;
difference(q0, q1, diff);
......@@ -237,7 +237,7 @@ namespace se3 {
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
const Eigen::MatrixBase<ConfigOut_t>& qout) const
{
if (u == 0) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q0;
else if(u == 1) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q1;
......@@ -249,7 +249,7 @@ namespace se3 {
typename LieGroupOperationBase<Derived>::Scalar
LieGroupOperationBase<Derived>::squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
const Eigen::MatrixBase<ConfigR_t> & q1) const
{
TangentVector_t t;
difference(q0, q1, t);
......@@ -261,7 +261,7 @@ namespace se3 {
bool LieGroupOperationBase<Derived>::isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
const Scalar & prec) const
{
return q0.isApprox(q1, prec);
}
......
......@@ -270,7 +270,7 @@ namespace se3
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
return R2crossSO2_t::isSameConfiguration(q0, q1, prec);
return R2crossSO2_t().isSameConfiguration(q0, q1, prec);
}
private:
......@@ -443,7 +443,7 @@ namespace se3
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
return R3crossSO3_t::isSameConfiguration(q0, q1, prec);
return R3crossSO3_t().isSameConfiguration(q0, q1, prec);
}
}; // struct SpecialEuclideanOperation<3>
......
......@@ -121,8 +121,8 @@ namespace se3
R(0,1) = - R(1,0);
Scalar w (Jlog(R));
const_cast< JacobianLOut_t& > (J0.derived()).coeffRef(0) = -w;
const_cast< JacobianROut_t& > (J1.derived()).coeffRef(0) = w;
const_cast< JacobianLOut_t& > (J0.derived()).coeffRef(0,0) = -w;
const_cast< JacobianROut_t& > (J1.derived()).coeffRef(0,0) = w;
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
......@@ -150,7 +150,7 @@ namespace se3
const Eigen::MatrixBase<JacobianOut_t>& J)
{
JacobianOut_t& Jout = const_cast< JacobianOut_t& >(J.derived());
Jout(0) = 1;
Jout(0,0) = 1;
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
......
......@@ -59,7 +59,7 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
SE3 M1 = jdata.M;
Motion v1 = jdata.v;
q2 = LieGroupType::integrate(q1,q1_dot);
q2 = LieGroupType().integrate(q1,q1_dot);
jmodel.calc(jdata,q2);
SE3 M2 = jdata.M;
......@@ -71,22 +71,22 @@ void test_lie_group_methods (T & jmodel, typename T::JointDataDerived &)
}
// Check the reversability of integrate