33 #ifndef TOON_INCLUDE_SE2_H
34 #define TOON_INCLUDE_SE2_H
51 template <
typename Precision =
double>
71 template <
int S,
typename P,
typename A>
118 template<
typename Accessor>
122 result.template slice<0,2>() =
my_rotation * vect.template slice<0,2>();
124 result[1] -= vect[2] * my_translation[0];
128 template <
typename Accessor>
131 for(
int i=0; i<3; ++i)
132 result.T()[i] =
adjoint(M.T()[i]);
133 for(
int i=0; i<3; ++i)
134 result[i] =
adjoint(result[i]);
145 template <
class Precision>
146 inline std::ostream& operator<<(std::ostream& os, const SE2<Precision> & rhs){
147 std::streamsize fw = os.width();
148 for(
int i=0; i<2; i++){
150 os << rhs.get_rotation().get_matrix()[i];
152 os << rhs.get_translation()[i] <<
'\n';
159 template <
class Precision>
161 for(
int i=0; i<2; i++)
174 template<
int S,
typename P,
typename PV,
typename A>
178 template<
int S,
typename P,
typename PV,
typename A>
185 template <
int S0,
typename P0,
typename A0>
188 res.template slice<0,2>()=lhs.
get_rotation()*rhs.template slice<0,2>();
197 template<
int S,
typename P,
typename PV,
typename A>
204 template <
typename P,
typename PV,
typename A>
215 template<
int S,
typename P,
typename PV,
typename A>
219 template<
int S,
typename P,
typename PV,
typename A>
226 template <
int S0,
typename P0,
typename A0>
229 res.template slice<0,2>() = lhs.template slice<0,2>()*rhs.
get_rotation();
238 template<
int S,
typename P,
typename PV,
typename A>
249 template <
int R,
int C,
typename PM,
typename A,
typename P>
253 template<
int R,
int Cols,
typename PM,
typename A,
typename P>
254 struct Operator<Internal::SE2MMult<R, Cols, PM, A, P> > {
260 template <
int R0,
int C0,
typename P0,
typename A0>
263 for(
int i=0; i<rhs.num_cols(); ++i)
264 res.T()[i] = lhs * rhs.T()[i];
272 template <
int R,
int Cols,
typename PM,
typename A,
typename P>
283 template <
int Rows,
int C,
typename PM,
typename A,
typename P>
287 template<
int Rows,
int C,
typename PM,
typename A,
typename P>
288 struct Operator<Internal::MSE2Mult<Rows, C, PM, A, P> > {
294 template <
int R0,
int C0,
typename P0,
typename A0>
297 for(
int i=0; i<lhs.num_rows(); ++i)
298 res[i] = lhs[i] * rhs;
306 template <
int Rows,
int C,
typename PM,
typename A,
typename P>
311 template <
typename Precision>
312 template <
int S,
typename PV,
typename Accessor>
317 static const Precision one_6th = 1.0/6.0;
318 static const Precision one_20th = 1.0/20.0;
322 const Precision theta = mu[2];
323 const Precision theta_sq = theta * theta;
328 if (theta_sq < 1e-8){
332 if (theta_sq < 1e-6) {
333 A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
334 B = 0.5 - 0.25 * one_6th * theta_sq;
336 const Precision inv_theta = (1.0/theta);
337 const Precision sine = result.
my_rotation.get_matrix()[1][0];
338 const Precision cosine = result.
my_rotation.get_matrix()[0][0];
339 A = sine * inv_theta;
340 B = (1 - cosine) * (inv_theta * inv_theta);
347 template <
typename Precision>
351 Precision shtot = 0.5;
352 if(fabs(theta) > 0.00001)
353 shtot = sin(theta/2)/theta;
357 result.template slice<0,2>() = (halfrotator * se2.
get_translation())/(2 * shtot);
365 template <
typename Precision>
const Matrix< Rows, C, PM, A > & lhs
SE2 inverse() const
compute the inverse of the transformation
const SO2< Precision > & get_rotation() const
Operator(const Vector< S, PV, A > &l, const SE2< P > &r)
void eval(Matrix< R0, C0, P0, A0 > &res) const
Vector< 3, typename Internal::MultiplyType< PV, P >::type > operator*(const Vector< S, PV, A > &lhs, const SE2< P > &rhs)
Everything lives inside this namespace.
void eval(Vector< S0, P0, A0 > &res) const
static void test(int s1, int s2)
Matrix< 3, Cols, typename Internal::MultiplyType< P, PM >::type > operator*(const SE2< P > &lhs, const Matrix< R, Cols, PM, A > &rhs)
SO2< Precision > & get_rotation()
Returns the rotation part of the transformation as a SO2.
std::istream & operator>>(std::istream &is, SE2< Precision > &rhs)
SE2< typename Internal::MultiplyType< Precision, P >::type > operator*(const SE2< P > &rhs) const
SE2()
Default constructor. Initialises the the rotation to zero (the identity) and the translation to zero...
SE2 & operator*=(const SE2 &rhs)
SE2< Precision > operator*(const SO2< Precision > &lhs, const SE2< Precision > &rhs)
void eval(Vector< S0, P0, A0 > &res) const
const Vector< S, PV, A > & rhs
static SE2 exp(const Vector< S, P, A > &vect)
Vector< 2, Precision > & get_translation()
Returns the translation part of the transformation as a Vector.
SE2(const SO2< Precision > &R, const Vector< 2, Precision, A > &T)
Vector< 1 > makeVector(double x1)
SE2(const Vector< S, P, A > &v)
Matrix< 3, 3, Precision > adjoint(const Matrix< 3, 3, Precision, Accessor > &M) const
void eval(Matrix< R0, C0, P0, A0 > &res) const
Vector< 3, Precision > ln() const
static Matrix< 3, 3, Precision > generator(int i)
const Matrix< R, Cols, PM, A > & rhs
SO2< Precision > my_rotation
const Vector< S, PV, A > & lhs
const Vector< 2, Precision > & get_translation() const
Operator(const Matrix< Rows, C, PM, A > &l, const SE2< P > &r)
Vector< 3, typename Internal::MultiplyType< P, PV >::type > operator*(const SE2< P > &lhs, const Vector< S, PV, A > &rhs)
Vector< 2, typename Internal::MultiplyType< P, PV >::type > operator*(const SE2< P > &lhs, const Vector< 2, PV, A > &rhs)
Vector< 2, Precision > my_translation
Matrix< Rows, 3, typename Internal::MultiplyType< PM, P >::type > operator*(const Matrix< Rows, C, PM, A > &lhs, const SE2< P > &rhs)
static SO2 exp(const Precision &d)
Exponentiate an angle in the Lie algebra to generate a new SO2.
Vector< 3, Precision > adjoint(const Vector< 3, Precision, Accessor > &vect) const
Vector< Internal::Sizer< S1, S2 >::size, typename Internal::MultiplyType< P1, P2 >::type > operator*(const DiagonalMatrix< S1, P1, B1 > &d, const Vector< S2, P2, B2 > &v)
Operator(const SE2< P > &l, const Matrix< R, Cols, PM, A > &r)
Operator(const SE2< P > &l, const Vector< S, PV, A > &r)
static Operator< Internal::Zero > Zeros