MIRA
RigidTransform.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 by
3  * MetraLabs GmbH (MLAB), GERMANY
4  * and
5  * Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
6  * All rights reserved.
7  *
8  * Contact: info@mira-project.org
9  *
10  * Commercial Usage:
11  * Licensees holding valid commercial licenses may use this file in
12  * accordance with the commercial license agreement provided with the
13  * software or, alternatively, in accordance with the terms contained in
14  * a written agreement between you and MLAB or NICR.
15  *
16  * GNU General Public License Usage:
17  * Alternatively, this file may be used under the terms of the GNU
18  * General Public License version 3.0 as published by the Free Software
19  * Foundation and appearing in the file LICENSE.GPL3 included in the
20  * packaging of this file. Please review the following information to
21  * ensure the GNU General Public License version 3.0 requirements will be
22  * met: http://www.gnu.org/copyleft/gpl.html.
23  * Alternatively you may (at your option) use any later version of the GNU
24  * General Public License if such license has been publicly approved by
25  * MLAB and NICR (or its successors, if any).
26  *
27  * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
28  * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
29  * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
30  * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *
32  * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
33  * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
34  * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
35  * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
36  * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
37  */
38 
47 #ifndef _MIRA_RIGID_TRANSFORM_H_
48 #define _MIRA_RIGID_TRANSFORM_H_
49 
50 #include <algorithm>
51 #include <limits>
52 
53 #include <math/Angle.h>
54 #include <math/Eigen.h>
55 #include <math/Lerp.h>
56 #include <math/YawPitchRoll.h>
57 
59 
60 #include <platform/Types.h>
61 
62 namespace mira {
63 
65 
67 
70 template< int D,
71  typename TransformDerived,
72  typename OtherDerived,
73  int OtherRows=OtherDerived::RowsAtCompileTime,
74  int OtherCols=OtherDerived::ColsAtCompileTime>
75 struct ei_rigidtransform_product_impl
76 {
77  static_assert(D!=D,"You are trying to apply a rigid transform to a matrix "
78  "of invalid size");
79 };
81 
83 
98 template <typename T, int D>
100 {
101  static_assert(D!=D,"RigidTransform is not defined for this dimension. "
102  "Only 2 and 3 dimensions are available.");
103 };
104 
106 
125 template <typename T, int D>
127 {
128  static_assert(D!=D,"RigidTransformCov is not defined for this dimension. "
129  "Only 2 and 3 dimensions are available.");
130 };
131 
133 
138 template <typename T, int D, typename TRotation, typename TransformDerived>
140 {
141 public:
142 
144  typedef T Type;
145 
151 
156  typedef TRotation RotationType;
157 
158  enum {
159  Dim = D,
160  HDim = D+1
161  };
162 
168 
169 public:
170 
171  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
172 
174  t(translation), r(rotation) {}
175 
176 
177 public:
178 
180  TransformDerived inverse() const
181  {
182  RotationType inv = r.inverse();
183  return TransformDerived(inv.RotationType::operator*(-t), inv);
184  }
185 
191  bool isApprox(const RigidTransformBase& other,
192  T prec = std::numeric_limits<T>::epsilon()) const
193  {
194  return t.isApprox(other.t,prec) && r.isApprox(other.r, prec);
195  }
196 
197 public:
198 
202  TransformDerived& operator*= (const RigidTransformBase& other)
203  {
204  t += r * other.t;
205  r *= other.r;
206  return *This();
207  }
208 
212  friend TransformDerived operator* (const RigidTransformBase& a,
213  const RigidTransformBase& b) {
214  return mul(a,b);
215  }
216 
222  template <typename OtherDerived>
223  typename ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived>::TResult
225  {
226  return ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived>::run(*this, other.derived());
227  }
228 
229 public:
230 
236  {
237  // order of transforms is from right to left (i.e. rotate first and
238  // translate afterwards)
239  return Eigen::Translation<T,D>(t)*r;
240  }
241 
242 public:
243 
251  {
252  MatrixType m = MatrixType::Identity ();
253  m.template block<Dim,Dim>(0,0) = r.toRotationMatrix();
254  m.template block<Dim,1>(0,Dim) = t.transpose();
255  return m;
256  }
257 
258 protected:
259 
261  TransformDerived* This() { return static_cast<TransformDerived*>(this); }
263  const TransformDerived* This() const {
264  return static_cast<const TransformDerived*>(this);
265  }
266 
267 protected:
268 
269  static TransformDerived mul(const RigidTransformBase& a,
270  const RigidTransformBase& b)
271  {
272  RotationType r = a.r*b.r; // this must be assigned to a temp variable
273  // (due to a bug in Eigen::Rotation2D<>? )
274  return TransformDerived(a.r * b.t + a.t, r);
275  }
276 
277 public:
278 
281 
284 };
285 
287 
294 template <typename T>
295 class RigidTransform<T,2> : public RigidTransformBase<T, 2,
296  Eigen::Rotation2D<T>, RigidTransform<T,2> >
297 {
299 
300 public:
301 
303  Base(Eigen::Matrix<T,2,1>(0,0),Eigen::Rotation2D<T>(0)) {}
304 
310  const Eigen::Rotation2D<T>& rotation) :
311  Base(translation,rotation) {}
312 
317  RigidTransform(const Eigen::Matrix<T,2,1>& translation, T angle) :
318  Base(translation,Eigen::Rotation2D<T>(angle)) {}
319 
324  RigidTransform(T x, T y, T angle) :
325  Base(Eigen::Matrix<T,2,1>(x,y),
326  Eigen::Rotation2D<T>(angle)) {}
327 
333  template <typename UnitTag, typename Derived>
335  Base(Eigen::Matrix<T,2,1>(x,y),
336  Eigen::Rotation2D<T>(angle.rad())) {}
337 
338 public:
339 
341  template <typename U>
343  return RigidTransform<U,2>(this->t.template cast<U>(),
344  this->r.template cast<U>());
345  }
346 
347 public:
348 
350  T& x() { return this->t.x(); }
352  T x() const { return this->t.x(); }
353 
355  T& y() { return this->t.y(); }
357  T y() const { return this->t.y(); }
358 
360  T& phi() { return this->r.angle(); }
362  T phi() const { return this->r.angle(); }
363 
364 public:
365 
366  template <typename Reflector>
367  void reflect(Reflector& m)
368  {
369  m.property("X", this->t[0], "The translational part of the transform");
370  m.property("Y", this->t[1], "The translational part of the transform");
371  m.property("Phi", getter<T>(rad2deg<T>, this->r.angle()),
372  setter<T>(deg2rad<T>, this->r.angle()),
373  "The orientation in degrees");
374  }
375 
376  friend std::ostream& operator<<(std::ostream& os, const RigidTransform& tf)
377  {
378  os << "t: " << tf.t.transpose() << "\n";
379  os << "r: " << tf.r.angle() << "\n";
380  return os;
381  }
382 };
383 
385 
401 template <typename T>
402 class RigidTransformCov<T,2> : public RigidTransform<T, 2>
403 {
404  typedef RigidTransform<T, 2> Base;
405 
406 public:
407 
409  static const int CovarianceDim = 3;
410 
416 
421  return CovMatrixType::Zero();
422  }
423 
424 public:
425 
426  RigidTransformCov() : Base(), cov(nullCov())
427  {}
428 
433  RigidTransformCov(const Base& transform) : Base(transform),
434  cov(nullCov()) {}
435 
441  const Eigen::Rotation2D<T>& rotation,
442  const CovMatrixType& covariance) :
443  Base(translation,rotation), cov(covariance) {}
444 
449  RigidTransformCov(const Eigen::Matrix<T,2,1>& translation, T angle,
450  const CovMatrixType& covariance) :
451  Base(translation,Eigen::Rotation2D<T>(angle)),
452  cov(covariance) {}
453 
458  RigidTransformCov(T x, T y, T angle, const CovMatrixType& covariance) :
459  Base(Eigen::Matrix<T,2,1>(x,y),
460  Eigen::Rotation2D<T>(angle)), cov(covariance) {}
461 
467  template <typename UnitTag, typename Derived>
469  const CovMatrixType& covariance) :
470  Base(Eigen::Matrix<T,2,1>(x,y),
471  Eigen::Rotation2D<T>(angle.rad())), cov(covariance) {}
472 
479  RigidTransformCov(T x, T y, T angle, T covX, T covY, T covAngle) :
480  Base(Eigen::Matrix<T,2,1>(x,y),
481  Eigen::Rotation2D<T>(angle)),
482  cov(Eigen::DiagonalMatrix<T,3>(covX, covY, covAngle)) {}
483 
489  template <typename UnitTag, typename Derived>
491  T covX, T covY, T covAngle) :
492  Base(Eigen::Matrix<T,2,1>(x,y),
493  Eigen::Rotation2D<T>(angle.rad())),
494  cov(Eigen::DiagonalMatrix<T,3>(covX, covY, covAngle)) {}
495 
496 public:
497 
499  template <typename U>
501  return RigidTransformCov<U,2>(this->t.template cast<U>(),
502  this->r.template cast<U>(),
503  this->cov.template cast<U>());
504  }
505 
506 public:
507 
508  template <typename Reflector>
509  void reflect(Reflector& r)
510  {
512  r.member("Cov", cov, "The covariance matrix (using radians for phi)");
513  }
514 
515  friend std::ostream& operator<<(std::ostream& os, const RigidTransformCov& tf)
516  {
517  os << "t: " << tf.t.transpose() << "\n";
518  os << "r: " << tf.r.angle() << "\n";
519  os << "cov:\n" << tf.cov << "\n";
520  return os;
521  }
522 
523 public:
524  // overwritten to implement covariance propagation
525 
527  {
528  typedef typename Base::RotationType RotationType;
529  RotationType inv = this->r.inverse();
530 
531  CovMatrixType J;
532 
533  T sinr = std::sin(this->r.angle());
534  T cosr = std::cos(this->r.angle());
535 
536  // compute the jacobians
537  J << -cosr, -sinr, this->t(0)*sinr -this->t(1)*cosr,
538  sinr, -cosr, this->t(0)*cosr +this->t(1)*sinr,
539  0, 0, -1;
540 
541  CovMatrixType c = J*cov*J.transpose();
542  return RigidTransformCov(inv.RotationType::operator*(-this->t), inv, c);
543  }
544 
545  RigidTransformCov& operator*= (const RigidTransformCov& other)
546  {
547  propagateCov(cov,*this,other); //< must be done here, since operator* changes our value
548  Base::operator*=(other);
549  return *this;
550  }
551 
552  RigidTransformCov& operator*= (const Base& other)
553  {
554  propagateCov(cov,*this,other); //< must be done here, since operator* changes our value
555  Base::operator*=(other);
556  return *this;
557  }
558 
559  friend RigidTransformCov operator* (const RigidTransformCov& a,
560  const RigidTransformCov& b)
561  {
562  RigidTransformCov t = Base::mul(a,b);
563  propagateCov(t.cov,a,b);
564  return t;
565  }
566 
567  friend RigidTransformCov operator* (const RigidTransformCov& a,
568  const Base& b)
569  {
570  RigidTransformCov t = Base::mul(a,b);
571  propagateCov(t.cov,a,b);
572  return t;
573  }
574 
575  friend RigidTransformCov operator* (const Base& a,
576  const RigidTransformCov& b)
577  {
578  RigidTransformCov t = Base::mul(a,b);
579  propagateCov(t.cov,a,b);
580  return t;
581  }
582 
583 private:
584 
585  // propagates the covariance matices of both transforms (impl. see below)
586  static void propagateCov(CovMatrixType& oC, const RigidTransformCov& a,
587  const RigidTransformCov& w);
588  // same as above, but w does not carry any covariance (impl. see below)
589  static void propagateCov(CovMatrixType& oC, const RigidTransformCov& a,
590  const Base& w);
591  // same as above, but a does not carry any covariance (impl. see below)
592  static void propagateCov(CovMatrixType& C, const Base& a,
593  const RigidTransformCov& w);
594 
595 public:
596 
599 };
600 
602 
609 template <typename T>
610 class RigidTransform<T,3> : public RigidTransformBase<T, 3,
611  Eigen::Quaternion<T>, RigidTransform<T,3> >
612 {
614 
615 public:
616 
618  Base(Eigen::Matrix<T,3,1>(0,0,0), Eigen::Quaternion<T>::Identity()) {}
619 
621  const Eigen::Quaternion<T>& rotation) :
622  Base(translation,rotation) {}
623 
624  RigidTransform(T x, T y, T z, T yaw, T pitch, T roll) :
625  Base(Eigen::Matrix<T,3,1>(x,y,z),
626  quaternionFromYawPitchRoll(yaw,pitch,roll)) {}
627 
628 public:
629 
631  template <typename U>
633  return RigidTransform<U,3>(this->t.template cast<U>(),
634  this->r.template cast<U>());
635  }
636 
637 public:
638 
640  T& x() { return this->t.x(); }
642  T x() const { return this->t.x(); }
643 
645  T& y() { return this->t.y(); }
647  T y() const { return this->t.y(); }
648 
650  T& z() { return this->t.z(); }
652  T z() const { return this->t.z(); }
653 
654 
661  T yaw() const { return eulerAngles(this->r.toRotationMatrix(), 2,1,0)(0,0); }
662 
669  T pitch() const { return eulerAngles(this->r.toRotationMatrix(), 2,1,0)(1,0); }
670 
677  T roll() const { return eulerAngles(this->r.toRotationMatrix(), 2,1,0)(2,0); }
678 
679 public:
680 
681  template <typename Derived>
683  {
684  r.property("X", this->t[0], "The translational part of the transform");
685  r.property("Y", this->t[1], "The translational part of the transform");
686  r.property("Z", this->t[2], "The translational part of the transform");
687  // rotation
688  r.member("Rotation", this->r.coeffs(), "The rotational part");
689  }
690 
691  template <typename Derived>
693  {
694  r.property("X", this->t[0], "The translational part of the transform");
695  r.property("Y", this->t[1], "The translational part of the transform");
696  r.property("Z", this->t[2], "The translational part of the transform");
697  // rotation
698  r.member("Rotation", this->r.coeffs(), "The rotational part");
699  }
700 
701  template <typename Reflector>
702  void reflectRead(Reflector& r)
703  {
704  r.property("X", this->t[0], "The translational part of the transform");
705  r.property("Y", this->t[1], "The translational part of the transform");
706  r.property("Z", this->t[2], "The translational part of the transform");
707 
708  auto ypr = quaternionToYawPitchRoll(this->r);
709  T yaw = rad2deg(ypr.template get<0>());
710  T pitch = rad2deg(ypr.template get<1>());
711  T roll = rad2deg(ypr.template get<2>());
712  r.member("Yaw" , yaw, "The yaw angle (phi, in degrees)");
713  r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
714  r.member("Roll" , roll, "The roll angle (psi, in degrees)");
715  }
716 
717  template <typename Reflector>
718  void reflectWrite(Reflector& r)
719  {
720  T yaw, pitch, roll;
721  r.property("X", this->t[0], "The translational part of the transform");
722  r.property("Y", this->t[1], "The translational part of the transform");
723  r.property("Z", this->t[2], "The translational part of the transform");
724  r.member("Yaw" , yaw, "The yaw angle (phi, in degrees)");
725  r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
726  r.member("Roll" , roll, "The roll angle (psi, in degrees)");
727  this->r = quaternionFromYawPitchRoll(deg2rad(yaw),deg2rad(pitch),deg2rad(roll));
728  }
729 
731 
732  friend std::ostream& operator<<(std::ostream& os, const RigidTransform& tf)
733  {
734  os << "t: " << tf.t.transpose() << "\n";
735  os << "r: " << tf.r.w() << " " << tf.r.x() << " " << tf.r.y() << " " << tf.r.z() << "\n";
736  return os;
737  }
738 };
739 
741 
760 template <typename T>
761 class RigidTransformCov<T,3> : public RigidTransform<T, 3>
762 {
763  typedef RigidTransform<T, 3> Base;
764 
765 public:
767  static const int CovarianceDim = 7;
768 
770  static const int EulerCovarianceDim = 6;
771 
777 
783 
784 public:
785 
790  return CovMatrixType::Zero();
791  }
792 
798  return YawPitchRollCovMatrixType::Zero();
799  }
800 
801 public:
802 
803  RigidTransformCov() : Base(), cov(nullCov()) {}
804 
809  RigidTransformCov(const Base& transform) : Base(transform),
810  cov(nullCov()) {}
811 
817  const Eigen::Quaternion<T>& rotation,
818  const CovMatrixType& covariance) :
819  Base(translation,rotation), cov(covariance) {}
820 
826  const Eigen::Quaternion<T>& rotation,
827  const YawPitchRollCovMatrixType& yawPitchRollCov) :
828  Base(translation, rotation),
829  cov(quaternionCovFromYawPitchRollCov(yawPitchRollCov, rotation)){}
830 
835  RigidTransformCov(T x, T y, T z, T yaw, T pitch, T roll,
836  const YawPitchRollCovMatrixType& yawPitchRollCov) {
837  Eigen::Quaternion<T> rotation;
838  quaternionCovFromYawPitchRollCov(yawPitchRollCov, yaw, pitch, roll,
839  rotation, cov);
840  this->t = Eigen::Matrix<T,3,1>(x,y,z);
841  this->r = rotation;
842  }
843 
844 public:
845 
847  template <typename U>
849  typedef typename RigidTransformCov<U,3>::CovMatrixType CovMatrixTypeU;
850  return RigidTransformCov<U,3>(this->t.template cast<U>(),
851  this->r.template cast<U>(),
852  CovMatrixTypeU(this->cov.template cast<U>()));
853  }
854 
855 public:
856 
862  return quaternionCovToYawPitchRollCov(cov,this->r);
863  }
864 
865 public:
866 
867  template <typename Derived>
869  {
870  r.property("X", this->t[0], "The translational part of the transform");
871  r.property("Y", this->t[1], "The translational part of the transform");
872  r.property("Z", this->t[2], "The translational part of the transform");
873  // rotation
874  r.member("Rotation", this->r.coeffs(), "The rotational part");
875  // 7x7 cov matrix
876  r.member("Cov", cov, "The covariance matrix");
877  }
878 
879  template <typename Derived>
881  {
882  r.property("X", this->t[0], "The translational part of the transform");
883  r.property("Y", this->t[1], "The translational part of the transform");
884  r.property("Z", this->t[2], "The translational part of the transform");
885  // rotation
886  r.member("Rotation", this->r.coeffs(), "The rotational part");
887  // 7x7 cov matrix
888  r.member("Cov", cov, "The covariance matrix");
889  }
890 
891  template <typename Reflector>
892  void reflectRead(Reflector& r)
893  {
894  r.property("X", this->t[0], "The translational part of the transform");
895  r.property("Y", this->t[1], "The translational part of the transform");
896  r.property("Z", this->t[2], "The translational part of the transform");
897 
898  auto ypr = quaternionToYawPitchRoll(this->r);
899  T yaw = rad2deg(ypr.template get<0>());
900  T pitch = rad2deg(ypr.template get<1>());
901  T roll = rad2deg(ypr.template get<2>());
902  r.member("Yaw" , yaw, "The yaw angle (phi, in degrees)");
903  r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
904  r.member("Roll" , roll, "The roll angle (psi, in degrees)");
905 
906  // convert to yaw pitch roll cov
907  YawPitchRollCovMatrixType yprcov = quaternionCovToYawPitchRollCov(this->cov, this->r);
908  r.member("Cov", yprcov, "The covariance matrix for [x,y,z,qw,qx,qy,qz]");
909  }
910 
911  template <typename Reflector>
912  void reflectWrite(Reflector& r)
913  {
914  T yaw, pitch, roll;
915  r.property("X", this->t[0], "The translational part of the transform");
916  r.property("Y", this->t[1], "The translational part of the transform");
917  r.property("Z", this->t[2], "The translational part of the transform");
918  r.member("Yaw" , yaw, "The yaw angle (phi, in degrees)");
919  r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
920  r.member("Roll" , roll, "The roll angle (psi, in degrees)");
921 
922  yaw = deg2rad(yaw);
923  pitch = deg2rad(pitch);
924  roll = deg2rad(roll);
925  this->r = quaternionFromYawPitchRoll(yaw,pitch,roll);
926 
928  r.member("Cov", yprcov, "The covariance matrix for [x,y,z,qw,qx,qy,qz]");
929 
930  // convert to quaternion cov
931  this->cov = quaternionCovFromYawPitchRollCov(yprcov, yaw, pitch, roll);
932  }
933 
935 
936  friend std::ostream& operator<<(std::ostream& os, const RigidTransformCov& tf)
937  {
938  os << "t: " << tf.t.transpose() << "\n";
939  os << "r: " << tf.r.w() << " " << tf.r.x() << " " << tf.r.y() << " " << tf.r.z() << "\n";
940  os << "cov:\n" << tf.cov << "\n";
941  return os;
942  }
943 
944 public:
945 
946  // overwritten to implement covariance propagation
947 
949  {
950  typedef typename Base::RotationType RotationType;
951  RotationType inv = this->r.inverse();
952 
953  CovMatrixType J = -CovMatrixType::Identity();
954 
955  J(3,3) = 1.0;
956  J.template block<3,3>(0,0) <<
957  (T)2.0 * (this->r.y() * this->r.y() + this->r.z() * this->r.z()) - (T)1.0,
958  -(T)2.0 * (this->r.w() * this->r.z() + this->r.x() * this->r.y()) ,
959  (T)2.0 * (this->r.w() * this->r.y() - this->r.x() * this->r.z()) ,
960 
961  (T)2.0 * (this->r.w() * this->r.z() - this->r.x() * this->r.y()) ,
962  (T)2.0 * (this->r.x() * this->r.x() + this->r.z() * this->r.z()) - (T)1.0,
963  -(T)2.0 * (this->r.w() * this->r.x() + this->r.y() * this->r.z()) ,
964 
965  -(T)2.0 * (this->r.w() * this->r.y() + this->r.x() * this->r.z()) ,
966  (T)2.0 * (this->r.w() * this->r.x() - this->r.y() * this->r.z()) ,
967  (T)2.0 * (this->r.x() * this->r.x() + this->r.y() * this->r.y()) - (T)1.0;
968 
969  // signs seem to be partially wrong in blanco2010se3 (see YawPitchRoll.h)
970  // this now corresponds to MRPT 1.5.5 CPose3DQuat::inverseComposePoint
971  J.template block<3,4>(0,3) <<
972  -(T)2.0 * (this->r.z() * this->t(1) - this->r.y() * this->t(2) ),
973  -(T)2.0 * (this->r.y() * this->t(1) + this->r.z() * this->t(2) ),
974  -(T)2.0 * (this->r.x() * this->t(1) - (T)2.0 * this->r.y() * this->t(0) - this->r.w() * this->t(2)),
975  -(T)2.0 * (this->r.x() * this->t(2) + this->r.w() * this->t(1) - (T)2.0 * this->r.z() * this->t(0)),
976 
977  -(T)2.0 * (this->r.x() * this->t(2) - this->r.z() * this->t(0) ),
978  -(T)2.0 * (this->r.y() * this->t(0) - (T)2.0 * this->r.x() * this->t(1) + this->r.w() * this->t(2)),
979  -(T)2.0 * (this->r.x() * this->t(0) + this->r.z() * this->t(2) ),
980  -(T)2.0 * (this->r.y() * this->t(2) - (T)2.0 * this->r.z() * this->t(1) - this->r.w() * this->t(0)),
981 
982  -(T)2.0 * (this->r.y() * this->t(0) - this->r.x() * this->t(1) ),
983  -(T)2.0 * (this->r.z() * this->t(0) - this->r.w() * this->t(1) - (T)2.0 * this->r.x() * this->t(2)),
984  -(T)2.0 * (this->r.z() * this->t(1) + this->r.w() * this->t(0) - (T)2.0 * this->r.y() * this->t(2)),
985  -(T)2.0 * (this->r.x() * this->t(0) + this->r.y() * this->t(1) );
986 
987  CovMatrixType c = J*cov*J.transpose();
988  return RigidTransformCov(inv.RotationType::operator*(-this->t), inv, c);
989  }
990 
991  RigidTransformCov& operator*= (const RigidTransformCov& other)
992  {
993  propagateCov(cov,*this,other); // must be done here, since operator* changes our value
994  Base::operator*=(other);
995  return *this;
996  }
997 
998  RigidTransformCov& operator*= (const Base& other)
999  {
1000  propagateCov(cov,*this,other); // must be done here, since operator* changes our value
1001  Base::operator*=(other);
1002  return *this;
1003  }
1004 
1005  friend RigidTransformCov operator* (const RigidTransformCov& a,
1006  const RigidTransformCov& b)
1007  {
1008  RigidTransformCov t = Base::mul(a,b);
1009  propagateCov(t.cov,a,b);
1010  return t;
1011  }
1012 
1013  friend RigidTransformCov operator* (const RigidTransformCov& a,
1014  const Base& b)
1015  {
1016  RigidTransformCov t = Base::mul(a,b);
1017  propagateCov(t.cov,a,b);
1018  return t;
1019  }
1020 
1021  friend RigidTransformCov operator* (const Base& a,
1022  const RigidTransformCov& b)
1023  {
1024  RigidTransformCov t = Base::mul(a,b);
1025  propagateCov(t.cov,a,b);
1026  return t;
1027  }
1028 
1029 private:
1030 
1031  // propagates the covariance matices to of both transforms (impl. see below)
1032  static void propagateCov(CovMatrixType& oC,
1033  const RigidTransformCov& w, const RigidTransformCov& a);
1034  // same as above, but w does not carry any covariance (impl. see below)
1035  static void propagateCov(CovMatrixType& oC,
1036  const RigidTransformCov& w, const Base& a);
1037  // same as above, but a does not carry any covariance (impl. see below)
1038  static void propagateCov(CovMatrixType& C,
1039  const Base& w, const RigidTransformCov& a);
1040 
1041 public:
1042 
1045 };
1046 
1048 
1053 
1058 
1063 
1068 
1070 // Covariance propagation maths
1071 
1072 // 2D case:
1073 //
1074 // g(a,w) = w * a
1075 // = [ Rw * ta + tw ;
1076 // phia + phiw ]
1077 //
1078 // Ja = d g(a,w) / da
1079 // Jw = d g(a,w) / dw
1080 // C = Ja*Ca*Ja' + Jw*Cw*Jw'
1081 
1082 template <typename T>
1083 inline void RigidTransformCov<T,2>::propagateCov(CovMatrixType& oC,
1084  const RigidTransformCov& w,
1085  const RigidTransformCov& a)
1086 {
1087  // NOTE: change oC at the end only, since it may be a reference on
1088  // the parameter w or a!
1089 
1090  typedef Eigen::Matrix<T,3,3> Mat3;
1091  Mat3 Ja, Jw;
1092 
1093  T sinrw = std::sin(w.r.angle());
1094  T cosrw = std::cos(w.r.angle());
1095 
1096  // compute the jacobians
1097  Ja << cosrw, -sinrw, 0,
1098  sinrw, cosrw, 0,
1099  0, 0, 1;
1100 
1101  Jw << 1, 0, -a.t(0)*sinrw -a.t(1)*cosrw,
1102  0, 1, a.t(0)*cosrw -a.t(1)*sinrw,
1103  0, 0, 1;
1104 
1105  // propagate the covariances:
1106  oC = Ja * a.cov * Ja.transpose() + Jw * w.cov * Jw.transpose();
1107 }
1108 
1109 // same as above, but w does not carry any covariance
1110 template <typename T>
1111 inline void RigidTransformCov<T,2>::propagateCov(CovMatrixType& oC,
1112  const RigidTransformCov& w,
1113  const Base& a)
1114 {
1115  // NOTE: change oC at the end only,
1116  // since it may be a reference on the parameter w or a!
1117 
1118  typedef Eigen::Matrix<T,3,3> Mat3;
1119  Mat3 Jw;
1120 
1121  T sinrw = std::sin(w.r.angle());
1122  T cosrw = std::cos(w.r.angle());
1123 
1124  // compute the jacobians
1125  Jw << 1, 0, -a.t(0)*sinrw -a.t(1)*cosrw,
1126  0, 1, a.t(0)*cosrw -a.t(1)*sinrw,
1127  0, 0, 1;
1128 
1129  // propagate the covariances:
1130  oC = Jw * w.cov * Jw.transpose();
1131 }
1132 
1133 template <typename T>
1134 inline void RigidTransformCov<T,2>::propagateCov(CovMatrixType& oC,
1135  const Base& w,
1136  const RigidTransformCov& a)
1137 {
1138  // NOTE: change oC at the end only,
1139  // since it may be a reference on the parameter w or a!
1140 
1141  typedef Eigen::Matrix<T,3,3> Mat3;
1142  Mat3 Ja;
1143 
1144  T sinrw = std::sin(w.r.angle());
1145  T cosrw = std::cos(w.r.angle());
1146 
1147  // compute the jacobians
1148  Ja << cosrw, -sinrw, 0,
1149  sinrw, cosrw, 0,
1150  0, 0, 1;
1151 
1152  // propagate the covariances:
1153  oC = Ja * a.cov * Ja.transpose();
1154 }
1155 
1156 // 3D case:
1157 //
1158 //
1159 // g(a,w) = w * a
1160 // = [ qw * ta + tw ;
1161 // qw * qa ]
1162 //
1163 // Ja = d g(a,w) / da
1164 // Jw = d g(a,w) / dw
1165 // C = Ja*Ca*Ja' + Jw*Cw*Jw'
1166 template <typename T>
1167 inline void RigidTransformCov<T,3>::propagateCov(CovMatrixType& oC,
1168  const RigidTransformCov& w,
1169  const RigidTransformCov& a)
1170 {
1171  // NOTE: change oC at the end only,
1172  // since it may be a reference on the parameter w or a!
1173 
1174  CovMatrixType Jw = CovMatrixType::Identity();
1175 
1176  Jw.template block<3,4>(0,3) <<
1177  - w.r.z()*a.t(1,0)+ w.r.y()*a.t(2,0),
1178  w.r.y()*a.t(1,0)+ w.r.z()*a.t(2,0),
1179  -(T)2.0*w.r.y()*a.t(0,0)+ w.r.x()*a.t(1,0)+ w.r.w()*a.t(2,0),
1180  -(T)2.0*w.r.z()*a.t(0,0)- w.r.w()*a.t(1,0)+ w.r.x()*a.t(2,0),
1181 
1182  w.r.z()*a.t(0,0) - w.r.x()*a.t(2,0),
1183  w.r.y()*a.t(0,0)-(T)2.0*w.r.x()*a.t(1,0)- w.r.w()*a.t(2,0),
1184  w.r.x()*a.t(0,0) + w.r.z()*a.t(2,0),
1185  w.r.w()*a.t(0,0)-(T)2.0*w.r.z()*a.t(1,0)+ w.r.y()*a.t(2,0),
1186 
1187  -w.r.y()*a.t(0,0)+ w.r.x()*a.t(1,0) ,
1188  w.r.z()*a.t(0,0)+ w.r.w()*a.t(1,0)-(T)2.0*w.r.x()*a.t(2,0),
1189  -w.r.w()*a.t(0,0)+ w.r.z()*a.t(1,0)-(T)2.0*w.r.y()*a.t(2,0),
1190  w.r.x()*a.t(0,0)+ w.r.y()*a.t(1,0);
1191  Jw.template block<3,4>(0,3) *= (T)2.0;
1192 
1193  Jw.template block<4,4>(3,3) <<
1194  a.r.w(),-a.r.x(),-a.r.y(),-a.r.z(),
1195  a.r.x(), a.r.w(), a.r.z(),-a.r.y(),
1196  a.r.y(),-a.r.z(), a.r.w(), a.r.x(),
1197  a.r.z(), a.r.y(),-a.r.x(), a.r.w();
1198 
1199  CovMatrixType Ja = CovMatrixType::Identity();
1200 
1201  Ja.template block<3,3>(0,0) <<
1202  (T)0.5-w.r.y()*w.r.y()-w.r.z()*w.r.z(),
1203  w.r.x()*w.r.y()-w.r.w()*w.r.z(),
1204  w.r.w()*w.r.y()+w.r.x()*w.r.z(),
1205 
1206  w.r.w()*w.r.z()+w.r.x()*w.r.y(),
1207  (T)0.5-w.r.x()*w.r.x()-w.r.z()*w.r.z(),
1208  w.r.y()*w.r.z()-w.r.w()*w.r.x(),
1209 
1210  w.r.x()*w.r.z()-w.r.w()*w.r.y(),
1211  w.r.w()*w.r.x()+w.r.y()*w.r.z(),
1212  (T)0.5-w.r.x()*w.r.x()-w.r.y()*w.r.y();
1213  Ja.template block<3,3>(0,0) *= (T)2.0;
1214 
1215  Ja.template block<4,4>(3,3) <<
1216  w.r.w(), -w.r.x(), -w.r.y(), -w.r.z(),
1217  w.r.x(), w.r.w(), -w.r.z(), w.r.y(),
1218  w.r.y(), w.r.z(), w.r.w(), -w.r.x(),
1219  w.r.z(), -w.r.y(), w.r.x(), w.r.w();
1220 
1221  // propagate the covariances:
1222  oC = Jw * w.cov * Jw.transpose() + Ja * a.cov * Ja.transpose();
1223 }
1224 
1225 template <typename T>
1226 inline void RigidTransformCov<T,3>::propagateCov(CovMatrixType& oC,
1227  const RigidTransformCov& w,
1228  const Base& a)
1229 {
1230  // NOTE: change oC at the end only,
1231  // since it may be a reference on the parameter w or a!
1232 
1233  CovMatrixType Jw = CovMatrixType::Identity();
1234 
1235  Jw.template block<3,4>(0,3) <<
1236  - w.r.z()*a.t(1,0)+ w.r.y()*a.t(2,0),
1237  w.r.y()*a.t(1,0)+ w.r.z()*a.t(2,0),
1238  -(T)2.0*w.r.y()*a.t(0,0)+ w.r.x()*a.t(1,0)+ w.r.w()*a.t(2,0),
1239  -(T)2.0*w.r.z()*a.t(0,0)- w.r.w()*a.t(1,0)+ w.r.x()*a.t(2,0),
1240 
1241  w.r.z()*a.t(0,0) - w.r.x()*a.t(2,0),
1242  w.r.y()*a.t(0,0)-(T)2.0*w.r.x()*a.t(1,0)- w.r.w()*a.t(2,0),
1243  w.r.x()*a.t(0,0) + w.r.z()*a.t(2,0),
1244  w.r.w()*a.t(0,0)-(T)2.0*w.r.z()*a.t(1,0)+ w.r.y()*a.t(2,0),
1245 
1246  -w.r.y()*a.t(0,0)+ w.r.x()*a.t(1,0) ,
1247  w.r.z()*a.t(0,0)+ w.r.w()*a.t(1,0)-2.0*w.r.x()*a.t(2,0),
1248  -w.r.w()*a.t(0,0)+ w.r.z()*a.t(1,0)-2.0*w.r.y()*a.t(2,0),
1249  w.r.x()*a.t(0,0)+ w.r.y()*a.t(1,0);
1250  Jw.template block<3,4>(0,3) *= (T)2.0;
1251 
1252  Jw.template block<4,4>(3,3) <<
1253  a.r.w(),-a.r.x(),-a.r.y(),-a.r.z(),
1254  a.r.x(), a.r.w(), a.r.z(),-a.r.y(),
1255  a.r.y(),-a.r.z(), a.r.w(), a.r.x(),
1256  a.r.z(), a.r.y(),-a.r.x(), a.r.w();
1257 
1258  // propagate the covariances:
1259  oC = Jw * w.cov * Jw.transpose();
1260 }
1261 
1262 template <typename T>
1263 inline void RigidTransformCov<T,3>::propagateCov(CovMatrixType& oC,
1264  const Base& w,
1265  const RigidTransformCov& a)
1266 {
1267  // NOTE: change oC at the end only,
1268  // since it may be a reference on the parameter w or a!
1269 
1270  CovMatrixType Ja = CovMatrixType::Identity();
1271 
1272  Ja.template block<3,3>(0,0) <<
1273  (T)0.5-w.r.y()*w.r.y()-w.r.z()*w.r.z(),
1274  w.r.x()*w.r.y()-w.r.w()*w.r.z(),
1275  w.r.w()*w.r.y()+w.r.x()*w.r.z(),
1276 
1277  w.r.w()*w.r.z()+w.r.x()*w.r.y(),
1278  (T)0.5-w.r.x()*w.r.x()-w.r.z()*w.r.z(),
1279  w.r.y()*w.r.z()-w.r.w()*w.r.x(),
1280 
1281  w.r.x()*w.r.z()-w.r.w()*w.r.y(),
1282  w.r.w()*w.r.x()+w.r.y()*w.r.z(),
1283  (T)0.5-w.r.x()*w.r.x()-w.r.y()*w.r.y();
1284  Ja.template block<3,3>(0,0) *= (T)2.0;
1285 
1286  Ja.template block<4,4>(3,3) <<
1287  w.r.w(), -w.r.x(), -w.r.y(), -w.r.z(),
1288  w.r.x(), w.r.w(), -w.r.z(), w.r.y(),
1289  w.r.y(), w.r.z(), w.r.w(), -w.r.x(),
1290  w.r.z(), -w.r.y(), w.r.x(), w.r.w();
1291 
1292  // propagate the covariances:
1293  oC = Ja * a.cov * Ja.transpose();
1294 }
1295 
1297 // Linear interpolation stuff
1298 
1308 template <typename T, int D, typename S>
1310  const RigidTransform<T, D>& t2,
1311  S alpha)
1312 {
1313  return RigidTransform<T, D>(lerp(t1.t, t2.t, alpha), // interpolate translation
1314  lerp(t1.r, t2.r, alpha)); // interpolate rotation
1315 }
1316 
1326 template <typename T, int D, typename S>
1328  const RigidTransformCov<T, D>& t2,
1329  S alpha)
1330 {
1331  return RigidTransformCov<T, D>(lerp(t1.t, t2.t, alpha), // interpolate translation
1332  lerp(t1.r, t2.r, alpha), // interpolate rotation
1333  lerp(t1.cov, t2.cov, alpha)); // interpolate covariance
1334 }
1335 
1337 // specializations for Dx1 and dynamic matrices
1338 
1340 template< int D, typename TransformDerived, typename OtherDerived>
1341 struct ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived,D, 1>
1342 {
1343  typedef typename OtherDerived::Scalar Scalar;
1344  typedef RigidTransformBase<Scalar, D, typename TransformDerived::RotationType, TransformDerived> TTransform;
1345  typedef const Eigen::Matrix<Scalar, D, 1> TResult;
1346 
1347 
1348  static TResult run(const TTransform& t, const OtherDerived& v)
1349  {
1350  // TODO: increase performance by using expression templates see Eigen/Geometry/Transform.h
1351  return t.r*v + t.t;
1352  }
1353 };
1354 
1355 template< int D, typename TransformDerived, typename OtherDerived>
1356 struct ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived,Eigen::Dynamic, Eigen::Dynamic>
1357 {
1358  typedef typename OtherDerived::Scalar Scalar;
1359  typedef RigidTransformBase<Scalar, D, typename TransformDerived::RotationType, TransformDerived> TTransform;
1360  typedef const Eigen::Matrix<Scalar, D, 1> TResult;
1361 
1362 
1363  static TResult run(const TTransform& t, const OtherDerived& v)
1364  {
1365  // TODO: increase performance by using expression templates see Eigen/Geometry/Transform.h
1366  return t.r*v + t.t;
1367  }
1368 };
1369 
1371 
1373 
1374 }
1375 
1376 #endif
RigidTransformCov(const Base &transform)
Initialization from corresponding RigidTransform (without covariance).
Definition: RigidTransform.h:433
T Type
The used floating point type (float, double, long double)
Definition: RigidTransform.h:144
CovMatrixType cov
the covariance of the transform as matrix:
Definition: RigidTransform.h:1044
INTERNAL std::enable_if< std::is_floating_point< T >::value, T >::type deg2rad(T value)
Convert degree to radian, for floating point arguments (return type = argument type) ...
Definition: Angle.h:81
void reflect(BinarySerializer< Derived > &r)
Definition: RigidTransform.h:682
Specialization of RigidTransform for 3 dimensions.
Definition: RigidTransform.h:610
ei_rigidtransform_product_impl< D, TransformDerived, OtherDerived >::TResult operator*(const Eigen::MatrixBase< OtherDerived > &other) const
Apply the transformation to an Eigen matrix.
Definition: RigidTransform.h:224
Typedefs for OS independent basic data types.
static CovMatrixType nullCov()
Returns a "null covariance matrix".
Definition: RigidTransform.h:789
Eigen::Matrix< T, 6, 6 > quaternionCovToYawPitchRollCov(const Eigen::Matrix< T, 7, 7 > &covariance, const Eigen::Quaternion< T > &q)
Converts a 7x7 dimensional quaternion covariance (3D + Quaternion) back to a 6x6 dimensional euler co...
Definition: YawPitchRoll.h:318
RigidTransformCov()
Definition: RigidTransform.h:803
Include file for all eigen related things.
RigidTransformCov< U, 2 > cast() const
Returns *this casted to U.
Definition: RigidTransform.h:500
RigidTransform< float, 2 > RigidTransform2f
Typedef for 2D float transform.
Definition: RigidTransform.h:1050
size of the corresponding homogeneous space
Definition: RigidTransform.h:160
T phi() const
Returns the rotation angle represented by this transform in radian.
Definition: RigidTransform.h:362
boost::tuples::tuple< T, T, T > quaternionToYawPitchRoll(const Eigen::Quaternion< T > &q)
Converts a quaternion back to yaw, pitch, roll angles.
Definition: YawPitchRoll.h:299
static CovMatrixType nullCov()
Returns a "null covariance matrix".
Definition: RigidTransform.h:420
specialize cv::DataType for our ImgPixel and inherit from cv::DataType<Vec>
Definition: IOService.h:67
RigidTransformCov(T x, T y, const AngleBase< T, UnitTag, Derived > &angle, T covX, T covY, T covAngle)
Creates a new 2D transform with the specified translation and a rotation that is specified as Angle (...
Definition: RigidTransform.h:490
RigidTransformCov()
Definition: RigidTransform.h:426
RigidTransformCov(T x, T y, const AngleBase< T, UnitTag, Derived > &angle, const CovMatrixType &covariance)
Creates a new 2D transform with the specified translation and a rotation that is specified as Angle (...
Definition: RigidTransform.h:468
void member(const char *name, T &member, const char *comment, ReflectCtrlFlags flags=REFLECT_CTRLFLAG_NONE)
Definition: RecursiveMemberReflector.h:871
This class represents an affine transformation that supports a translation followed by a rotation (a ...
Definition: RigidTransform.h:126
void quaternionCovFromYawPitchRollCov(const Eigen::Matrix< T, 6, 6 > &eulerCovariance, float yaw, float pitch, float roll, Eigen::Quaternion< T > &oOrientation, Eigen::Matrix< T, 7, 7 > &oCovariance)
Converts 6x6 dimensional covariance matrix (3D + yaw, pitch and roll) angles to a 7x7 dimensional qua...
Definition: YawPitchRoll.h:201
T & phi()
Returns the rotation angle represented by this transform in radian.
Definition: RigidTransform.h:360
TransformDerived * This()
casts this to the actual derived type (see Curiously recurring template pattern)
Definition: RigidTransform.h:261
#define MIRA_SPLIT_REFLECT_MEMBER
Macro that insert a class member reflect() method just splitting reflection into a reflectRead() and ...
Definition: SplitReflect.h:238
Eigen::Matrix< T, CovarianceDim, CovarianceDim > CovMatrixType
The type of the matrix representing the covariance of the transformation.
Definition: RigidTransform.h:415
#define MIRA_REFLECT_BASE(reflector, BaseClass)
Macro that can be used to reflect the base class easily.
Definition: ReflectorInterface.h:956
Definition: YawPitchRoll.h:684
Quaternion inverse(void) const
RigidTransform< float, 3 > RigidTransform3f
Typedef for 3D float transform.
Definition: RigidTransform.h:1052
T lerp(const T &a, const T &b, S alpha)
Linear interpolation of different types like scalars, angles and rotations, vectors, etc.
Definition: Lerp.h:76
RigidTransformCov(T x, T y, T z, T yaw, T pitch, T roll, const YawPitchRollCovMatrixType &yawPitchRollCov)
Initialization from the translation, rotation given as yaw/pitch/roll angles and the covariance matri...
Definition: RigidTransform.h:835
Implementation of RigidTransforms with different dimensionality D.
Definition: RigidTransform.h:139
static TransformDerived mul(const RigidTransformBase &a, const RigidTransformBase &b)
Definition: RigidTransform.h:269
Rotation2D inverse() const
void property(const char *name, T &member, const char *comment, PropertyHint &&hint=PropertyHint(), ReflectCtrlFlags flags=REFLECT_CTRLFLAG_NONE)
Definition: RecursiveMemberReflector.h:973
Eigen::Matrix< T, EulerCovarianceDim, EulerCovarianceDim > YawPitchRollCovMatrixType
The type of the matrix representing the euler covariance of the transformation.
Definition: RigidTransform.h:782
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RigidTransformBase(TranslationType translation, RotationType rotation)
Definition: RigidTransform.h:173
YawPitchRollCovMatrixType getYawPitchRollCov() const
Returns covariance matrix where the rotation covariance is represented using yaw, pitch...
Definition: RigidTransform.h:861
Functions for linear interpolation of different types like scalars, angles and rotations.
CovMatrixType cov
The covariance of the transform as matrix.
Definition: RigidTransform.h:598
void reflectWrite(Reflector &r)
Definition: RigidTransform.h:912
RigidTransformCov(const Base &transform)
Initialization from corresponding RigidTransform (without covariance).
Definition: RigidTransform.h:809
RigidTransformCov inverse() const
Definition: RigidTransform.h:526
Eigen::Matrix< T, D, 1 > TranslationType
Vector type used to represent the translational part (dimension depends on dimensionality of transfor...
Definition: RigidTransform.h:150
RigidTransformCov(T x, T y, T angle, T covX, T covY, T covAngle)
Creates a new 2D transform with the specified translation and a rotation that is specified in rad...
Definition: RigidTransform.h:479
friend std::ostream & operator<<(std::ostream &os, const RigidTransformCov &tf)
Definition: RigidTransform.h:515
RigidTransformCov< double, 3 > RigidTransformCov3d
Typedef for 3D double transform with covariance.
Definition: RigidTransform.h:1067
RigidTransform< U, 2 > cast() const
Returns *this casted to U.
Definition: RigidTransform.h:342
MIRA_SPLIT_REFLECT_MEMBER friend std::ostream & operator<<(std::ostream &os, const RigidTransform &tf)
Definition: RigidTransform.h:732
T pitch() const
Returns the pitch angle in rad.
Definition: RigidTransform.h:669
dimension of the Eucleadian space
Definition: RigidTransform.h:159
Conversion from yaw, pitch and roll to Quaternion and vice versa.
Deserializer that uses BinaryIstream to deserialize the objects from binary format.
Definition: BinarySerializer.h:928
RigidTransformCov< double, 2 > RigidTransformCov2d
Typedef for 2D double transform with covariance.
Definition: RigidTransform.h:1065
RigidTransform(T x, T y, const AngleBase< T, UnitTag, Derived > &angle)
Creates a new 2D transform with the specified translation and a rotation that is specified as Angle (...
Definition: RigidTransform.h:334
TRotation RotationType
Type used to represent the rotational part (Eigen::Rotation2D for 2D transforms, Eigen::Quaternion fo...
Definition: RigidTransform.h:156
MIRA_SPLIT_REFLECT_MEMBER friend std::ostream & operator<<(std::ostream &os, const RigidTransformCov &tf)
Definition: RigidTransform.h:936
RigidTransformCov(const Eigen::Matrix< T, 2, 1 > &translation, const Eigen::Rotation2D< T > &rotation, const CovMatrixType &covariance)
Creates a new 2D transform with the specified translation, the specified Rotation2D and a covairance ...
Definition: RigidTransform.h:440
T roll() const
Returns the roll angle in rad.
Definition: RigidTransform.h:677
RigidTransform(const Eigen::Matrix< T, 2, 1 > &translation, T angle)
Creates a new 2D transform with the specified translation and a rotation that is specified in rad...
Definition: RigidTransform.h:317
MatrixType getMatrix() const
Computes and returns the matrix that describes the affine transformation in homogeneous space...
Definition: RigidTransform.h:250
void reflectRead(Reflector &r)
Definition: RigidTransform.h:702
T & x()
Returns the x-coordinate of the translational part of the transform.
Definition: RigidTransform.h:640
RigidTransformCov< float, 2 > RigidTransformCov2f
Typedef for 2D float transform with covariance.
Definition: RigidTransform.h:1060
Implementations of angles (values in periodic interval of width 2*pi) with arbitrary base type...
T & x()
Returns the x-coordinate of the translational part of the transform.
Definition: RigidTransform.h:350
RigidTransform< U, 3 > cast() const
Returns *this casted to U.
Definition: RigidTransform.h:632
TransformDerived & operator*=(const RigidTransformBase &other)
Concatenates this transform with an other transform.
Definition: RigidTransform.h:202
RotationType r
The rotational part of the transform.
Definition: RigidTransform.h:283
friend std::ostream & operator<<(std::ostream &os, const RigidTransform &tf)
Definition: RigidTransform.h:376
T yaw() const
Returns the yaw angle in rad (This corresponds to phi in a 2D transform).
Definition: RigidTransform.h:661
RigidTransformCov(const Eigen::Matrix< T, 3, 1 > &translation, const Eigen::Quaternion< T > &rotation, const CovMatrixType &covariance)
Initialization from the translation, rotation as quaternion and the covariance matrix in quaternion s...
Definition: RigidTransform.h:816
void reflect(BinarySerializer< Derived > &r)
Definition: RigidTransform.h:868
Specialization of RigidTransform for 2 dimensions.
Definition: RigidTransform.h:295
friend TransformDerived operator*(const RigidTransformBase &a, const RigidTransformBase &b)
Concatenates the two transformations.
Definition: RigidTransform.h:212
void reflect(Reflector &m)
Definition: RigidTransform.h:367
RigidTransformCov< U, 3 > cast() const
Returns *this casted to U.
Definition: RigidTransform.h:848
bool isApprox(const RigidTransformBase &other, T prec=std::numeric_limits< T >::epsilon()) const
Returns true if this is approximately equal to other, within the precision determined by prec...
Definition: RigidTransform.h:191
RigidTransformCov< float, 3 > RigidTransformCov3f
Typedef for 3D float transform with covariance.
Definition: RigidTransform.h:1062
T & y()
Returns the y-coordinate of the translational part of the transform.
Definition: RigidTransform.h:645
RigidTransformCov inverse() const
Definition: RigidTransform.h:948
TransformDerived inverse() const
Computes and returns the inverse transform of this.
Definition: RigidTransform.h:180
T & y()
Returns the y-coordinate of the translational part of the transform.
Definition: RigidTransform.h:355
void reflect(Reflector &r)
Definition: RigidTransform.h:509
static YawPitchRollCovMatrixType nullYawPitchRollCov()
Returns an "null covariance matrix" witch yaw, pitch, roll rotation representation.
Definition: RigidTransform.h:797
T x() const
Returns the x-coordinate of the translational part of the transform.
Definition: RigidTransform.h:352
RigidTransform< double, 3 > RigidTransform3d
Typedef for 3D double transform.
Definition: RigidTransform.h:1057
T y() const
Returns the y-coordinate of the translational part of the transform.
Definition: RigidTransform.h:357
Eigen::Matrix< typename Eigen::MatrixBase< Derived >::Scalar, 3, 1 > eulerAngles(const Eigen::MatrixBase< Derived > &mat, typename Eigen::MatrixBase< Derived >::Index a0, typename Eigen::MatrixBase< Derived >::Index a1, typename Eigen::MatrixBase< Derived >::Index a2)
Returns the Euler-angles of the rotation matrix mat using the convention defined by the triplet (a0...
Definition: YawPitchRoll.h:86
Eigen::Matrix< T, HDim, HDim > MatrixType
Type of the matrix that can describe the full affine transform in homogeneous space.
Definition: RigidTransform.h:167
T x() const
Returns the x-coordinate of the translational part of the transform.
Definition: RigidTransform.h:642
T y() const
Returns the y-coordinate of the translational part of the transform.
Definition: RigidTransform.h:647
RigidTransform(T x, T y, T angle)
Creates a new 2D transform with the specified translation and a rotation that is specified in rad...
Definition: RigidTransform.h:324
Definition: BinarySerializer.h:257
RigidTransform< double, 2 > RigidTransform2d
Typedef for 2D double transform.
Definition: RigidTransform.h:1055
const TransformDerived * This() const
casts this to the actual derived type (see Curiously recurring template pattern)
Definition: RigidTransform.h:263
void reflectRead(Reflector &r)
Definition: RigidTransform.h:892
RigidTransform(T x, T y, T z, T yaw, T pitch, T roll)
Definition: RigidTransform.h:624
RigidTransform(const Eigen::Matrix< T, 3, 1 > &translation, const Eigen::Quaternion< T > &rotation)
Definition: RigidTransform.h:620
void reflect(BinaryDeserializer< Derived > &r)
Definition: RigidTransform.h:692
std::enable_if< std::is_floating_point< T >::value, T >::type rad2deg(T value)
Convert radian to degree, for floating point arguments (return type = argument type) ...
Definition: Angle.h:105
RigidTransform()
Definition: RigidTransform.h:617
Eigen::Quaternion< T > quaternionFromYawPitchRoll(T yaw, T pitch, T roll)
Converts yaw, pitch and roll angles to a quaternion.
Definition: YawPitchRoll.h:156
RigidTransform()
Definition: RigidTransform.h:302
T & z()
Returns the z-coordinate of the translational part of the transform.
Definition: RigidTransform.h:650
void reflectWrite(Reflector &r)
Definition: RigidTransform.h:718
This class represents an affine transformation that supports a translation followed by a rotation (a ...
Definition: RigidTransform.h:99
RigidTransformCov(const Eigen::Matrix< T, 2, 1 > &translation, T angle, const CovMatrixType &covariance)
Creates a new 2D transform with the specified translation, a rotation that is specified in rad...
Definition: RigidTransform.h:449
RigidTransformCov(const Eigen::Matrix< T, 3, 1 > &translation, const Eigen::Quaternion< T > &rotation, const YawPitchRollCovMatrixType &yawPitchRollCov)
Initialization from the translation, rotation as quaternion and the covariance matrix in yaw/pitch/ro...
Definition: RigidTransform.h:825
Eigen::Matrix< T, CovarianceDim, CovarianceDim > CovMatrixType
The type of the matrix representing the covariance of the transformation.
Definition: RigidTransform.h:776
RigidTransformCov(T x, T y, T angle, const CovMatrixType &covariance)
Creates a new 2D transform with the specified translation, a rotation that is specified in rad...
Definition: RigidTransform.h:458
TranslationType t
Vector that describes the translational part of the transform.
Definition: RigidTransform.h:280
Provides definition for getters and setters that are used with the serialization framework.
void reflect(BinaryDeserializer< Derived > &r)
Definition: RigidTransform.h:880
T z() const
Returns the z-coordinate of the translational part of the transform.
Definition: RigidTransform.h:652
Base class template for derived Angle implementations.
Definition: Angle.h:182
RigidTransform(const Eigen::Matrix< T, 2, 1 > &translation, const Eigen::Rotation2D< T > &rotation)
Creates a new 2D transform with the specified translation and the specified Rotation2D.
Definition: RigidTransform.h:309