Commit 475a8af0 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Add specific return type for toVector method

parent e69978b3
......@@ -37,11 +37,10 @@ namespace se3
{
typedef double Scalar;
typedef Eigen::Matrix<double,3,1,0> Vector3;
typedef Eigen::Matrix<double,4,1,0> Vector4;
typedef Eigen::Matrix<double,6,1,0> Vector6;
typedef Eigen::Matrix<double,3,3,0> Matrix3;
typedef Eigen::Matrix<double,4,4,0> Matrix4;
typedef Eigen::Matrix<double,6,6,0> Matrix6;
typedef EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
......
......@@ -38,6 +38,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,3,1,0> Vector3;
typedef Eigen::Matrix<Scalar,6,1,0> Vector6;
typedef Eigen::Matrix<Scalar,6,6,0> Matrix6;
typedef EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
......
......@@ -61,6 +61,8 @@ namespace se3
typedef Eigen::Matrix<double,3,1,0> Vector3;
typedef Eigen::Matrix<double,6,1,0> Vector6;
typedef Eigen::Matrix<double,6,6,0> Matrix6;
typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
......
......@@ -37,6 +37,8 @@ namespace se3
typedef Eigen::Matrix<double,3,1,0> Vector3;
typedef Eigen::Matrix<double,6,1,0> Vector6;
typedef Eigen::Matrix<double,6,6,0> Matrix6;
typedef EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
......
......@@ -64,6 +64,8 @@ namespace se3
typedef Eigen::Matrix<double,3,1,0> Vector3;
typedef Eigen::Matrix<double,6,1,0> Vector6;
typedef Eigen::Matrix<double,6,6,0> Matrix6;
typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
......
......@@ -41,6 +41,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
typedef Eigen::Matrix<Scalar,6,1,_Options> Vector6;
typedef Eigen::Matrix<Scalar,6,6,_Options> Matrix6;
typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Matrix6 ActionMatrixType;
typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
......
......@@ -41,6 +41,8 @@ namespace se3
typedef Eigen::Matrix<double,3,1,0> Vector3;
typedef Eigen::Matrix<double,6,1,0> Vector6;
typedef Eigen::Matrix<double,6,6,0> Matrix6;
typedef EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
......
......@@ -38,6 +38,8 @@ namespace se3
typedef Eigen::Matrix<double,3,1,0> Vector3;
typedef Eigen::Matrix<double,6,1,0> Vector6;
typedef Eigen::Matrix<double,6,6,0> Matrix6;
typedef EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Vector3 AngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstAngularType;
......
......@@ -37,12 +37,12 @@ namespace se3
LinearType linear() { return derived().linear_impl(); }
template<typename V3>
void angular(const Eigen::MatrixBase<V3> & w) { derived().angular_impl(w); }
void angular(const Eigen::MatrixBase<V3> & w) { derived().angular_impl(w.derived()); }
template<typename V3>
void linear(const Eigen::MatrixBase<V3> & v) { derived().linear_impl(v); }
void linear(const Eigen::MatrixBase<V3> & v) { derived().linear_impl(v.derived()); }
const Vector6 & toVector() const { return derived().toVector_impl(); }
Vector6 & toVector() { return derived().toVector_impl(); }
ToVectorConstReturnType toVector() const { return derived().toVector_impl(); }
ToVectorReturnType toVector() { return derived().toVector_impl(); }
operator Vector6 () const { return toVector(); }
ActionMatrixType toActionMatrix() const { return derived().toActionMatrix_impl(); }
......
......@@ -41,7 +41,9 @@ namespace se3
typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
typedef MotionTpl<Scalar,Options> MotionPlain;
typedef typename EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
typedef DataRefType ToVectorReturnType;
typedef typename EIGEN_REF_CONSTTYPE(Vector6ArgType) ConstDataRefType;
typedef ConstDataRefType ToVectorConstReturnType;
}; // traits MotionRef
......
......@@ -29,6 +29,8 @@ namespace se3
typedef Eigen::Matrix<Scalar,6,1,_Options> Vector6;
typedef Eigen::Matrix<Scalar,6,6,_Options> Matrix6;
typedef Matrix6 ActionMatrixType;
typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
......@@ -50,6 +52,8 @@ namespace se3
MOTION_TYPEDEF_TPL(MotionTpl);
using Base::operator=;
using Base::linear;
using Base::angular;
// Constructors
MotionTpl() : data() {}
......@@ -75,12 +79,16 @@ namespace se3
: data(clone.toVector())
{}
template<typename M2>
explicit MotionTpl(const MotionDense<M2> & clone)
{ linear() = clone.linear(); angular() = clone.angular(); }
// initializers
static MotionTpl Zero() { return MotionTpl(Vector6::Zero()); }
static MotionTpl Random() { return MotionTpl(Vector6::Random()); }
const Vector6 & toVector_impl() const { return data; }
Vector6 & toVector_impl() { return data; }
ToVectorConstReturnType toVector_impl() const { return data; }
ToVectorReturnType toVector_impl() { return data; }
// Getters
ConstAngularType angular_impl() const { return data.template segment<3> (ANGULAR); }
......@@ -89,9 +97,17 @@ namespace se3
LinearType linear_impl() { return data.template segment<3> (LINEAR); }
template<typename V3>
void angular_impl(const Eigen::MatrixBase<V3> & w) { angular_impl()=w; }
void angular_impl(const Eigen::MatrixBase<V3> & w)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
angular_impl()=w;
}
template<typename V3>
void linear_impl(const Eigen::MatrixBase<V3> & v) { linear_impl()=v; }
void linear_impl(const Eigen::MatrixBase<V3> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
linear_impl()=v;
}
protected:
Vector6 data;
......
......@@ -30,6 +30,8 @@ namespace se3
typedef Eigen::Matrix<double,6,1,0> Vector6;
typedef Eigen::Matrix<double,3,3,0> Matrix3;
typedef Eigen::Matrix<double,6,6,0> Matrix6;
typedef EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Matrix6 ActionMatrixType;
typedef Vector3 AngularType;
typedef const Vector3 ConstAngularType;
......
......@@ -31,6 +31,8 @@ typedef TYPENAME traits<Derived>::Scalar Scalar; \
typedef TYPENAME traits<Derived>::Vector3 Vector3; \
typedef TYPENAME traits<Derived>::Vector6 Vector6; \
typedef TYPENAME traits<Derived>::Matrix6 Matrix6; \
typedef TYPENAME traits<Derived>::ToVectorReturnType ToVectorReturnType; \
typedef TYPENAME traits<Derived>::ToVectorConstReturnType ToVectorConstReturnType; \
typedef TYPENAME traits<Derived>::AngularType AngularType; \
typedef TYPENAME traits<Derived>::LinearType LinearType; \
typedef TYPENAME traits<Derived>::ConstAngularType ConstAngularType; \
......
Supports Markdown
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