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 #include <utils/IsCheapToCopy.h>
63 
64 namespace mira {
65 
67 
69 
72 template< int D,
73  typename TransformDerived,
74  typename OtherDerived,
75  int OtherRows=OtherDerived::RowsAtCompileTime,
76  int OtherCols=OtherDerived::ColsAtCompileTime>
77 struct ei_rigidtransform_product_impl
78 {
79  static_assert(D!=D,"You are trying to apply a rigid transform to a matrix "
80  "of invalid size");
81 };
83 
85 
100 template <typename T, int D>
102 {
103  static_assert(D!=D,"RigidTransform is not defined for this dimension. "
104  "Only 2 and 3 dimensions are available.");
105 };
106 
108 
127 template <typename T, int D>
129 {
130  static_assert(D!=D,"RigidTransformCov is not defined for this dimension. "
131  "Only 2 and 3 dimensions are available.");
132 };
133 
135 
140 template <typename T, int D, typename TRotation, typename TransformDerived>
142 {
143 public:
144 
146  typedef T Type;
147 
153 
158  typedef TRotation RotationType;
159 
160  enum {
161  Dim = D,
162  HDim = D+1
163  };
164 
170 
171 public:
172 
173  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
174 
176  t(translation), r(rotation) {}
177 
178 
179 public:
180 
182  TransformDerived inverse() const
183  {
184  RotationType inv = r.inverse();
185  return TransformDerived(inv.RotationType::operator*(-t), inv);
186  }
187 
193  bool isApprox(const RigidTransformBase& other,
194  T prec = std::numeric_limits<T>::epsilon()) const
195  {
196  return t.isApprox(other.t,prec) && r.isApprox(other.r, prec);
197  }
198 
199 public:
200 
204  TransformDerived& operator*= (const RigidTransformBase& other)
205  {
206  t += r * other.t;
207  r *= other.r;
208  return *This();
209  }
210 
214  friend TransformDerived operator* (const RigidTransformBase& a,
215  const RigidTransformBase& b) {
216  return mul(a,b);
217  }
218 
224  template <typename OtherDerived>
225  typename ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived>::TResult
227  {
228  return ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived>::run(*this, other.derived());
229  }
230 
231 public:
232 
238  {
239  // order of transforms is from right to left (i.e. rotate first and
240  // translate afterwards)
241  return Eigen::Translation<T,D>(t)*r;
242  }
243 
244 public:
245 
253  {
254  MatrixType m = MatrixType::Identity ();
255  m.template block<Dim,Dim>(0,0) = r.toRotationMatrix();
256  m.template block<Dim,1>(0,Dim) = t.transpose();
257  return m;
258  }
259 
260 protected:
261 
263  TransformDerived* This() { return static_cast<TransformDerived*>(this); }
265  const TransformDerived* This() const {
266  return static_cast<const TransformDerived*>(this);
267  }
268 
269 protected:
270 
271  static TransformDerived mul(const RigidTransformBase& a,
272  const RigidTransformBase& b)
273  {
274  RotationType r = a.r*b.r; // this must be assigned to a temp variable
275  // (due to a bug in Eigen::Rotation2D<>? )
276  return TransformDerived(a.r * b.t + a.t, r);
277  }
278 
279 public:
280 
283 
286 };
287 
289 
296 template <typename T>
297 class RigidTransform<T,2> : public RigidTransformBase<T, 2,
298  Eigen::Rotation2D<T>, RigidTransform<T,2> >
299 {
301 
302 public:
303 
305  Base(Eigen::Matrix<T,2,1>(0,0),Eigen::Rotation2D<T>(0)) {}
306 
312  const Eigen::Rotation2D<T>& rotation) :
313  Base(translation,rotation) {}
314 
319  RigidTransform(const Eigen::Matrix<T,2,1>& translation, T angle) :
320  Base(translation,Eigen::Rotation2D<T>(angle)) {}
321 
326  RigidTransform(T x, T y, T angle) :
327  Base(Eigen::Matrix<T,2,1>(x,y),
328  Eigen::Rotation2D<T>(angle)) {}
329 
335  template <typename UnitTag, typename Derived>
337  Base(Eigen::Matrix<T,2,1>(x,y),
338  Eigen::Rotation2D<T>(angle.rad())) {}
339 
340 public:
341 
343  template <typename U>
345  return RigidTransform<U,2>(this->t.template cast<U>(),
346  this->r.template cast<U>());
347  }
348 
349 public:
350 
352  T& x() { return this->t.x(); }
354  T x() const { return this->t.x(); }
355 
357  T& y() { return this->t.y(); }
359  T y() const { return this->t.y(); }
360 
362  T& phi() { return this->r.angle(); }
364  T phi() const { return this->r.angle(); }
365 
366 public:
367 
368  template <typename Reflector>
369  void reflect(Reflector& m)
370  {
371  m.property("X", this->t[0], "The translational part of the transform");
372  m.property("Y", this->t[1], "The translational part of the transform");
373  m.property("Phi", getter<T>(rad2deg<T>, this->r.angle()),
374  setter<T>(deg2rad<T>, this->r.angle()),
375  "The orientation in degrees");
376  }
377 
378  friend std::ostream& operator<<(std::ostream& os, const RigidTransform& tf)
379  {
380  os << "t: " << tf.t.transpose() << "\n";
381  os << "r: " << tf.r.angle() << "\n";
382  return os;
383  }
384 };
385 
387 
403 template <typename T>
404 class RigidTransformCov<T,2> : public RigidTransform<T, 2>
405 {
406  typedef RigidTransform<T, 2> Base;
407 
408 public:
409 
411  static const int CovarianceDim = 3;
412 
418 
423  return CovMatrixType::Zero();
424  }
425 
426 public:
427 
428  RigidTransformCov() : Base(), cov(nullCov())
429  {}
430 
435  RigidTransformCov(const Base& transform) : Base(transform),
436  cov(nullCov()) {}
437 
443  const Eigen::Rotation2D<T>& rotation,
444  const CovMatrixType& covariance) :
445  Base(translation,rotation), cov(covariance) {}
446 
451  RigidTransformCov(const Eigen::Matrix<T,2,1>& translation, T angle,
452  const CovMatrixType& covariance) :
453  Base(translation,Eigen::Rotation2D<T>(angle)),
454  cov(covariance) {}
455 
460  RigidTransformCov(T x, T y, T angle, const CovMatrixType& covariance) :
461  Base(Eigen::Matrix<T,2,1>(x,y),
462  Eigen::Rotation2D<T>(angle)), cov(covariance) {}
463 
469  template <typename UnitTag, typename Derived>
471  const CovMatrixType& covariance) :
472  Base(Eigen::Matrix<T,2,1>(x,y),
473  Eigen::Rotation2D<T>(angle.rad())), cov(covariance) {}
474 
481  RigidTransformCov(T x, T y, T angle, T covX, T covY, T covAngle) :
482  Base(Eigen::Matrix<T,2,1>(x,y),
483  Eigen::Rotation2D<T>(angle)),
484  cov(Eigen::DiagonalMatrix<T,3>(covX, covY, covAngle)) {}
485 
491  template <typename UnitTag, typename Derived>
493  T covX, T covY, T covAngle) :
494  Base(Eigen::Matrix<T,2,1>(x,y),
495  Eigen::Rotation2D<T>(angle.rad())),
496  cov(Eigen::DiagonalMatrix<T,3>(covX, covY, covAngle)) {}
497 
498 public:
499 
501  template <typename U>
503  return RigidTransformCov<U,2>(this->t.template cast<U>(),
504  this->r.template cast<U>(),
505  this->cov.template cast<U>());
506  }
507 
508 public:
509 
510  template <typename Reflector>
511  void reflect(Reflector& r)
512  {
514  r.member("Cov", cov, "The covariance matrix (using radians for phi)");
515  }
516 
517  friend std::ostream& operator<<(std::ostream& os, const RigidTransformCov& tf)
518  {
519  os << "t: " << tf.t.transpose() << "\n";
520  os << "r: " << tf.r.angle() << "\n";
521  os << "cov:\n" << tf.cov << "\n";
522  return os;
523  }
524 
525 public:
526  // overwritten to implement covariance propagation
527 
529  {
530  typedef typename Base::RotationType RotationType;
531  RotationType inv = this->r.inverse();
532 
533  CovMatrixType J;
534 
535  T sinr = std::sin(this->r.angle());
536  T cosr = std::cos(this->r.angle());
537 
538  // compute the jacobians
539  J << -cosr, -sinr, this->t(0)*sinr -this->t(1)*cosr,
540  sinr, -cosr, this->t(0)*cosr +this->t(1)*sinr,
541  0, 0, -1;
542 
543  CovMatrixType c = J*cov*J.transpose();
544  return RigidTransformCov(inv.RotationType::operator*(-this->t), inv, c);
545  }
546 
547  RigidTransformCov& operator*= (const RigidTransformCov& other)
548  {
549  propagateCov(cov,*this,other); //< must be done here, since operator* changes our value
550  Base::operator*=(other);
551  return *this;
552  }
553 
554  RigidTransformCov& operator*= (const Base& other)
555  {
556  propagateCov(cov,*this,other); //< must be done here, since operator* changes our value
557  Base::operator*=(other);
558  return *this;
559  }
560 
561  friend RigidTransformCov operator* (const RigidTransformCov& a,
562  const RigidTransformCov& b)
563  {
564  RigidTransformCov t = Base::mul(a,b);
565  propagateCov(t.cov,a,b);
566  return t;
567  }
568 
569  friend RigidTransformCov operator* (const RigidTransformCov& a,
570  const Base& b)
571  {
572  RigidTransformCov t = Base::mul(a,b);
573  propagateCov(t.cov,a,b);
574  return t;
575  }
576 
577  friend RigidTransformCov operator* (const Base& a,
578  const RigidTransformCov& b)
579  {
580  RigidTransformCov t = Base::mul(a,b);
581  propagateCov(t.cov,a,b);
582  return t;
583  }
584 
585 private:
586 
587  // propagates the covariance matices of both transforms (impl. see below)
588  static void propagateCov(CovMatrixType& oC, const RigidTransformCov& a,
589  const RigidTransformCov& w);
590  // same as above, but w does not carry any covariance (impl. see below)
591  static void propagateCov(CovMatrixType& oC, const RigidTransformCov& a,
592  const Base& w);
593  // same as above, but a does not carry any covariance (impl. see below)
594  static void propagateCov(CovMatrixType& C, const Base& a,
595  const RigidTransformCov& w);
596 
597 public:
598 
601 };
602 
604 
611 template <typename T>
612 class RigidTransform<T,3> : public RigidTransformBase<T, 3,
613  Eigen::Quaternion<T>, RigidTransform<T,3> >
614 {
616 
617 public:
618 
620  Base(Eigen::Matrix<T,3,1>(0,0,0), Eigen::Quaternion<T>::Identity()) {}
621 
623  const Eigen::Quaternion<T>& rotation) :
624  Base(translation,rotation) {}
625 
626  RigidTransform(T x, T y, T z, T yaw, T pitch, T roll) :
627  Base(Eigen::Matrix<T,3,1>(x,y,z),
628  quaternionFromYawPitchRoll(yaw,pitch,roll)) {}
629 
630 public:
631 
633  template <typename U>
635  return RigidTransform<U,3>(this->t.template cast<U>(),
636  this->r.template cast<U>());
637  }
638 
639 public:
640 
642  T& x() { return this->t.x(); }
644  T x() const { return this->t.x(); }
645 
647  T& y() { return this->t.y(); }
649  T y() const { return this->t.y(); }
650 
652  T& z() { return this->t.z(); }
654  T z() const { return this->t.z(); }
655 
656 
663  T yaw() const { return eulerAngles(this->r.toRotationMatrix(), 2,1,0)(0,0); }
664 
671  T pitch() const { return eulerAngles(this->r.toRotationMatrix(), 2,1,0)(1,0); }
672 
679  T roll() const { return eulerAngles(this->r.toRotationMatrix(), 2,1,0)(2,0); }
680 
681 public:
682 
683  template <typename Derived>
685  {
686  r.property("X", this->t[0], "The translational part of the transform");
687  r.property("Y", this->t[1], "The translational part of the transform");
688  r.property("Z", this->t[2], "The translational part of the transform");
689  // rotation
690  r.member("Rotation", this->r.coeffs(), "The rotational part");
691  }
692 
693  template <typename Derived>
695  {
696  r.property("X", this->t[0], "The translational part of the transform");
697  r.property("Y", this->t[1], "The translational part of the transform");
698  r.property("Z", this->t[2], "The translational part of the transform");
699  // rotation
700  r.member("Rotation", this->r.coeffs(), "The rotational part");
701  }
702 
703  template <typename Reflector>
704  void reflectRead(Reflector& r)
705  {
706  r.property("X", this->t[0], "The translational part of the transform");
707  r.property("Y", this->t[1], "The translational part of the transform");
708  r.property("Z", this->t[2], "The translational part of the transform");
709 
710  auto ypr = quaternionToYawPitchRoll(this->r);
711  T yaw = rad2deg(ypr.template get<0>());
712  T pitch = rad2deg(ypr.template get<1>());
713  T roll = rad2deg(ypr.template get<2>());
714  r.member("Yaw" , yaw, "The yaw angle (phi, in degrees)");
715  r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
716  r.member("Roll" , roll, "The roll angle (psi, in degrees)");
717  }
718 
719  template <typename Reflector>
720  void reflectWrite(Reflector& r)
721  {
722  T yaw, pitch, roll;
723  r.property("X", this->t[0], "The translational part of the transform");
724  r.property("Y", this->t[1], "The translational part of the transform");
725  r.property("Z", this->t[2], "The translational part of the transform");
726  r.member("Yaw" , yaw, "The yaw angle (phi, in degrees)");
727  r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
728  r.member("Roll" , roll, "The roll angle (psi, in degrees)");
729  this->r = quaternionFromYawPitchRoll(deg2rad(yaw),deg2rad(pitch),deg2rad(roll));
730  }
731 
733 
734  friend std::ostream& operator<<(std::ostream& os, const RigidTransform& tf)
735  {
736  os << "t: " << tf.t.transpose() << "\n";
737  os << "r: " << tf.r.w() << " " << tf.r.x() << " " << tf.r.y() << " " << tf.r.z() << "\n";
738  return os;
739  }
740 };
741 
743 
762 template <typename T>
763 class RigidTransformCov<T,3> : public RigidTransform<T, 3>
764 {
765  typedef RigidTransform<T, 3> Base;
766 
767 public:
769  static const int CovarianceDim = 7;
770 
772  static const int EulerCovarianceDim = 6;
773 
779 
785 
786 public:
787 
792  return CovMatrixType::Zero();
793  }
794 
800  return YawPitchRollCovMatrixType::Zero();
801  }
802 
803 public:
804 
805  RigidTransformCov() : Base(), cov(nullCov()) {}
806 
811  RigidTransformCov(const Base& transform) : Base(transform),
812  cov(nullCov()) {}
813 
819  const Eigen::Quaternion<T>& rotation,
820  const CovMatrixType& covariance) :
821  Base(translation,rotation), cov(covariance) {}
822 
828  const Eigen::Quaternion<T>& rotation,
829  const YawPitchRollCovMatrixType& yawPitchRollCov) :
830  Base(translation, rotation),
831  cov(quaternionCovFromYawPitchRollCov(yawPitchRollCov, rotation)){}
832 
837  RigidTransformCov(T x, T y, T z, T yaw, T pitch, T roll,
838  const YawPitchRollCovMatrixType& yawPitchRollCov) {
839  Eigen::Quaternion<T> rotation;
840  quaternionCovFromYawPitchRollCov(yawPitchRollCov, yaw, pitch, roll,
841  rotation, cov);
842  this->t = Eigen::Matrix<T,3,1>(x,y,z);
843  this->r = rotation;
844  }
845 
846 public:
847 
849  template <typename U>
851  typedef typename RigidTransformCov<U,3>::CovMatrixType CovMatrixTypeU;
852  return RigidTransformCov<U,3>(this->t.template cast<U>(),
853  this->r.template cast<U>(),
854  CovMatrixTypeU(this->cov.template cast<U>()));
855  }
856 
857 public:
858 
864  return quaternionCovToYawPitchRollCov(cov,this->r);
865  }
866 
867 public:
868 
869  template <typename Derived>
871  {
872  r.property("X", this->t[0], "The translational part of the transform");
873  r.property("Y", this->t[1], "The translational part of the transform");
874  r.property("Z", this->t[2], "The translational part of the transform");
875  // rotation
876  r.member("Rotation", this->r.coeffs(), "The rotational part");
877  // 7x7 cov matrix
878  r.member("Cov", cov, "The covariance matrix");
879  }
880 
881  template <typename Derived>
883  {
884  r.property("X", this->t[0], "The translational part of the transform");
885  r.property("Y", this->t[1], "The translational part of the transform");
886  r.property("Z", this->t[2], "The translational part of the transform");
887  // rotation
888  r.member("Rotation", this->r.coeffs(), "The rotational part");
889  // 7x7 cov matrix
890  r.member("Cov", cov, "The covariance matrix");
891  }
892 
893  template <typename Reflector>
894  void reflectRead(Reflector& r)
895  {
896  r.property("X", this->t[0], "The translational part of the transform");
897  r.property("Y", this->t[1], "The translational part of the transform");
898  r.property("Z", this->t[2], "The translational part of the transform");
899 
900  auto ypr = quaternionToYawPitchRoll(this->r);
901  T yaw = rad2deg(ypr.template get<0>());
902  T pitch = rad2deg(ypr.template get<1>());
903  T roll = rad2deg(ypr.template get<2>());
904  r.member("Yaw" , yaw, "The yaw angle (phi, in degrees)");
905  r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
906  r.member("Roll" , roll, "The roll angle (psi, in degrees)");
907 
908  // convert to yaw pitch roll cov
909  YawPitchRollCovMatrixType yprcov = quaternionCovToYawPitchRollCov(this->cov, this->r);
910  r.member("Cov", yprcov, "The covariance matrix for [x,y,z,qw,qx,qy,qz]");
911  }
912 
913  template <typename Reflector>
914  void reflectWrite(Reflector& r)
915  {
916  T yaw, pitch, roll;
917  r.property("X", this->t[0], "The translational part of the transform");
918  r.property("Y", this->t[1], "The translational part of the transform");
919  r.property("Z", this->t[2], "The translational part of the transform");
920  r.member("Yaw" , yaw, "The yaw angle (phi, in degrees)");
921  r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
922  r.member("Roll" , roll, "The roll angle (psi, in degrees)");
923 
924  yaw = deg2rad(yaw);
925  pitch = deg2rad(pitch);
926  roll = deg2rad(roll);
927  this->r = quaternionFromYawPitchRoll(yaw,pitch,roll);
928 
930  r.member("Cov", yprcov, "The covariance matrix for [x,y,z,qw,qx,qy,qz]");
931 
932  // convert to quaternion cov
933  this->cov = quaternionCovFromYawPitchRollCov(yprcov, yaw, pitch, roll);
934  }
935 
937 
938  friend std::ostream& operator<<(std::ostream& os, const RigidTransformCov& tf)
939  {
940  os << "t: " << tf.t.transpose() << "\n";
941  os << "r: " << tf.r.w() << " " << tf.r.x() << " " << tf.r.y() << " " << tf.r.z() << "\n";
942  os << "cov:\n" << tf.cov << "\n";
943  return os;
944  }
945 
946 public:
947 
948  // overwritten to implement covariance propagation
949 
951  {
952  typedef typename Base::RotationType RotationType;
953  RotationType inv = this->r.inverse();
954 
955  CovMatrixType J = -CovMatrixType::Identity();
956 
957  J(3,3) = 1.0;
958  J.template block<3,3>(0,0) <<
959  (T)2.0 * (this->r.y() * this->r.y() + this->r.z() * this->r.z()) - (T)1.0,
960  -(T)2.0 * (this->r.w() * this->r.z() + this->r.x() * this->r.y()) ,
961  (T)2.0 * (this->r.w() * this->r.y() - this->r.x() * this->r.z()) ,
962 
963  (T)2.0 * (this->r.w() * this->r.z() - this->r.x() * this->r.y()) ,
964  (T)2.0 * (this->r.x() * this->r.x() + this->r.z() * this->r.z()) - (T)1.0,
965  -(T)2.0 * (this->r.w() * this->r.x() + this->r.y() * this->r.z()) ,
966 
967  -(T)2.0 * (this->r.w() * this->r.y() + this->r.x() * this->r.z()) ,
968  (T)2.0 * (this->r.w() * this->r.x() - this->r.y() * this->r.z()) ,
969  (T)2.0 * (this->r.x() * this->r.x() + this->r.y() * this->r.y()) - (T)1.0;
970 
971  // signs seem to be partially wrong in blanco2010se3 (see YawPitchRoll.h)
972  // this now corresponds to MRPT 1.5.5 CPose3DQuat::inverseComposePoint
973  J.template block<3,4>(0,3) <<
974  -(T)2.0 * (this->r.z() * this->t(1) - this->r.y() * this->t(2) ),
975  -(T)2.0 * (this->r.y() * this->t(1) + this->r.z() * this->t(2) ),
976  -(T)2.0 * (this->r.x() * this->t(1) - (T)2.0 * this->r.y() * this->t(0) - this->r.w() * this->t(2)),
977  -(T)2.0 * (this->r.x() * this->t(2) + this->r.w() * this->t(1) - (T)2.0 * this->r.z() * this->t(0)),
978 
979  -(T)2.0 * (this->r.x() * this->t(2) - this->r.z() * this->t(0) ),
980  -(T)2.0 * (this->r.y() * this->t(0) - (T)2.0 * this->r.x() * this->t(1) + this->r.w() * this->t(2)),
981  -(T)2.0 * (this->r.x() * this->t(0) + this->r.z() * this->t(2) ),
982  -(T)2.0 * (this->r.y() * this->t(2) - (T)2.0 * this->r.z() * this->t(1) - this->r.w() * this->t(0)),
983 
984  -(T)2.0 * (this->r.y() * this->t(0) - this->r.x() * this->t(1) ),
985  -(T)2.0 * (this->r.z() * this->t(0) - this->r.w() * this->t(1) - (T)2.0 * this->r.x() * this->t(2)),
986  -(T)2.0 * (this->r.z() * this->t(1) + this->r.w() * this->t(0) - (T)2.0 * this->r.y() * this->t(2)),
987  -(T)2.0 * (this->r.x() * this->t(0) + this->r.y() * this->t(1) );
988 
989  CovMatrixType c = J*cov*J.transpose();
990  return RigidTransformCov(inv.RotationType::operator*(-this->t), inv, c);
991  }
992 
993  RigidTransformCov& operator*= (const RigidTransformCov& other)
994  {
995  propagateCov(cov,*this,other); // must be done here, since operator* changes our value
996  Base::operator*=(other);
997  return *this;
998  }
999 
1000  RigidTransformCov& operator*= (const Base& other)
1001  {
1002  propagateCov(cov,*this,other); // must be done here, since operator* changes our value
1003  Base::operator*=(other);
1004  return *this;
1005  }
1006 
1007  friend RigidTransformCov operator* (const RigidTransformCov& a,
1008  const RigidTransformCov& b)
1009  {
1010  RigidTransformCov t = Base::mul(a,b);
1011  propagateCov(t.cov,a,b);
1012  return t;
1013  }
1014 
1015  friend RigidTransformCov operator* (const RigidTransformCov& a,
1016  const Base& b)
1017  {
1018  RigidTransformCov t = Base::mul(a,b);
1019  propagateCov(t.cov,a,b);
1020  return t;
1021  }
1022 
1023  friend RigidTransformCov operator* (const Base& a,
1024  const RigidTransformCov& b)
1025  {
1026  RigidTransformCov t = Base::mul(a,b);
1027  propagateCov(t.cov,a,b);
1028  return t;
1029  }
1030 
1031 private:
1032 
1033  // propagates the covariance matices to of both transforms (impl. see below)
1034  static void propagateCov(CovMatrixType& oC,
1035  const RigidTransformCov& w, const RigidTransformCov& a);
1036  // same as above, but w does not carry any covariance (impl. see below)
1037  static void propagateCov(CovMatrixType& oC,
1038  const RigidTransformCov& w, const Base& a);
1039  // same as above, but a does not carry any covariance (impl. see below)
1040  static void propagateCov(CovMatrixType& C,
1041  const Base& w, const RigidTransformCov& a);
1042 
1043 public:
1044 
1047 };
1048 
1050 
1055 
1060 
1065 
1070 
1072 
1073 template <>
1074 class IsCheapToCopy<RigidTransform2f> : public std::true_type {};
1075 
1076 template <>
1077 class IsCheapToCopy<RigidTransform3f> : public std::true_type {};
1078 
1079 template <>
1080 class IsCheapToCopy<RigidTransform2d> : public std::true_type {};
1081 
1082 template <>
1083 class IsCheapToCopy<RigidTransform3d> : public std::true_type {};
1084 
1085 template <>
1086 class IsCheapToCopy<RigidTransformCov2f> : public std::true_type {};
1087 
1088 template <>
1089 class IsCheapToCopy<RigidTransformCov3f> : public std::true_type {};
1090 
1091 template <>
1092 class IsCheapToCopy<RigidTransformCov2d> : public std::true_type {};
1093 
1094 template <>
1095 class IsCheapToCopy<RigidTransformCov3d> : public std::true_type {};
1096 
1098 // Covariance propagation maths
1099 
1100 // 2D case:
1101 //
1102 // g(a,w) = w * a
1103 // = [ Rw * ta + tw ;
1104 // phia + phiw ]
1105 //
1106 // Ja = d g(a,w) / da
1107 // Jw = d g(a,w) / dw
1108 // C = Ja*Ca*Ja' + Jw*Cw*Jw'
1109 
1110 template <typename T>
1111 inline void RigidTransformCov<T,2>::propagateCov(CovMatrixType& oC,
1112  const RigidTransformCov& w,
1113  const RigidTransformCov& a)
1114 {
1115  // NOTE: change oC at the end only, since it may be a reference on
1116  // the parameter w or a!
1117 
1118  typedef Eigen::Matrix<T,3,3> Mat3;
1119  Mat3 Ja, Jw;
1120 
1121  T sinrw = std::sin(w.r.angle());
1122  T cosrw = std::cos(w.r.angle());
1123 
1124  // compute the jacobians
1125  Ja << cosrw, -sinrw, 0,
1126  sinrw, cosrw, 0,
1127  0, 0, 1;
1128 
1129  Jw << 1, 0, -a.t(0)*sinrw -a.t(1)*cosrw,
1130  0, 1, a.t(0)*cosrw -a.t(1)*sinrw,
1131  0, 0, 1;
1132 
1133  // propagate the covariances:
1134  oC = Ja * a.cov * Ja.transpose() + Jw * w.cov * Jw.transpose();
1135 }
1136 
1137 // same as above, but w does not carry any covariance
1138 template <typename T>
1139 inline void RigidTransformCov<T,2>::propagateCov(CovMatrixType& oC,
1140  const RigidTransformCov& w,
1141  const Base& a)
1142 {
1143  // NOTE: change oC at the end only,
1144  // since it may be a reference on the parameter w or a!
1145 
1146  typedef Eigen::Matrix<T,3,3> Mat3;
1147  Mat3 Jw;
1148 
1149  T sinrw = std::sin(w.r.angle());
1150  T cosrw = std::cos(w.r.angle());
1151 
1152  // compute the jacobians
1153  Jw << 1, 0, -a.t(0)*sinrw -a.t(1)*cosrw,
1154  0, 1, a.t(0)*cosrw -a.t(1)*sinrw,
1155  0, 0, 1;
1156 
1157  // propagate the covariances:
1158  oC = Jw * w.cov * Jw.transpose();
1159 }
1160 
1161 template <typename T>
1162 inline void RigidTransformCov<T,2>::propagateCov(CovMatrixType& oC,
1163  const Base& w,
1164  const RigidTransformCov& a)
1165 {
1166  // NOTE: change oC at the end only,
1167  // since it may be a reference on the parameter w or a!
1168 
1169  typedef Eigen::Matrix<T,3,3> Mat3;
1170  Mat3 Ja;
1171 
1172  T sinrw = std::sin(w.r.angle());
1173  T cosrw = std::cos(w.r.angle());
1174 
1175  // compute the jacobians
1176  Ja << cosrw, -sinrw, 0,
1177  sinrw, cosrw, 0,
1178  0, 0, 1;
1179 
1180  // propagate the covariances:
1181  oC = Ja * a.cov * Ja.transpose();
1182 }
1183 
1184 // 3D case:
1185 //
1186 //
1187 // g(a,w) = w * a
1188 // = [ qw * ta + tw ;
1189 // qw * qa ]
1190 //
1191 // Ja = d g(a,w) / da
1192 // Jw = d g(a,w) / dw
1193 // C = Ja*Ca*Ja' + Jw*Cw*Jw'
1194 template <typename T>
1195 inline void RigidTransformCov<T,3>::propagateCov(CovMatrixType& oC,
1196  const RigidTransformCov& w,
1197  const RigidTransformCov& a)
1198 {
1199  // NOTE: change oC at the end only,
1200  // since it may be a reference on the parameter w or a!
1201 
1202  CovMatrixType Jw = CovMatrixType::Identity();
1203 
1204  Jw.template block<3,4>(0,3) <<
1205  - w.r.z()*a.t(1,0)+ w.r.y()*a.t(2,0),
1206  w.r.y()*a.t(1,0)+ w.r.z()*a.t(2,0),
1207  -(T)2.0*w.r.y()*a.t(0,0)+ w.r.x()*a.t(1,0)+ w.r.w()*a.t(2,0),
1208  -(T)2.0*w.r.z()*a.t(0,0)- w.r.w()*a.t(1,0)+ w.r.x()*a.t(2,0),
1209 
1210  w.r.z()*a.t(0,0) - w.r.x()*a.t(2,0),
1211  w.r.y()*a.t(0,0)-(T)2.0*w.r.x()*a.t(1,0)- w.r.w()*a.t(2,0),
1212  w.r.x()*a.t(0,0) + w.r.z()*a.t(2,0),
1213  w.r.w()*a.t(0,0)-(T)2.0*w.r.z()*a.t(1,0)+ w.r.y()*a.t(2,0),
1214 
1215  -w.r.y()*a.t(0,0)+ w.r.x()*a.t(1,0) ,
1216  w.r.z()*a.t(0,0)+ w.r.w()*a.t(1,0)-(T)2.0*w.r.x()*a.t(2,0),
1217  -w.r.w()*a.t(0,0)+ w.r.z()*a.t(1,0)-(T)2.0*w.r.y()*a.t(2,0),
1218  w.r.x()*a.t(0,0)+ w.r.y()*a.t(1,0);
1219  Jw.template block<3,4>(0,3) *= (T)2.0;
1220 
1221  Jw.template block<4,4>(3,3) <<
1222  a.r.w(),-a.r.x(),-a.r.y(),-a.r.z(),
1223  a.r.x(), a.r.w(), a.r.z(),-a.r.y(),
1224  a.r.y(),-a.r.z(), a.r.w(), a.r.x(),
1225  a.r.z(), a.r.y(),-a.r.x(), a.r.w();
1226 
1227  CovMatrixType Ja = CovMatrixType::Identity();
1228 
1229  Ja.template block<3,3>(0,0) <<
1230  (T)0.5-w.r.y()*w.r.y()-w.r.z()*w.r.z(),
1231  w.r.x()*w.r.y()-w.r.w()*w.r.z(),
1232  w.r.w()*w.r.y()+w.r.x()*w.r.z(),
1233 
1234  w.r.w()*w.r.z()+w.r.x()*w.r.y(),
1235  (T)0.5-w.r.x()*w.r.x()-w.r.z()*w.r.z(),
1236  w.r.y()*w.r.z()-w.r.w()*w.r.x(),
1237 
1238  w.r.x()*w.r.z()-w.r.w()*w.r.y(),
1239  w.r.w()*w.r.x()+w.r.y()*w.r.z(),
1240  (T)0.5-w.r.x()*w.r.x()-w.r.y()*w.r.y();
1241  Ja.template block<3,3>(0,0) *= (T)2.0;
1242 
1243  Ja.template block<4,4>(3,3) <<
1244  w.r.w(), -w.r.x(), -w.r.y(), -w.r.z(),
1245  w.r.x(), w.r.w(), -w.r.z(), w.r.y(),
1246  w.r.y(), w.r.z(), w.r.w(), -w.r.x(),
1247  w.r.z(), -w.r.y(), w.r.x(), w.r.w();
1248 
1249  // propagate the covariances:
1250  oC = Jw * w.cov * Jw.transpose() + Ja * a.cov * Ja.transpose();
1251 }
1252 
1253 template <typename T>
1254 inline void RigidTransformCov<T,3>::propagateCov(CovMatrixType& oC,
1255  const RigidTransformCov& w,
1256  const Base& a)
1257 {
1258  // NOTE: change oC at the end only,
1259  // since it may be a reference on the parameter w or a!
1260 
1261  CovMatrixType Jw = CovMatrixType::Identity();
1262 
1263  Jw.template block<3,4>(0,3) <<
1264  - w.r.z()*a.t(1,0)+ w.r.y()*a.t(2,0),
1265  w.r.y()*a.t(1,0)+ w.r.z()*a.t(2,0),
1266  -(T)2.0*w.r.y()*a.t(0,0)+ w.r.x()*a.t(1,0)+ w.r.w()*a.t(2,0),
1267  -(T)2.0*w.r.z()*a.t(0,0)- w.r.w()*a.t(1,0)+ w.r.x()*a.t(2,0),
1268 
1269  w.r.z()*a.t(0,0) - w.r.x()*a.t(2,0),
1270  w.r.y()*a.t(0,0)-(T)2.0*w.r.x()*a.t(1,0)- w.r.w()*a.t(2,0),
1271  w.r.x()*a.t(0,0) + w.r.z()*a.t(2,0),
1272  w.r.w()*a.t(0,0)-(T)2.0*w.r.z()*a.t(1,0)+ w.r.y()*a.t(2,0),
1273 
1274  -w.r.y()*a.t(0,0)+ w.r.x()*a.t(1,0) ,
1275  w.r.z()*a.t(0,0)+ w.r.w()*a.t(1,0)-2.0*w.r.x()*a.t(2,0),
1276  -w.r.w()*a.t(0,0)+ w.r.z()*a.t(1,0)-2.0*w.r.y()*a.t(2,0),
1277  w.r.x()*a.t(0,0)+ w.r.y()*a.t(1,0);
1278  Jw.template block<3,4>(0,3) *= (T)2.0;
1279 
1280  Jw.template block<4,4>(3,3) <<
1281  a.r.w(),-a.r.x(),-a.r.y(),-a.r.z(),
1282  a.r.x(), a.r.w(), a.r.z(),-a.r.y(),
1283  a.r.y(),-a.r.z(), a.r.w(), a.r.x(),
1284  a.r.z(), a.r.y(),-a.r.x(), a.r.w();
1285 
1286  // propagate the covariances:
1287  oC = Jw * w.cov * Jw.transpose();
1288 }
1289 
1290 template <typename T>
1291 inline void RigidTransformCov<T,3>::propagateCov(CovMatrixType& oC,
1292  const Base& w,
1293  const RigidTransformCov& a)
1294 {
1295  // NOTE: change oC at the end only,
1296  // since it may be a reference on the parameter w or a!
1297 
1298  CovMatrixType Ja = CovMatrixType::Identity();
1299 
1300  Ja.template block<3,3>(0,0) <<
1301  (T)0.5-w.r.y()*w.r.y()-w.r.z()*w.r.z(),
1302  w.r.x()*w.r.y()-w.r.w()*w.r.z(),
1303  w.r.w()*w.r.y()+w.r.x()*w.r.z(),
1304 
1305  w.r.w()*w.r.z()+w.r.x()*w.r.y(),
1306  (T)0.5-w.r.x()*w.r.x()-w.r.z()*w.r.z(),
1307  w.r.y()*w.r.z()-w.r.w()*w.r.x(),
1308 
1309  w.r.x()*w.r.z()-w.r.w()*w.r.y(),
1310  w.r.w()*w.r.x()+w.r.y()*w.r.z(),
1311  (T)0.5-w.r.x()*w.r.x()-w.r.y()*w.r.y();
1312  Ja.template block<3,3>(0,0) *= (T)2.0;
1313 
1314  Ja.template block<4,4>(3,3) <<
1315  w.r.w(), -w.r.x(), -w.r.y(), -w.r.z(),
1316  w.r.x(), w.r.w(), -w.r.z(), w.r.y(),
1317  w.r.y(), w.r.z(), w.r.w(), -w.r.x(),
1318  w.r.z(), -w.r.y(), w.r.x(), w.r.w();
1319 
1320  // propagate the covariances:
1321  oC = Ja * a.cov * Ja.transpose();
1322 }
1323 
1325 // Linear interpolation stuff
1326 
1336 template <typename T, int D, typename S>
1338  const RigidTransform<T, D>& t2,
1339  S alpha)
1340 {
1341  return RigidTransform<T, D>(lerp(t1.t, t2.t, alpha), // interpolate translation
1342  lerp(t1.r, t2.r, alpha)); // interpolate rotation
1343 }
1344 
1354 template <typename T, int D, typename S>
1356  const RigidTransformCov<T, D>& t2,
1357  S alpha)
1358 {
1359  return RigidTransformCov<T, D>(lerp(t1.t, t2.t, alpha), // interpolate translation
1360  lerp(t1.r, t2.r, alpha), // interpolate rotation
1361  lerp(t1.cov, t2.cov, alpha)); // interpolate covariance
1362 }
1363 
1364 
1366 // Sanity checks
1367 
1377 template<typename T, int D>
1379 {
1380  const auto transformMatrix = p.getMatrix();
1381  constexpr auto matrixDim = RigidTransform<T, D>::HDim;
1382  for (int i = 0; i < matrixDim; ++i) {
1383  for (int j = 0; j < matrixDim; ++j) {
1384  if (boost::math::isnan(transformMatrix(i,j))) {
1385  return false;
1386  }
1387  }
1388  }
1389  return true;
1390 }
1391 
1402 template<typename T, int D>
1404 {
1405  const auto transformValid = isWellDefined(static_cast<const RigidTransform<T, D>&>(p));
1406  if (!transformValid) {
1407  return false;
1408  }
1409 
1410  constexpr auto precision = Eigen::NumTraits<T>::dummy_precision();
1411  constexpr auto covDim = RigidTransformCov<T, D>::CovarianceDim;
1412 
1413  // check for nan
1414  for (int i = 0; i < covDim; ++i) {
1415  for (int j = 0; j < covDim; ++j) {
1416  if (boost::math::isnan(p.cov(i, j))) {
1417  return false;
1418  }
1419  }
1420  }
1421 
1422  // check for symmetry
1423  for (int i = 0; i < covDim; ++i) {
1424  for (int j = i + 1; j < covDim; ++j) {
1425  const bool almostEqual = std::abs(p.cov(i, j) - p.cov(j, i)) <= precision;
1426  if (!almostEqual) {
1427  return false;
1428  }
1429  }
1430  }
1431 
1432  // is positive semi definite?
1434  const auto covValid = ldlt.info() != Eigen::NumericalIssue && ldlt.isPositive();
1435  return covValid;
1436 }
1437 
1439 // specializations for Dx1 and dynamic matrices
1440 
1442 template< int D, typename TransformDerived, typename OtherDerived>
1443 struct ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived,D, 1>
1444 {
1445  typedef typename OtherDerived::Scalar Scalar;
1446  typedef RigidTransformBase<Scalar, D, typename TransformDerived::RotationType, TransformDerived> TTransform;
1447  typedef const Eigen::Matrix<Scalar, D, 1> TResult;
1448 
1449 
1450  static TResult run(const TTransform& t, const OtherDerived& v)
1451  {
1452  // TODO: increase performance by using expression templates see Eigen/Geometry/Transform.h
1453  return t.r*v + t.t;
1454  }
1455 };
1456 
1457 template< int D, typename TransformDerived, typename OtherDerived>
1458 struct ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived,Eigen::Dynamic, Eigen::Dynamic>
1459 {
1460  typedef typename OtherDerived::Scalar Scalar;
1461  typedef RigidTransformBase<Scalar, D, typename TransformDerived::RotationType, TransformDerived> TTransform;
1462  typedef const Eigen::Matrix<Scalar, D, 1> TResult;
1463 
1464 
1465  static TResult run(const TTransform& t, const OtherDerived& v)
1466  {
1467  // TODO: increase performance by using expression templates see Eigen/Geometry/Transform.h
1468  return t.r*v + t.t;
1469  }
1470 };
1471 
1473 
1475 
1476 }
1477 
1478 #endif
RigidTransformCov(const Base &transform)
Initialization from corresponding RigidTransform (without covariance).
Definition: RigidTransform.h:435
T Type
The used floating point type (float, double, long double)
Definition: RigidTransform.h:146
CovMatrixType cov
the covariance of the transform as matrix:
Definition: RigidTransform.h:1046
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:82
void reflect(BinarySerializer< Derived > &r)
Definition: RigidTransform.h:684
Specialization of RigidTransform for 3 dimensions.
Definition: RigidTransform.h:612
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:226
Typedefs for OS independent basic data types.
static CovMatrixType nullCov()
Returns a "null covariance matrix".
Definition: RigidTransform.h:791
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:805
Include file for all eigen related things.
RigidTransformCov< U, 2 > cast() const
Returns *this casted to U.
Definition: RigidTransform.h:502
RigidTransform< float, 2 > RigidTransform2f
Typedef for 2D float transform.
Definition: RigidTransform.h:1052
size of the corresponding homogeneous space
Definition: RigidTransform.h:162
T phi() const
Returns the rotation angle represented by this transform in radian.
Definition: RigidTransform.h:364
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:422
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:492
RigidTransformCov()
Definition: RigidTransform.h:428
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:470
void member(const char *name, T &member, const char *comment, ReflectCtrlFlags flags=REFLECT_CTRLFLAG_NONE)
Definition: RecursiveMemberReflector.h:862
This class represents an affine transformation that supports a translation followed by a rotation (a ...
Definition: RigidTransform.h:128
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:362
TransformDerived * This()
casts this to the actual derived type (see Curiously recurring template pattern)
Definition: RigidTransform.h:263
#define MIRA_SPLIT_REFLECT_MEMBER
Macro that insert a class member reflect() method just splitting reflection into a reflectRead() and ...
Definition: SplitReflect.h:209
Eigen::Matrix< T, CovarianceDim, CovarianceDim > CovMatrixType
The type of the matrix representing the covariance of the transformation.
Definition: RigidTransform.h:417
#define MIRA_REFLECT_BASE(reflector, BaseClass)
Macro that can be used to reflect the base class easily.
Definition: ReflectorInterface.h:912
Definition: YawPitchRoll.h:684
Quaternion inverse(void) const
RigidTransform< float, 3 > RigidTransform3f
Typedef for 3D float transform.
Definition: RigidTransform.h:1054
bool isWellDefined(const RigidTransform< T, D > &p)
Checks whether a RigidTransform is valid.
Definition: RigidTransform.h:1378
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:837
Implementation of RigidTransforms with different dimensionality D.
Definition: RigidTransform.h:141
static TransformDerived mul(const RigidTransformBase &a, const RigidTransformBase &b)
Definition: RigidTransform.h:271
Rotation2D inverse() const
void property(const char *name, T &member, const char *comment, PropertyHint &&hint=PropertyHint(), ReflectCtrlFlags flags=REFLECT_CTRLFLAG_NONE)
Definition: RecursiveMemberReflector.h:964
Eigen::Matrix< T, EulerCovarianceDim, EulerCovarianceDim > YawPitchRollCovMatrixType
The type of the matrix representing the euler covariance of the transformation.
Definition: RigidTransform.h:784
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RigidTransformBase(TranslationType translation, RotationType rotation)
Definition: RigidTransform.h:175
YawPitchRollCovMatrixType getYawPitchRollCov() const
Returns covariance matrix where the rotation covariance is represented using yaw, pitch...
Definition: RigidTransform.h:863
Functions for linear interpolation of different types like scalars, angles and rotations.
CovMatrixType cov
The covariance of the transform as matrix.
Definition: RigidTransform.h:600
void reflectWrite(Reflector &r)
Definition: RigidTransform.h:914
RigidTransformCov(const Base &transform)
Initialization from corresponding RigidTransform (without covariance).
Definition: RigidTransform.h:811
RigidTransformCov inverse() const
Definition: RigidTransform.h:528
Eigen::Matrix< T, D, 1 > TranslationType
Vector type used to represent the translational part (dimension depends on dimensionality of transfor...
Definition: RigidTransform.h:152
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:481
friend std::ostream & operator<<(std::ostream &os, const RigidTransformCov &tf)
Definition: RigidTransform.h:517
RigidTransformCov< double, 3 > RigidTransformCov3d
Typedef for 3D double transform with covariance.
Definition: RigidTransform.h:1069
RigidTransform< U, 2 > cast() const
Returns *this casted to U.
Definition: RigidTransform.h:344
MIRA_SPLIT_REFLECT_MEMBER friend std::ostream & operator<<(std::ostream &os, const RigidTransform &tf)
Definition: RigidTransform.h:734
T pitch() const
Returns the pitch angle in rad.
Definition: RigidTransform.h:671
dimension of the Eucleadian space
Definition: RigidTransform.h:161
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:1067
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:336
TRotation RotationType
Type used to represent the rotational part (Eigen::Rotation2D for 2D transforms, Eigen::Quaternion fo...
Definition: RigidTransform.h:158
By default, IsCheapToCopy<T>::value evaluates to true for fundamental types T, false for all other ty...
Definition: IsCheapToCopy.h:63
MIRA_SPLIT_REFLECT_MEMBER friend std::ostream & operator<<(std::ostream &os, const RigidTransformCov &tf)
Definition: RigidTransform.h:938
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:442
T roll() const
Returns the roll angle in rad.
Definition: RigidTransform.h:679
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:319
MatrixType getMatrix() const
Computes and returns the matrix that describes the affine transformation in homogeneous space...
Definition: RigidTransform.h:252
void reflectRead(Reflector &r)
Definition: RigidTransform.h:704
T & x()
Returns the x-coordinate of the translational part of the transform.
Definition: RigidTransform.h:642
RigidTransformCov< float, 2 > RigidTransformCov2f
Typedef for 2D float transform with covariance.
Definition: RigidTransform.h:1062
Type trait to define if a class is cheap to copy.
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:352
RigidTransform< U, 3 > cast() const
Returns *this casted to U.
Definition: RigidTransform.h:634
TransformDerived & operator*=(const RigidTransformBase &other)
Concatenates this transform with an other transform.
Definition: RigidTransform.h:204
RotationType r
The rotational part of the transform.
Definition: RigidTransform.h:285
friend std::ostream & operator<<(std::ostream &os, const RigidTransform &tf)
Definition: RigidTransform.h:378
T yaw() const
Returns the yaw angle in rad (This corresponds to phi in a 2D transform).
Definition: RigidTransform.h:663
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:818
void reflect(BinarySerializer< Derived > &r)
Definition: RigidTransform.h:870
Specialization of RigidTransform for 2 dimensions.
Definition: RigidTransform.h:297
friend TransformDerived operator*(const RigidTransformBase &a, const RigidTransformBase &b)
Concatenates the two transformations.
Definition: RigidTransform.h:214
void reflect(Reflector &m)
Definition: RigidTransform.h:369
RigidTransformCov< U, 3 > cast() const
Returns *this casted to U.
Definition: RigidTransform.h:850
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:193
RigidTransformCov< float, 3 > RigidTransformCov3f
Typedef for 3D float transform with covariance.
Definition: RigidTransform.h:1064
T & y()
Returns the y-coordinate of the translational part of the transform.
Definition: RigidTransform.h:647
RigidTransformCov inverse() const
Definition: RigidTransform.h:950
TransformDerived inverse() const
Computes and returns the inverse transform of this.
Definition: RigidTransform.h:182
T & y()
Returns the y-coordinate of the translational part of the transform.
Definition: RigidTransform.h:357
void reflect(Reflector &r)
Definition: RigidTransform.h:511
static YawPitchRollCovMatrixType nullYawPitchRollCov()
Returns an "null covariance matrix" witch yaw, pitch, roll rotation representation.
Definition: RigidTransform.h:799
T x() const
Returns the x-coordinate of the translational part of the transform.
Definition: RigidTransform.h:354
RigidTransform< double, 3 > RigidTransform3d
Typedef for 3D double transform.
Definition: RigidTransform.h:1059
T y() const
Returns the y-coordinate of the translational part of the transform.
Definition: RigidTransform.h:359
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:169
T x() const
Returns the x-coordinate of the translational part of the transform.
Definition: RigidTransform.h:644
T y() const
Returns the y-coordinate of the translational part of the transform.
Definition: RigidTransform.h:649
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:326
Definition: BinarySerializer.h:257
RigidTransform< double, 2 > RigidTransform2d
Typedef for 2D double transform.
Definition: RigidTransform.h:1057
const TransformDerived * This() const
casts this to the actual derived type (see Curiously recurring template pattern)
Definition: RigidTransform.h:265
void reflectRead(Reflector &r)
Definition: RigidTransform.h:894
PropertyHint precision(int p)
Sets the attribute "precision".
Definition: PropertyHint.h:285
RigidTransform(T x, T y, T z, T yaw, T pitch, T roll)
Definition: RigidTransform.h:626
RigidTransform(const Eigen::Matrix< T, 3, 1 > &translation, const Eigen::Quaternion< T > &rotation)
Definition: RigidTransform.h:622
void reflect(BinaryDeserializer< Derived > &r)
Definition: RigidTransform.h:694
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:106
RigidTransform()
Definition: RigidTransform.h:619
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:304
T & z()
Returns the z-coordinate of the translational part of the transform.
Definition: RigidTransform.h:652
void reflectWrite(Reflector &r)
Definition: RigidTransform.h:720
This class represents an affine transformation that supports a translation followed by a rotation (a ...
Definition: RigidTransform.h:101
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:451
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:827
Eigen::Matrix< T, CovarianceDim, CovarianceDim > CovMatrixType
The type of the matrix representing the covariance of the transformation.
Definition: RigidTransform.h:778
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:460
TranslationType t
Vector that describes the translational part of the transform.
Definition: RigidTransform.h:282
Provides definition for getters and setters that are used with the serialization framework.
void reflect(BinaryDeserializer< Derived > &r)
Definition: RigidTransform.h:882
T z() const
Returns the z-coordinate of the translational part of the transform.
Definition: RigidTransform.h:654
Base class template for derived Angle implementations.
Definition: Angle.h:183
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:311