Transform.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
6 // Copyright (C) 2010 Hauke Heibel <hauke.heibel@gmail.com>
7 //
8 // This Source Code Form is subject to the terms of the Mozilla
9 // Public License v. 2.0. If a copy of the MPL was not distributed
10 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
11 
12 #ifndef EIGEN_TRANSFORM_H
13 #define EIGEN_TRANSFORM_H
14 
15 namespace Eigen {
16 
17 namespace internal {
18 
19 template<typename Transform>
21 {
22  enum
23  {
24  Dim = Transform::Dim,
25  HDim = Transform::HDim,
26  Mode = Transform::Mode,
28  };
29 };
30 
31 template< typename TransformType,
32  typename MatrixType,
34  : int(MatrixType::RowsAtCompileTime) == int(transform_traits<TransformType>::HDim) ? 1
35  : 2,
36  int RhsCols = MatrixType::ColsAtCompileTime>
38 
39 template< typename Other,
40  int Mode,
41  int Options,
42  int Dim,
43  int HDim,
44  int OtherRows=Other::RowsAtCompileTime,
45  int OtherCols=Other::ColsAtCompileTime>
47 
48 template< typename Lhs,
49  typename Rhs,
50  bool AnyProjective =
54 
55 template< typename Other,
56  int Mode,
57  int Options,
58  int Dim,
59  int HDim,
60  int OtherRows=Other::RowsAtCompileTime,
61  int OtherCols=Other::ColsAtCompileTime>
63 
64 template<typename TransformType> struct transform_take_affine_part;
65 
66 template<typename _Scalar, int _Dim, int _Mode, int _Options>
67 struct traits<Transform<_Scalar,_Dim,_Mode,_Options> >
68 {
69  typedef _Scalar Scalar;
71  typedef Dense StorageKind;
72  enum {
73  Dim1 = _Dim==Dynamic ? _Dim : _Dim + 1,
74  RowsAtCompileTime = _Mode==Projective ? Dim1 : _Dim,
75  ColsAtCompileTime = Dim1,
76  MaxRowsAtCompileTime = RowsAtCompileTime,
77  MaxColsAtCompileTime = ColsAtCompileTime,
78  Flags = 0
79  };
80 };
81 
82 template<int Mode> struct transform_make_affine;
83 
84 } // end namespace internal
85 
203 template<typename _Scalar, int _Dim, int _Mode, int _Options>
204 class Transform
205 {
206 public:
208  enum {
209  Mode = _Mode,
210  Options = _Options,
211  Dim = _Dim,
212  HDim = _Dim+1,
213  Rows = int(Mode)==(AffineCompact) ? Dim : HDim
214  };
216  typedef _Scalar Scalar;
218  typedef Eigen::Index Index;
219 
222  typedef const MatrixType ConstMatrixType;
230  typedef typename internal::conditional<int(Mode)==int(AffineCompact),
231  MatrixType&,
234  typedef typename internal::conditional<int(Mode)==int(AffineCompact),
235  const MatrixType&,
245 
246  // this intermediate enum is needed to avoid an ICE with gcc 3.4 and 4.0
247  enum { TransformTimeDiagonalMode = ((Mode==int(Isometry))?Affine:int(Mode)) };
250 
251 protected:
252 
253  MatrixType m_matrix;
254 
255 public:
256 
260  {
261  check_template_params();
263  }
264 
265  EIGEN_DEVICE_FUNC inline explicit Transform(const TranslationType& t)
266  {
267  check_template_params();
268  *this = t;
269  }
271  {
272  check_template_params();
273  *this = s;
274  }
275  template<typename Derived>
277  {
278  check_template_params();
279  *this = r;
280  }
281 
283 
285  template<typename OtherDerived>
287  {
289  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY);
290 
291  check_template_params();
293  }
294 
296  template<typename OtherDerived>
298  {
300  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY);
301 
303  return *this;
304  }
305 
306  template<int OtherOptions>
308  {
309  check_template_params();
310  // only the options change, we can directly copy the matrices
311  m_matrix = other.matrix();
312  }
313 
314  template<int OtherMode,int OtherOptions>
316  {
317  check_template_params();
318  // prevent conversions as:
319  // Affine | AffineCompact | Isometry = Projective
321  YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION)
322 
323  // prevent conversions as:
324  // Isometry = Affine | AffineCompact
325  EIGEN_STATIC_ASSERT(EIGEN_IMPLIES(OtherMode==int(Affine)||OtherMode==int(AffineCompact), Mode!=int(Isometry)),
326  YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION)
327 
328  enum { ModeIsAffineCompact = Mode == int(AffineCompact),
329  OtherModeIsAffineCompact = OtherMode == int(AffineCompact)
330  };
331 
332  if(EIGEN_CONST_CONDITIONAL(ModeIsAffineCompact == OtherModeIsAffineCompact))
333  {
334  // We need the block expression because the code is compiled for all
335  // combinations of transformations and will trigger a compile time error
336  // if one tries to assign the matrices directly
337  m_matrix.template block<Dim,Dim+1>(0,0) = other.matrix().template block<Dim,Dim+1>(0,0);
338  makeAffine();
339  }
340  else if(EIGEN_CONST_CONDITIONAL(OtherModeIsAffineCompact))
341  {
342  typedef typename Transform<Scalar,Dim,OtherMode,OtherOptions>::MatrixType OtherMatrixType;
344  }
345  else
346  {
347  // here we know that Mode == AffineCompact and OtherMode != AffineCompact.
348  // if OtherMode were Projective, the static assert above would already have caught it.
349  // So the only possibility is that OtherMode == Affine
350  linear() = other.linear();
351  translation() = other.translation();
352  }
353  }
354 
355  template<typename OtherDerived>
357  {
358  check_template_params();
359  other.evalTo(*this);
360  }
361 
362  template<typename OtherDerived>
364  {
365  other.evalTo(*this);
366  return *this;
367  }
368 
369  #ifdef EIGEN_QT_SUPPORT
370  inline Transform(const QMatrix& other);
371  inline Transform& operator=(const QMatrix& other);
372  inline QMatrix toQMatrix(void) const;
373  inline Transform(const QTransform& other);
374  inline Transform& operator=(const QTransform& other);
375  inline QTransform toQTransform(void) const;
376  #endif
377 
378  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return int(Mode)==int(Projective) ? m_matrix.cols() : (m_matrix.cols()-1); }
379  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
380 
383  EIGEN_DEVICE_FUNC inline Scalar operator() (Index row, Index col) const { return m_matrix(row,col); }
386  EIGEN_DEVICE_FUNC inline Scalar& operator() (Index row, Index col) { return m_matrix(row,col); }
387 
389  EIGEN_DEVICE_FUNC inline const MatrixType& matrix() const { return m_matrix; }
391  EIGEN_DEVICE_FUNC inline MatrixType& matrix() { return m_matrix; }
392 
394  EIGEN_DEVICE_FUNC inline ConstLinearPart linear() const { return ConstLinearPart(m_matrix,0,0); }
396  EIGEN_DEVICE_FUNC inline LinearPart linear() { return LinearPart(m_matrix,0,0); }
397 
399  EIGEN_DEVICE_FUNC inline ConstAffinePart affine() const { return take_affine_part::run(m_matrix); }
401  EIGEN_DEVICE_FUNC inline AffinePart affine() { return take_affine_part::run(m_matrix); }
402 
407 
432  // note: this function is defined here because some compilers cannot find the respective declaration
433  template<typename OtherDerived>
437 
445  template<typename OtherDerived> friend
449 
456  template<typename DiagonalDerived>
457  EIGEN_DEVICE_FUNC inline const TransformTimeDiagonalReturnType
459  {
460  TransformTimeDiagonalReturnType res(*this);
461  res.linearExt() *= b;
462  return res;
463  }
464 
471  template<typename DiagonalDerived>
472  EIGEN_DEVICE_FUNC friend inline TransformTimeDiagonalReturnType
474  {
475  TransformTimeDiagonalReturnType res;
476  res.linear().noalias() = a*b.linear();
477  res.translation().noalias() = a*b.translation();
479  res.matrix().row(Dim) = b.matrix().row(Dim);
480  return res;
481  }
482 
483  template<typename OtherDerived>
484  EIGEN_DEVICE_FUNC inline Transform& operator*=(const EigenBase<OtherDerived>& other) { return *this = *this * other; }
485 
487  EIGEN_DEVICE_FUNC inline const Transform operator * (const Transform& other) const
488  {
490  }
491 
492  #if EIGEN_COMP_ICC
493 private:
494  // this intermediate structure permits to workaround a bug in ICC 11:
495  // error: template instantiation resulted in unexpected function type of "Eigen::Transform<double, 3, 32, 0>
496  // (const Eigen::Transform<double, 3, 2, 0> &) const"
497  // (the meaning of a name may have changed since the template declaration -- the type of the template is:
498  // "Eigen::internal::transform_transform_product_impl<Eigen::Transform<double, 3, 32, 0>,
499  // Eigen::Transform<double, 3, Mode, Options>, <expression>>::ResultType (const Eigen::Transform<double, 3, Mode, Options> &) const")
500  //
501  template<int OtherMode,int OtherOptions> struct icc_11_workaround
502  {
504  typedef typename ProductType::ResultType ResultType;
505  };
506 
507 public:
509  template<int OtherMode,int OtherOptions>
510  inline typename icc_11_workaround<OtherMode,OtherOptions>::ResultType
512  {
513  typedef typename icc_11_workaround<OtherMode,OtherOptions>::ProductType ProductType;
514  return ProductType::run(*this,other);
515  }
516  #else
517 
518  template<int OtherMode,int OtherOptions>
521  {
523  }
524  #endif
525 
527  EIGEN_DEVICE_FUNC void setIdentity() { m_matrix.setIdentity(); }
528 
534  {
535  return Transform(MatrixType::Identity());
536  }
537 
538  template<typename OtherDerived>
540  inline Transform& scale(const MatrixBase<OtherDerived> &other);
541 
542  template<typename OtherDerived>
544  inline Transform& prescale(const MatrixBase<OtherDerived> &other);
545 
546  EIGEN_DEVICE_FUNC inline Transform& scale(const Scalar& s);
547  EIGEN_DEVICE_FUNC inline Transform& prescale(const Scalar& s);
548 
549  template<typename OtherDerived>
551  inline Transform& translate(const MatrixBase<OtherDerived> &other);
552 
553  template<typename OtherDerived>
555  inline Transform& pretranslate(const MatrixBase<OtherDerived> &other);
556 
557  template<typename RotationType>
559  inline Transform& rotate(const RotationType& rotation);
560 
561  template<typename RotationType>
563  inline Transform& prerotate(const RotationType& rotation);
564 
565  EIGEN_DEVICE_FUNC Transform& shear(const Scalar& sx, const Scalar& sy);
566  EIGEN_DEVICE_FUNC Transform& preshear(const Scalar& sx, const Scalar& sy);
567 
568  EIGEN_DEVICE_FUNC inline Transform& operator=(const TranslationType& t);
569 
571  inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); }
572 
573  EIGEN_DEVICE_FUNC inline Transform operator*(const TranslationType& t) const;
574 
576  inline Transform& operator=(const UniformScaling<Scalar>& t);
577 
579  inline Transform& operator*=(const UniformScaling<Scalar>& s) { return scale(s.factor()); }
580 
582  inline TransformTimeDiagonalReturnType operator*(const UniformScaling<Scalar>& s) const
583  {
584  TransformTimeDiagonalReturnType res = *this;
585  res.scale(s.factor());
586  return res;
587  }
588 
590  inline Transform& operator*=(const DiagonalMatrix<Scalar,Dim>& s) { linearExt() *= s; return *this; }
591 
592  template<typename Derived>
593  EIGEN_DEVICE_FUNC inline Transform& operator=(const RotationBase<Derived,Dim>& r);
594  template<typename Derived>
596  template<typename Derived>
598 
600  EIGEN_DEVICE_FUNC RotationReturnType rotation() const;
601 
602  template<typename RotationMatrixType, typename ScalingMatrixType>
604  void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const;
605  template<typename ScalingMatrixType, typename RotationMatrixType>
607  void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const;
608 
609  template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
611  Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
612  const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale);
613 
615  inline Transform inverse(TransformTraits traits = (TransformTraits)Mode) const;
616 
618  EIGEN_DEVICE_FUNC const Scalar* data() const { return m_matrix.data(); }
620  EIGEN_DEVICE_FUNC Scalar* data() { return m_matrix.data(); }
621 
627  template<typename NewScalarType>
630 
632  template<typename OtherScalarType>
634  {
635  check_template_params();
636  m_matrix = other.matrix().template cast<Scalar>();
637  }
638 
644  { return m_matrix.isApprox(other.m_matrix, prec); }
645 
649  {
651  }
652 
658  { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,Dim>(0,0); }
664  { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,Dim>(0,0); }
665 
671  { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,1>(0,Dim); }
677  { return m_matrix.template block<int(Mode)==int(Projective)?HDim:Dim,1>(0,Dim); }
678 
679 
680  #ifdef EIGEN_TRANSFORM_PLUGIN
681  #include EIGEN_TRANSFORM_PLUGIN
682  #endif
683 
684 protected:
685  #ifndef EIGEN_PARSED_BY_DOXYGEN
687  {
688  EIGEN_STATIC_ASSERT((Options & (DontAlign|RowMajor)) == Options, INVALID_MATRIX_TEMPLATE_PARAMETERS)
689  }
690  #endif
691 
692 };
693 
702 
711 
720 
729 
730 /**************************
731 *** Optional QT support ***
732 **************************/
733 
734 #ifdef EIGEN_QT_SUPPORT
735 
739 template<typename Scalar, int Dim, int Mode,int Options>
741 {
742  check_template_params();
743  *this = other;
744 }
745 
750 template<typename Scalar, int Dim, int Mode,int Options>
752 {
753  EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
755  m_matrix << other.m11(), other.m21(), other.dx(),
756  other.m12(), other.m22(), other.dy();
757  else
758  m_matrix << other.m11(), other.m21(), other.dx(),
759  other.m12(), other.m22(), other.dy(),
760  0, 0, 1;
761  return *this;
762 }
763 
770 template<typename Scalar, int Dim, int Mode, int Options>
772 {
773  check_template_params();
774  EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
775  return QMatrix(m_matrix.coeff(0,0), m_matrix.coeff(1,0),
776  m_matrix.coeff(0,1), m_matrix.coeff(1,1),
777  m_matrix.coeff(0,2), m_matrix.coeff(1,2));
778 }
779 
784 template<typename Scalar, int Dim, int Mode,int Options>
785 Transform<Scalar,Dim,Mode,Options>::Transform(const QTransform& other)
786 {
787  check_template_params();
788  *this = other;
789 }
790 
795 template<typename Scalar, int Dim, int Mode, int Options>
797 {
798  check_template_params();
799  EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
801  m_matrix << other.m11(), other.m21(), other.dx(),
802  other.m12(), other.m22(), other.dy();
803  else
804  m_matrix << other.m11(), other.m21(), other.dx(),
805  other.m12(), other.m22(), other.dy(),
806  other.m13(), other.m23(), other.m33();
807  return *this;
808 }
809 
814 template<typename Scalar, int Dim, int Mode, int Options>
816 {
817  EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
819  return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0),
820  m_matrix.coeff(0,1), m_matrix.coeff(1,1),
821  m_matrix.coeff(0,2), m_matrix.coeff(1,2));
822  else
823  return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0), m_matrix.coeff(2,0),
824  m_matrix.coeff(0,1), m_matrix.coeff(1,1), m_matrix.coeff(2,1),
825  m_matrix.coeff(0,2), m_matrix.coeff(1,2), m_matrix.coeff(2,2));
826 }
827 #endif
828 
829 /*********************
830 *** Procedural API ***
831 *********************/
832 
837 template<typename Scalar, int Dim, int Mode, int Options>
838 template<typename OtherDerived>
841 {
843  EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
844  linearExt().noalias() = (linearExt() * other.asDiagonal());
845  return *this;
846 }
847 
852 template<typename Scalar, int Dim, int Mode, int Options>
854 {
855  EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
856  linearExt() *= s;
857  return *this;
858 }
859 
864 template<typename Scalar, int Dim, int Mode, int Options>
865 template<typename OtherDerived>
868 {
870  EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
871  affine().noalias() = (other.asDiagonal() * affine());
872  return *this;
873 }
874 
879 template<typename Scalar, int Dim, int Mode, int Options>
881 {
882  EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
883  m_matrix.template topRows<Dim>() *= s;
884  return *this;
885 }
886 
891 template<typename Scalar, int Dim, int Mode, int Options>
892 template<typename OtherDerived>
895 {
897  translationExt() += linearExt() * other;
898  return *this;
899 }
900 
905 template<typename Scalar, int Dim, int Mode, int Options>
906 template<typename OtherDerived>
909 {
911  if(EIGEN_CONST_CONDITIONAL(int(Mode)==int(Projective)))
912  affine() += other * m_matrix.row(Dim);
913  else
914  translation() += other;
915  return *this;
916 }
917 
935 template<typename Scalar, int Dim, int Mode, int Options>
936 template<typename RotationType>
938 Transform<Scalar,Dim,Mode,Options>::rotate(const RotationType& rotation)
939 {
940  linearExt() *= internal::toRotationMatrix<Scalar,Dim>(rotation);
941  return *this;
942 }
943 
951 template<typename Scalar, int Dim, int Mode, int Options>
952 template<typename RotationType>
955 {
956  m_matrix.template block<Dim,HDim>(0,0) = internal::toRotationMatrix<Scalar,Dim>(rotation)
957  * m_matrix.template block<Dim,HDim>(0,0);
958  return *this;
959 }
960 
966 template<typename Scalar, int Dim, int Mode, int Options>
968 Transform<Scalar,Dim,Mode,Options>::shear(const Scalar& sx, const Scalar& sy)
969 {
970  EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
971  EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
972  VectorType tmp = linear().col(0)*sy + linear().col(1);
973  linear() << linear().col(0) + linear().col(1)*sx, tmp;
974  return *this;
975 }
976 
982 template<typename Scalar, int Dim, int Mode, int Options>
984 Transform<Scalar,Dim,Mode,Options>::preshear(const Scalar& sx, const Scalar& sy)
985 {
986  EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
987  EIGEN_STATIC_ASSERT(Mode!=int(Isometry), THIS_METHOD_IS_ONLY_FOR_SPECIFIC_TRANSFORMATIONS)
988  m_matrix.template block<Dim,HDim>(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0);
989  return *this;
990 }
991 
992 /******************************************************
993 *** Scaling, Translation and Rotation compatibility ***
994 ******************************************************/
995 
996 template<typename Scalar, int Dim, int Mode, int Options>
998 {
999  linear().setIdentity();
1000  translation() = t.vector();
1001  makeAffine();
1002  return *this;
1003 }
1004 
1005 template<typename Scalar, int Dim, int Mode, int Options>
1007 {
1008  Transform res = *this;
1009  res.translate(t.vector());
1010  return res;
1011 }
1012 
1013 template<typename Scalar, int Dim, int Mode, int Options>
1015 {
1016  m_matrix.setZero();
1017  linear().diagonal().fill(s.factor());
1018  makeAffine();
1019  return *this;
1020 }
1021 
1022 template<typename Scalar, int Dim, int Mode, int Options>
1023 template<typename Derived>
1025 {
1026  linear() = internal::toRotationMatrix<Scalar,Dim>(r);
1027  translation().setZero();
1028  makeAffine();
1029  return *this;
1030 }
1031 
1032 template<typename Scalar, int Dim, int Mode, int Options>
1033 template<typename Derived>
1035 {
1036  Transform res = *this;
1037  res.rotate(r.derived());
1038  return res;
1039 }
1040 
1041 /************************
1042 *** Special functions ***
1043 ************************/
1044 
1045 namespace internal {
1046 template<int Mode> struct transform_rotation_impl {
1047  template<typename TransformType>
1048  EIGEN_DEVICE_FUNC static inline
1049  const typename TransformType::LinearMatrixType run(const TransformType& t)
1050  {
1051  typedef typename TransformType::LinearMatrixType LinearMatrixType;
1052  LinearMatrixType result;
1053  t.computeRotationScaling(&result, (LinearMatrixType*)0);
1054  return result;
1055  }
1056 };
1057 template<> struct transform_rotation_impl<Isometry> {
1058  template<typename TransformType>
1059  EIGEN_DEVICE_FUNC static inline
1060  typename TransformType::ConstLinearPart run(const TransformType& t)
1061  {
1062  return t.linear();
1063  }
1064 };
1065 }
1076 template<typename Scalar, int Dim, int Mode, int Options>
1080 {
1082 }
1083 
1084 
1096 template<typename Scalar, int Dim, int Mode, int Options>
1097 template<typename RotationMatrixType, typename ScalingMatrixType>
1098 EIGEN_DEVICE_FUNC void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const
1099 {
1100  // Note that JacobiSVD is faster than BDCSVD for small matrices.
1102 
1103  Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant() < Scalar(0) ? Scalar(-1) : Scalar(1); // so x has absolute value 1
1104  VectorType sv(svd.singularValues());
1105  sv.coeffRef(Dim-1) *= x;
1106  if(scaling) *scaling = svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint();
1107  if(rotation)
1108  {
1109  LinearMatrixType m(svd.matrixU());
1110  m.col(Dim-1) *= x;
1111  *rotation = m * svd.matrixV().adjoint();
1112  }
1113 }
1114 
1126 template<typename Scalar, int Dim, int Mode, int Options>
1127 template<typename ScalingMatrixType, typename RotationMatrixType>
1128 EIGEN_DEVICE_FUNC void Transform<Scalar,Dim,Mode,Options>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const
1129 {
1130  // Note that JacobiSVD is faster than BDCSVD for small matrices.
1132 
1133  Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant() < Scalar(0) ? Scalar(-1) : Scalar(1); // so x has absolute value 1
1134  VectorType sv(svd.singularValues());
1135  sv.coeffRef(Dim-1) *= x;
1136  if(scaling) *scaling = svd.matrixU() * sv.asDiagonal() * svd.matrixU().adjoint();
1137  if(rotation)
1138  {
1139  LinearMatrixType m(svd.matrixU());
1140  m.col(Dim-1) *= x;
1141  *rotation = m * svd.matrixV().adjoint();
1142  }
1143 }
1144 
1148 template<typename Scalar, int Dim, int Mode, int Options>
1149 template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
1152  const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale)
1153 {
1154  linear() = internal::toRotationMatrix<Scalar,Dim>(orientation);
1155  linear() *= scale.asDiagonal();
1156  translation() = position;
1157  makeAffine();
1158  return *this;
1159 }
1160 
1161 namespace internal {
1162 
1163 template<int Mode>
1164 struct transform_make_affine
1165 {
1166  template<typename MatrixType>
1167  EIGEN_DEVICE_FUNC static void run(MatrixType &mat)
1168  {
1169  static const int Dim = MatrixType::ColsAtCompileTime-1;
1170  mat.template block<1,Dim>(Dim,0).setZero();
1171  mat.coeffRef(Dim,Dim) = typename MatrixType::Scalar(1);
1172  }
1173 };
1174 
1175 template<>
1177 {
1178  template<typename MatrixType> EIGEN_DEVICE_FUNC static void run(MatrixType &) { }
1179 };
1180 
1181 // selector needed to avoid taking the inverse of a 3x4 matrix
1182 template<typename TransformType, int Mode=TransformType::Mode>
1184 {
1185  EIGEN_DEVICE_FUNC static inline void run(const TransformType&, TransformType&)
1186  {}
1187 };
1188 
1189 template<typename TransformType>
1191 {
1192  EIGEN_DEVICE_FUNC static inline void run(const TransformType& m, TransformType& res)
1193  {
1194  res.matrix() = m.matrix().inverse();
1195  }
1196 };
1197 
1198 } // end namespace internal
1199 
1200 
1221 template<typename Scalar, int Dim, int Mode, int Options>
1224 {
1225  Transform res;
1226  if (hint == Projective)
1227  {
1229  }
1230  else
1231  {
1232  if (hint == Isometry)
1233  {
1234  res.matrix().template topLeftCorner<Dim,Dim>() = linear().transpose();
1235  }
1236  else if(hint&Affine)
1237  {
1238  res.matrix().template topLeftCorner<Dim,Dim>() = linear().inverse();
1239  }
1240  else
1241  {
1242  eigen_assert(false && "Invalid transform traits in Transform::Inverse");
1243  }
1244  // translation and remaining parts
1245  res.matrix().template topRightCorner<Dim,1>()
1246  = - res.matrix().template topLeftCorner<Dim,Dim>() * translation();
1247  res.makeAffine(); // we do need this, because in the beginning res is uninitialized
1248  }
1249  return res;
1250 }
1251 
1252 namespace internal {
1253 
1254 /*****************************************************
1255 *** Specializations of take affine part ***
1256 *****************************************************/
1257 
1258 template<typename TransformType> struct transform_take_affine_part {
1260  typedef typename TransformType::AffinePart AffinePart;
1261  typedef typename TransformType::ConstAffinePart ConstAffinePart;
1262  static inline AffinePart run(MatrixType& m)
1263  { return m.template block<TransformType::Dim,TransformType::HDim>(0,0); }
1264  static inline ConstAffinePart run(const MatrixType& m)
1265  { return m.template block<TransformType::Dim,TransformType::HDim>(0,0); }
1266 };
1267 
1268 template<typename Scalar, int Dim, int Options>
1271  static inline MatrixType& run(MatrixType& m) { return m; }
1272  static inline const MatrixType& run(const MatrixType& m) { return m; }
1273 };
1274 
1275 /*****************************************************
1276 *** Specializations of construct from matrix ***
1277 *****************************************************/
1278 
1279 template<typename Other, int Mode, int Options, int Dim, int HDim>
1281 {
1282  static inline void run(Transform<typename Other::Scalar,Dim,Mode,Options> *transform, const Other& other)
1283  {
1284  transform->linear() = other;
1285  transform->translation().setZero();
1286  transform->makeAffine();
1287  }
1288 };
1289 
1290 template<typename Other, int Mode, int Options, int Dim, int HDim>
1292 {
1293  static inline void run(Transform<typename Other::Scalar,Dim,Mode,Options> *transform, const Other& other)
1294  {
1295  transform->affine() = other;
1296  transform->makeAffine();
1297  }
1298 };
1299 
1300 template<typename Other, int Mode, int Options, int Dim, int HDim>
1302 {
1303  static inline void run(Transform<typename Other::Scalar,Dim,Mode,Options> *transform, const Other& other)
1304  { transform->matrix() = other; }
1305 };
1306 
1307 template<typename Other, int Options, int Dim, int HDim>
1309 {
1311  { transform->matrix() = other.template block<Dim,HDim>(0,0); }
1312 };
1313 
1314 /**********************************************************
1315 *** Specializations of operator* with rhs EigenBase ***
1316 **********************************************************/
1317 
1318 template<int LhsMode,int RhsMode>
1320 {
1321  enum
1322  {
1324  (LhsMode == (int)Projective || RhsMode == (int)Projective ) ? Projective :
1325  (LhsMode == (int)Affine || RhsMode == (int)Affine ) ? Affine :
1326  (LhsMode == (int)AffineCompact || RhsMode == (int)AffineCompact ) ? AffineCompact :
1327  (LhsMode == (int)Isometry || RhsMode == (int)Isometry ) ? Isometry : Projective
1328  };
1329 };
1330 
1331 template< typename TransformType, typename MatrixType, int RhsCols>
1332 struct transform_right_product_impl< TransformType, MatrixType, 0, RhsCols>
1333 {
1334  typedef typename MatrixType::PlainObject ResultType;
1335 
1336  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other)
1337  {
1338  return T.matrix() * other;
1339  }
1340 };
1341 
1342 template< typename TransformType, typename MatrixType, int RhsCols>
1343 struct transform_right_product_impl< TransformType, MatrixType, 1, RhsCols>
1344 {
1345  enum {
1346  Dim = TransformType::Dim,
1347  HDim = TransformType::HDim,
1348  OtherRows = MatrixType::RowsAtCompileTime,
1349  OtherCols = MatrixType::ColsAtCompileTime
1350  };
1351 
1352  typedef typename MatrixType::PlainObject ResultType;
1353 
1354  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other)
1355  {
1356  EIGEN_STATIC_ASSERT(OtherRows==HDim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
1357 
1359 
1360  ResultType res(other.rows(),other.cols());
1361  TopLeftLhs(res, 0, 0, Dim, other.cols()).noalias() = T.affine() * other;
1362  res.row(OtherRows-1) = other.row(OtherRows-1);
1363 
1364  return res;
1365  }
1366 };
1367 
1368 template< typename TransformType, typename MatrixType, int RhsCols>
1369 struct transform_right_product_impl< TransformType, MatrixType, 2, RhsCols>
1370 {
1371  enum {
1372  Dim = TransformType::Dim,
1373  HDim = TransformType::HDim,
1374  OtherRows = MatrixType::RowsAtCompileTime,
1375  OtherCols = MatrixType::ColsAtCompileTime
1376  };
1377 
1378  typedef typename MatrixType::PlainObject ResultType;
1379 
1380  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other)
1381  {
1382  EIGEN_STATIC_ASSERT(OtherRows==Dim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
1383 
1384  typedef Block<ResultType, Dim, OtherCols, true> TopLeftLhs;
1385  ResultType res(Replicate<typename TransformType::ConstTranslationPart, 1, OtherCols>(T.translation(),1,other.cols()));
1386  TopLeftLhs(res, 0, 0, Dim, other.cols()).noalias() += T.linear() * other;
1387 
1388  return res;
1389  }
1390 };
1391 
1392 template< typename TransformType, typename MatrixType >
1393 struct transform_right_product_impl< TransformType, MatrixType, 2, 1> // rhs is a vector of size Dim
1394 {
1396  enum {
1397  Dim = TransformType::Dim,
1398  HDim = TransformType::HDim,
1399  OtherRows = MatrixType::RowsAtCompileTime,
1400  WorkingRows = EIGEN_PLAIN_ENUM_MIN(TransformMatrix::RowsAtCompileTime,HDim)
1401  };
1402 
1403  typedef typename MatrixType::PlainObject ResultType;
1404 
1405  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType& T, const MatrixType& other)
1406  {
1407  EIGEN_STATIC_ASSERT(OtherRows==Dim, YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES);
1408 
1410  rhs.template head<Dim>() = other; rhs[Dim] = typename ResultType::Scalar(1);
1412  return res.template head<Dim>();
1413  }
1414 };
1415 
1416 /**********************************************************
1417 *** Specializations of operator* with lhs EigenBase ***
1418 **********************************************************/
1419 
1420 // generic HDim x HDim matrix * T => Projective
1421 template<typename Other,int Mode, int Options, int Dim, int HDim>
1423 {
1427  static ResultType run(const Other& other,const TransformType& tr)
1428  { return ResultType(other * tr.matrix()); }
1429 };
1430 
1431 // generic HDim x HDim matrix * AffineCompact => Projective
1432 template<typename Other, int Options, int Dim, int HDim>
1434 {
1438  static ResultType run(const Other& other,const TransformType& tr)
1439  {
1440  ResultType res;
1441  res.matrix().noalias() = other.template block<HDim,Dim>(0,0) * tr.matrix();
1442  res.matrix().col(Dim) += other.col(Dim);
1443  return res;
1444  }
1445 };
1446 
1447 // affine matrix * T
1448 template<typename Other,int Mode, int Options, int Dim, int HDim>
1450 {
1453  typedef TransformType ResultType;
1454  static ResultType run(const Other& other,const TransformType& tr)
1455  {
1456  ResultType res;
1457  res.affine().noalias() = other * tr.matrix();
1458  res.matrix().row(Dim) = tr.matrix().row(Dim);
1459  return res;
1460  }
1461 };
1462 
1463 // affine matrix * AffineCompact
1464 template<typename Other, int Options, int Dim, int HDim>
1466 {
1469  typedef TransformType ResultType;
1470  static ResultType run(const Other& other,const TransformType& tr)
1471  {
1472  ResultType res;
1473  res.matrix().noalias() = other.template block<Dim,Dim>(0,0) * tr.matrix();
1474  res.translation() += other.col(Dim);
1475  return res;
1476  }
1477 };
1478 
1479 // linear matrix * T
1480 template<typename Other,int Mode, int Options, int Dim, int HDim>
1482 {
1485  typedef TransformType ResultType;
1486  static ResultType run(const Other& other, const TransformType& tr)
1487  {
1488  TransformType res;
1489  if(Mode!=int(AffineCompact))
1490  res.matrix().row(Dim) = tr.matrix().row(Dim);
1491  res.matrix().template topRows<Dim>().noalias()
1492  = other * tr.matrix().template topRows<Dim>();
1493  return res;
1494  }
1495 };
1496 
1497 /**********************************************************
1498 *** Specializations of operator* with another Transform ***
1499 **********************************************************/
1500 
1501 template<typename Scalar, int Dim, int LhsMode, int LhsOptions, int RhsMode, int RhsOptions>
1502 struct transform_transform_product_impl<Transform<Scalar,Dim,LhsMode,LhsOptions>,Transform<Scalar,Dim,RhsMode,RhsOptions>,false >
1503 {
1508  static ResultType run(const Lhs& lhs, const Rhs& rhs)
1509  {
1510  ResultType res;
1511  res.linear() = lhs.linear() * rhs.linear();
1512  res.translation() = lhs.linear() * rhs.translation() + lhs.translation();
1513  res.makeAffine();
1514  return res;
1515  }
1516 };
1517 
1518 template<typename Scalar, int Dim, int LhsMode, int LhsOptions, int RhsMode, int RhsOptions>
1519 struct transform_transform_product_impl<Transform<Scalar,Dim,LhsMode,LhsOptions>,Transform<Scalar,Dim,RhsMode,RhsOptions>,true >
1520 {
1524  static ResultType run(const Lhs& lhs, const Rhs& rhs)
1525  {
1526  return ResultType( lhs.matrix() * rhs.matrix() );
1527  }
1528 };
1529 
1530 template<typename Scalar, int Dim, int LhsOptions, int RhsOptions>
1531 struct transform_transform_product_impl<Transform<Scalar,Dim,AffineCompact,LhsOptions>,Transform<Scalar,Dim,Projective,RhsOptions>,true >
1532 {
1536  static ResultType run(const Lhs& lhs, const Rhs& rhs)
1537  {
1538  ResultType res;
1539  res.matrix().template topRows<Dim>() = lhs.matrix() * rhs.matrix();
1540  res.matrix().row(Dim) = rhs.matrix().row(Dim);
1541  return res;
1542  }
1543 };
1544 
1545 template<typename Scalar, int Dim, int LhsOptions, int RhsOptions>
1546 struct transform_transform_product_impl<Transform<Scalar,Dim,Projective,LhsOptions>,Transform<Scalar,Dim,AffineCompact,RhsOptions>,true >
1547 {
1551  static ResultType run(const Lhs& lhs, const Rhs& rhs)
1552  {
1553  ResultType res(lhs.matrix().template leftCols<Dim>() * rhs.matrix());
1554  res.matrix().col(Dim) += lhs.matrix().col(Dim);
1555  return res;
1556  }
1557 };
1558 
1559 } // end namespace internal
1560 
1561 } // end namespace Eigen
1562 
1563 #endif // EIGEN_TRANSFORM_H
static EIGEN_DEVICE_FUNC const Transform Identity()
Returns an identity transformation.
Definition: Transform.h:533
Matrix3f m
Transform< typename Other::Scalar, Dim, Mode, Options > TransformType
Definition: Transform.h:1483
EIGEN_DEVICE_FUNC Transform & operator*=(const DiagonalMatrix< Scalar, Dim > &s)
Definition: Transform.h:590
SCALAR Scalar
Definition: bench_gemm.cpp:46
EIGEN_DEVICE_FUNC Transform & preshear(const Scalar &sx, const Scalar &sy)
Definition: Transform.h:984
TransformType::AffinePart AffinePart
Definition: Transform.h:1260
const MatrixType ConstMatrixType
Definition: Transform.h:222
cout<< "Here is the matrix m:"<< endl<< m<< endl;JacobiSVD< MatrixXf > svd(m, ComputeThinU|ComputeThinV)
#define EIGEN_STRONG_INLINE
Definition: Macros.h:917
Point3 translation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
static EIGEN_DEVICE_FUNC const TransformType::LinearMatrixType run(const TransformType &t)
Definition: Transform.h:1049
Transform< float, 3, Affine > Affine3f
Definition: Transform.h:706
Transform< double, 2, AffineCompact > AffineCompact2d
Definition: Transform.h:717
Scalar * b
Definition: benchVecAdd.cpp:17
EIGEN_DEVICE_FUNC const Scalar * data() const
Definition: Transform.h:618
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
Definition: Transform.h:379
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
TransformType::MatrixType MatrixType
Definition: Transform.h:1259
static ResultType run(const Other &other, const TransformType &tr)
Definition: Transform.h:1427
static void run(Transform< typename Other::Scalar, Dim, Mode, Options > *transform, const Other &other)
Definition: Transform.h:1282
void determinant(const MatrixType &m)
Definition: determinant.cpp:14
EIGEN_DEVICE_FUNC Transform(const UniformScaling< Scalar > &s)
Definition: Transform.h:270
Eigen::Index Index
Definition: Transform.h:218
_Scalar Scalar
Definition: Transform.h:214
EIGEN_DEVICE_FUNC Transform inverse(TransformTraits traits=(TransformTraits) Mode) const
Definition: Transform.h:1223
static EIGEN_DEVICE_FUNC void run(const TransformType &m, TransformType &res)
Definition: Transform.h:1192
EIGEN_DEVICE_FUNC Transform(const Transform< Scalar, Dim, Mode, OtherOptions > &other)
Definition: Transform.h:307
Represents a diagonal matrix with its storage.
Transform< typename Other::Scalar, Dim, AffineCompact, Options > TransformType
Definition: Transform.h:1435
EIGEN_DEVICE_FUNC Transform & operator*=(const EigenBase< OtherDerived > &other)
Definition: Transform.h:484
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
MatrixXf MatrixType
EIGEN_DEVICE_FUNC MatrixType & matrix()
Definition: Transform.h:391
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:232
Transform< typename Other::Scalar, Dim, AffineCompact, Options > TransformType
Definition: Transform.h:1467
internal::transform_take_affine_part< Transform > take_affine_part
Definition: Transform.h:282
#define EIGEN_STATIC_ASSERT(CONDITION, MSG)
Definition: StaticAssert.h:127
EIGEN_DEVICE_FUNC Transform & operator=(const ReturnByValue< OtherDerived > &other)
Definition: Transform.h:363
EIGEN_DEVICE_FUNC void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const
Definition: Transform.h:1128
internal::conditional< int(Mode)==Isometry, ConstLinearPart, const LinearMatrixType >::type RotationReturnType
Definition: Transform.h:599
EIGEN_DEVICE_FUNC LinearPart linear()
Definition: Transform.h:396
const Scalar & factor() const
EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
Definition: Transform.h:378
EIGEN_DEVICE_FUNC Transform & prerotate(const RotationType &rotation)
EIGEN_DEVICE_FUNC const Block< MatrixType, int(Mode)==int(Projective)?HDim:Dim, Dim > linearExt() const
Definition: Transform.h:663
Transform< Scalar, Dim, AffineCompact, Options >::MatrixType MatrixType
Definition: Transform.h:1270
const unsigned int RowMajorBit
Definition: Constants.h:66
EIGEN_DEVICE_FUNC Transform()
Definition: Transform.h:259
#define EIGEN_IMPLIES(a, b)
Definition: Macros.h:1315
EIGEN_DEVICE_FUNC Transform & scale(const MatrixBase< OtherDerived > &other)
EIGEN_DEVICE_FUNC RotationReturnType rotation() const
Definition: Transform.h:1079
EIGEN_DEVICE_FUNC Transform & operator*=(const TranslationType &t)
Definition: Transform.h:571
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
Matrix< Scalar, Dim, Dim, Options > LinearMatrixType
Definition: Transform.h:224
EIGEN_DEVICE_FUNC Transform & rotate(const RotationType &rotation)
Matrix< Scalar, Dim, 1 > VectorType
Definition: Transform.h:238
static void run(Transform< typename Other::Scalar, Dim, Mode, Options > *transform, const Other &other)
Definition: Transform.h:1293
EIGEN_DEVICE_FUNC Transform(const EigenBase< OtherDerived > &other)
Definition: Transform.h:286
EIGEN_DEVICE_FUNC internal::cast_return_type< Transform, Transform< NewScalarType, Dim, Mode, Options > >::type cast() const
Definition: Transform.h:628
EIGEN_DEVICE_FUNC Transform & operator*=(const RotationBase< Derived, Dim > &r)
Definition: Transform.h:595
static EIGEN_DEVICE_FUNC void run(MatrixType &)
Definition: Transform.h:1178
#define EIGEN_CONST_CONDITIONAL(cond)
Definition: Macros.h:1153
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void check_template_params()
Definition: Transform.h:686
Represents a translation transformation.
Transform< double, 2, Projective > Projective2d
Definition: Transform.h:726
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
EIGEN_DEVICE_FUNC const MatrixType & matrix() const
Definition: Transform.h:389
Transform< float, 2, AffineCompact > AffineCompact2f
Definition: Transform.h:713
static ResultType run(const Other &other, const TransformType &tr)
Definition: Transform.h:1454
internal::conditional< int(Mode)==int(AffineCompact), MatrixType &, Block< MatrixType, Dim, HDim > >::type AffinePart
Definition: Transform.h:232
EIGEN_DEVICE_FUNC Transform & shear(const Scalar &sx, const Scalar &sy)
Definition: Transform.h:968
EIGEN_DEVICE_FUNC Transform & translate(const MatrixBase< OtherDerived > &other)
EIGEN_DEVICE_FUNC Transform & operator*=(const UniformScaling< Scalar > &s)
Definition: Transform.h:579
Values result
EIGEN_DEVICE_FUNC ConstTranslationPart translation() const
Definition: Transform.h:404
Transform< typename Other::Scalar, Dim, Projective, Options > ResultType
Definition: Transform.h:1426
EIGEN_DEVICE_FUNC const VectorType & vector() const
Definition: Translation.h:87
EIGEN_DEVICE_FUNC Transform & pretranslate(const MatrixBase< OtherDerived > &other)
TransformTraits
Definition: Constants.h:455
m row(1)
EIGEN_DEVICE_FUNC ConstLinearPart linear() const
Definition: Transform.h:394
EIGEN_DEVICE_FUNC const Derived & derived() const
Definition: RotationBase.h:41
Transform< double, 3, Affine > Affine3d
Definition: Transform.h:710
#define EIGEN_NOEXCEPT
Definition: Macros.h:1418
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar & coeff(Index rowId, Index colId) const
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
#define eigen_assert(x)
Definition: Macros.h:1037
P rotate(const T &r, const P &pt)
Definition: lieProxies.h:47
EIGEN_DEVICE_FUNC Transform(const TranslationType &t)
Definition: Transform.h:265
EIGEN_DEVICE_FUNC Transform & prescale(const MatrixBase< OtherDerived > &other)
Eigen::Index StorageIndex
Definition: Transform.h:217
Expression of the multiple replication of a matrix or vector.
Definition: Replicate.h:61
Represents a generic uniform scaling transformation.
EIGEN_DEVICE_FUNC void setIdentity()
Definition: Transform.h:527
EIGEN_DEVICE_FUNC AffinePart affine()
Definition: Transform.h:401
Common base class for compact rotation representations.
RealScalar s
Transform< typename Other::Scalar, Dim, Mode, Options > TransformType
Definition: Transform.h:1451
EIGEN_DEVICE_FUNC Transform & operator=(const EigenBase< OtherDerived > &other)
Definition: Transform.h:297
#define EIGEN_CONSTEXPR
Definition: Macros.h:787
EIGEN_DEVICE_FUNC void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const
Definition: Transform.h:1098
Transform< double, 3, Isometry > Isometry3d
Definition: Transform.h:701
EIGEN_DEVICE_FUNC void evalTo(Dest &dst) const
Definition: ReturnByValue.h:61
MatrixType m_matrix
Definition: Transform.h:253
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
static ResultType run(const Other &other, const TransformType &tr)
Definition: Transform.h:1486
EIGEN_DEVICE_FUNC Block< MatrixType, int(Mode)==int(Projective)?HDim:Dim, 1 > translationExt()
Definition: Transform.h:670
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorUInt128< uint64_t, uint64_t > operator*(const TensorUInt128< HL, LL > &lhs, const TensorUInt128< HR, LR > &rhs)
static AffinePart run(MatrixType &m)
Definition: Transform.h:1262
Transform< float, 2, Projective > Projective2f
Definition: Transform.h:722
static EIGEN_DEVICE_FUNC void run(MatrixType &mat)
Definition: Transform.h:1167
EIGEN_DEVICE_FUNC RotationMatrixType toRotationMatrix() const
Definition: RotationBase.h:45
static EIGEN_DEVICE_FUNC void run(const TransformType &, TransformType &)
Definition: Transform.h:1185
EIGEN_DEVICE_FUNC TransformTimeDiagonalReturnType operator*(const UniformScaling< Scalar > &s) const
Definition: Transform.h:582
EIGEN_DEVICE_FUNC const DiagonalWrapper< const Derived > asDiagonal() const
EIGEN_DEVICE_FUNC Transform & fromPositionOrientationScale(const MatrixBase< PositionDerived > &position, const OrientationType &orientation, const MatrixBase< ScaleDerived > &scale)
Transform< float, 3, Projective > Projective3f
Definition: Transform.h:724
static void run(Transform< typename Other::Scalar, Dim, Mode, Options > *transform, const Other &other)
Definition: Transform.h:1303
#define EIGEN_DEVICE_FUNC
Definition: Macros.h:976
Transform< Scalar, Dim, TransformTimeDiagonalMode > TransformTimeDiagonalReturnType
Definition: Transform.h:249
Transform< float, 3, AffineCompact > AffineCompact3f
Definition: Transform.h:715
EIGEN_DEVICE_FUNC Transform(const Transform< OtherScalarType, Dim, Mode, Options > &other)
Definition: Transform.h:633
Transform< double, 3, AffineCompact > AffineCompact3d
Definition: Transform.h:719
internal::make_proper_matrix_type< Scalar, Rows, HDim, Options >::type MatrixType
Definition: Transform.h:220
EIGEN_DEVICE_FUNC bool isApprox(const Transform &other, const typename NumTraits< Scalar >::Real &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: Transform.h:643
Transform< float, 2, Affine > Affine2f
Definition: Transform.h:704
EIGEN_DEVICE_FUNC Transform(const ReturnByValue< OtherDerived > &other)
Definition: Transform.h:356
TransformType::ConstAffinePart ConstAffinePart
Definition: Transform.h:1261
EIGEN_DEVICE_FUNC Scalar * data()
Definition: Transform.h:620
Transform< float, 3, Isometry > Isometry3f
Definition: Transform.h:697
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType &T, const MatrixType &other)
Definition: Transform.h:1380
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
Translation< Scalar, Dim > TranslationType
Definition: Transform.h:244
#define EIGEN_PLAIN_ENUM_MIN(a, b)
Definition: Macros.h:1288
Transform< double, 2, Affine > Affine2d
Definition: Transform.h:708
static void run(Transform< typename Other::Scalar, Dim, AffineCompact, Options > *transform, const Other &other)
Definition: Transform.h:1310
Transform< double, 2, Isometry > Isometry2d
Definition: Transform.h:699
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType &T, const MatrixType &other)
Definition: Transform.h:1405
static EIGEN_DEVICE_FUNC TransformType::ConstLinearPart run(const TransformType &t)
Definition: Transform.h:1060
Two-sided Jacobi SVD decomposition of a rectangular matrix.
const SingularValuesType & singularValues() const
Definition: SVDBase.h:129
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType &T, const MatrixType &other)
Definition: Transform.h:1336
m col(1)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
const Block< ConstMatrixType, Dim, Dim, int(Mode)==(AffineCompact) &&(int(Options)&RowMajor)==0 > ConstLinearPart
Definition: Transform.h:228
static ConstAffinePart run(const MatrixType &m)
Definition: Transform.h:1264
const int Dynamic
Definition: Constants.h:22
EIGEN_DEVICE_FUNC ConstAffinePart affine() const
Definition: Transform.h:399
EIGEN_DEVICE_FUNC Transform(const RotationBase< Derived, Dim > &r)
Definition: Transform.h:276
Generic expression where a coefficient-wise unary operator is applied to an expression.
Definition: CwiseUnaryOp.h:55
The matrix class, also used for vectors and row-vectors.
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
internal::enable_if< internal::valid_indexed_view_overload< RowIndices, ColIndices >::value &&internal::traits< typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::ReturnAsIndexedView, typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::type operator()(const RowIndices &rowIndices, const ColIndices &colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
EIGEN_DEVICE_FUNC TranslationPart translation()
Definition: Transform.h:406
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
EIGEN_DEVICE_FUNC Derived & derived()
Definition: EigenBase.h:46
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Block< MatrixType, Dim, Dim, int(Mode)==(AffineCompact) &&(int(Options)&RowMajor)==0 > LinearPart
Definition: Transform.h:226
Represents an homogeneous transformation in a N dimensional space.
EIGEN_DEVICE_FUNC Transform(const Transform< Scalar, Dim, OtherMode, OtherOptions > &other)
Definition: Transform.h:315
Transform< float, 2, Isometry > Isometry2f
Definition: Transform.h:695
Point2 t(10, 10)
Definition: pytypes.h:1370
Transform< double, 3, Projective > Projective3d
Definition: Transform.h:728
#define EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(TYPE, SIZE)
Definition: StaticAssert.h:157
internal::conditional< int(Mode)==int(AffineCompact), const MatrixType &, const Block< const MatrixType, Dim, HDim > >::type ConstAffinePart
Definition: Transform.h:236
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType run(const TransformType &T, const MatrixType &other)
Definition: Transform.h:1354
EIGEN_DEVICE_FUNC Block< MatrixType, int(Mode)==int(Projective)?HDim:Dim, Dim > linearExt()
Definition: Transform.h:657
EIGEN_DEVICE_FUNC void makeAffine()
Definition: Transform.h:648
v setZero(3)
EIGEN_DEVICE_FUNC const Block< MatrixType, int(Mode)==int(Projective)?HDim:Dim, 1 > translationExt() const
Definition: Transform.h:676
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar, _Dim==Dynamic ? Dynamic :(_Dim+1) *(_Dim+1)) enum
Definition: Transform.h:207
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const internal::transform_right_product_impl< Transform, OtherDerived >::ResultType operator*(const EigenBase< OtherDerived > &other) const
Definition: Transform.h:435


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:22