19 #ifndef __se3_motion_ref_hpp__ 20 #define __se3_motion_ref_hpp__ 25 template<
typename Vector6ArgType>
28 typedef typename Vector6ArgType::Scalar Scalar;
33 Options = Vector6::Options
35 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
36 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
37 typedef Matrix6 ActionMatrixType;
38 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
39 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
40 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
41 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
43 typedef typename EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
44 typedef DataRefType ToVectorReturnType;
45 typedef typename EIGEN_REF_CONSTTYPE(Vector6ArgType) ConstDataRefType;
46 typedef ConstDataRefType ToVectorConstReturnType;
53 template<
typename Vector6ArgType>
54 struct SE3GroupAction< MotionRef<Vector6ArgType> >
59 template<
typename Vector6ArgType,
typename MotionDerived>
60 struct MotionAlgebraAction< MotionRef<Vector6ArgType>, MotionDerived >
65 template<
typename Vector6ArgType>
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
74 using Base::operator=;
79 using Base::__opposite__;
80 using Base::__minus__;
85 MotionRef(
const Eigen::MatrixBase<Vector6ArgType> & v_like)
86 : m_ref(const_cast<Vector6ArgType &>(v_like.derived()))
88 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
89 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
90 assert(v_like.size() == 6);
93 ToVectorConstReturnType toVector_impl()
const {
return m_ref; }
94 ToVectorReturnType toVector_impl() {
return m_ref; }
97 ConstAngularType angular_impl()
const {
return ConstAngularType(m_ref.derived(),ANGULAR); }
98 ConstLinearType linear_impl()
const {
return ConstLinearType(m_ref.derived(),LINEAR); }
99 AngularType angular_impl() {
return m_ref.template segment<3> (ANGULAR); }
100 LinearType linear_impl() {
return m_ref.template segment<3> (LINEAR); }
102 template<
typename V3>
103 void angular_impl(
const Eigen::MatrixBase<V3> & w)
105 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
109 template<
typename V3>
110 void linear_impl(
const Eigen::MatrixBase<V3> & v)
112 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
117 template<
typename S1,
int O1>
121 template<
typename Vector6Like>
125 template<
typename S1,
int O1>
129 template<
typename Vector6Like>
133 template<
typename S1,
int O1>
135 { m_ref += v.toVector();
return *
this; }
137 template<
typename Vector6Like>
139 { m_ref += v.toVector();
return *
this; }
141 template<
typename S1,
int O1>
143 { m_ref -= v.toVector();
return *
this; }
145 template<
typename Vector6Like>
147 { m_ref -= v.toVector();
return *
this; }
149 template<
typename OtherScalar>
150 MotionPlain __mult__(
const OtherScalar & alpha)
const 162 #endif // ifndef __se3_motion_ref_hpp__ EIGEN_PLAIN_TYPE(Matrix3x) cross(const Eigen
Applies the cross product onto the columns of M.