12 #ifndef EIGEN_TRANSFORM_H 13 #define EIGEN_TRANSFORM_H 19 template<
typename Transform>
31 template<
typename TransformType,
36 int RhsCols = MatrixType::ColsAtCompileTime>
39 template<
typename Other,
44 int OtherRows=Other::RowsAtCompileTime,
45 int OtherCols=Other::ColsAtCompileTime>
48 template<
typename Lhs,
55 template<
typename Other,
60 int OtherRows=Other::RowsAtCompileTime,
61 int OtherCols=Other::ColsAtCompileTime>
66 template<
typename _Scalar,
int _Dim,
int _Mode,
int _Options>
75 ColsAtCompileTime = Dim1,
76 MaxRowsAtCompileTime = RowsAtCompileTime,
77 MaxColsAtCompileTime = ColsAtCompileTime,
203 template<
typename _Scalar,
int _Dim,
int _Mode,
int _Options>
261 check_template_params();
267 check_template_params();
272 check_template_params();
275 template<
typename Derived>
278 check_template_params();
285 template<
typename OtherDerived>
289 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY);
291 check_template_params();
296 template<
typename OtherDerived>
300 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY);
306 template<
int OtherOptions>
309 check_template_params();
311 m_matrix = other.
matrix();
314 template<
int OtherMode,
int OtherOptions>
317 check_template_params();
321 YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION)
326 YOU_PERFORMED_AN_INVALID_TRANSFORMATION_CONVERSION)
337 m_matrix.template block<Dim,Dim+1>(0,0) = other.
matrix().template block<Dim,Dim+1>(0,0);
350 linear() = other.
linear();
355 template<
typename OtherDerived>
358 check_template_params();
362 template<
typename OtherDerived>
369 #ifdef EIGEN_QT_SUPPORT 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;
433 template<
typename OtherDerived>
445 template<
typename OtherDerived>
friend 456 template<
typename DiagonalDerived>
460 TransformTimeDiagonalReturnType
res(*
this);
471 template<
typename DiagonalDerived>
475 TransformTimeDiagonalReturnType
res;
483 template<
typename OtherDerived>
501 template<
int OtherMode,
int OtherOptions>
struct icc_11_workaround
504 typedef typename ProductType::ResultType ResultType;
509 template<
int OtherMode,
int OtherOptions>
510 inline typename icc_11_workaround<OtherMode,OtherOptions>::ResultType
513 typedef typename icc_11_workaround<OtherMode,OtherOptions>::ProductType ProductType;
518 template<
int OtherMode,
int OtherOptions>
535 return Transform(MatrixType::Identity());
538 template<
typename OtherDerived>
542 template<
typename OtherDerived>
549 template<
typename OtherDerived>
553 template<
typename OtherDerived>
557 template<
typename RotationType>
561 template<
typename RotationType>
563 inline Transform& prerotate(
const RotationType& rotation);
584 TransformTimeDiagonalReturnType
res = *
this;
592 template<
typename Derived>
594 template<
typename Derived>
596 template<
typename Derived>
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;
609 template<
typename PositionDerived,
typename OrientationType,
typename ScaleDerived>
627 template<
typename NewScalarType>
632 template<
typename OtherScalarType>
635 check_template_params();
636 m_matrix = other.
matrix().template cast<Scalar>();
644 {
return m_matrix.isApprox(other.
m_matrix, prec); }
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); }
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); }
680 #ifdef EIGEN_TRANSFORM_PLUGIN 681 #include EIGEN_TRANSFORM_PLUGIN 685 #ifndef EIGEN_PARSED_BY_DOXYGEN 734 #ifdef EIGEN_QT_SUPPORT 739 template<
typename Scalar,
int Dim,
int Mode,
int Options>
742 check_template_params();
750 template<
typename Scalar,
int Dim,
int Mode,
int Options>
755 m_matrix << other.m11(), other.m21(), other.dx(),
756 other.m12(), other.m22(), other.dy();
758 m_matrix << other.m11(), other.m21(), other.dx(),
759 other.m12(), other.m22(), other.dy(),
770 template<
typename Scalar,
int Dim,
int Mode,
int Options>
773 check_template_params();
775 return QMatrix(m_matrix.
coeff(0,0), m_matrix.
coeff(1,0),
784 template<
typename Scalar,
int Dim,
int Mode,
int Options>
787 check_template_params();
795 template<
typename Scalar,
int Dim,
int Mode,
int Options>
798 check_template_params();
801 m_matrix << other.m11(), other.m21(), other.dx(),
802 other.m12(), other.m22(), other.dy();
804 m_matrix << other.m11(), other.m21(), other.dx(),
805 other.m12(), other.m22(), other.dy(),
806 other.m13(), other.m23(), other.m33();
814 template<
typename Scalar,
int Dim,
int Mode,
int Options>
819 return QTransform(m_matrix.
coeff(0,0), m_matrix.
coeff(1,0),
823 return QTransform(m_matrix.
coeff(0,0), m_matrix.
coeff(1,0), m_matrix.
coeff(2,0),
837 template<
typename Scalar,
int Dim,
int Mode,
int Options>
838 template<
typename OtherDerived>
844 linearExt().noalias() = (linearExt() * other.
asDiagonal());
852 template<
typename Scalar,
int Dim,
int Mode,
int Options>
864 template<
typename Scalar,
int Dim,
int Mode,
int Options>
865 template<
typename OtherDerived>
871 affine().noalias() = (other.
asDiagonal() * affine());
879 template<
typename Scalar,
int Dim,
int Mode,
int Options>
883 m_matrix.template topRows<Dim>() *= s;
891 template<
typename Scalar,
int Dim,
int Mode,
int Options>
892 template<
typename OtherDerived>
897 translationExt() += linearExt() *
other;
905 template<
typename Scalar,
int Dim,
int Mode,
int Options>
906 template<
typename OtherDerived>
912 affine() += other * m_matrix.row(
Dim);
935 template<
typename Scalar,
int Dim,
int Mode,
int Options>
936 template<
typename RotationType>
940 linearExt() *= internal::toRotationMatrix<Scalar,Dim>(
rotation);
951 template<
typename Scalar,
int Dim,
int Mode,
int Options>
952 template<
typename RotationType>
956 m_matrix.template block<Dim,HDim>(0,0) = internal::toRotationMatrix<Scalar,Dim>(rotation)
957 * m_matrix.template block<Dim,HDim>(0,0);
966 template<
typename Scalar,
int Dim,
int Mode,
int Options>
972 VectorType tmp = linear().col(0)*sy + linear().col(1);
973 linear() << linear().col(0) + linear().col(1)*sx, tmp;
982 template<
typename Scalar,
int Dim,
int Mode,
int Options>
988 m_matrix.template block<Dim,HDim>(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0);
996 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1005 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1013 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1017 linear().diagonal().fill(s.
factor());
1022 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1023 template<
typename Derived>
1026 linear() = internal::toRotationMatrix<Scalar,Dim>(r);
1032 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1033 template<
typename Derived>
1047 template<
typename TransformType>
1049 const typename TransformType::LinearMatrixType
run(
const TransformType& t)
1051 typedef typename TransformType::LinearMatrixType LinearMatrixType;
1053 t.computeRotationScaling(&result, (LinearMatrixType*)0);
1058 template<
typename TransformType>
1060 typename TransformType::ConstLinearPart
run(
const TransformType& t)
1076 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1096 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1097 template<
typename RotationMatrixType,
typename ScalingMatrixType>
1105 sv.coeffRef(
Dim-1) *=
x;
1106 if(scaling) *scaling = svd.
matrixV() * sv.asDiagonal() * svd.
matrixV().adjoint();
1111 *rotation =
m * svd.
matrixV().adjoint();
1126 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1127 template<
typename ScalingMatrixType,
typename RotationMatrixType>
1135 sv.coeffRef(
Dim-1) *=
x;
1136 if(scaling) *scaling = svd.
matrixU() * sv.asDiagonal() * svd.
matrixU().adjoint();
1141 *rotation =
m * svd.
matrixV().adjoint();
1148 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1149 template<
typename PositionDerived,
typename OrientationType,
typename ScaleDerived>
1154 linear() = internal::toRotationMatrix<Scalar,Dim>(orientation);
1164 struct transform_make_affine
1166 template<
typename MatrixType>
1169 static const int Dim = MatrixType::ColsAtCompileTime-1;
1182 template<
typename TransformType,
int Mode=TransformType::Mode>
1189 template<
typename TransformType>
1194 res.matrix() = m.matrix().inverse();
1221 template<
typename Scalar,
int Dim,
int Mode,
int Options>
1234 res.
matrix().template topLeftCorner<Dim,Dim>() = linear().transpose();
1238 res.
matrix().template topLeftCorner<Dim,Dim>() = linear().inverse();
1242 eigen_assert(
false &&
"Invalid transform traits in Transform::Inverse");
1245 res.
matrix().template topRightCorner<Dim,1>()
1258 template<
typename TransformType>
struct transform_take_affine_part {
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); }
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; }
1279 template<
typename Other,
int Mode,
int Options,
int Dim,
int HDim>
1290 template<
typename Other,
int Mode,
int Options,
int Dim,
int HDim>
1300 template<
typename Other,
int Mode,
int Options,
int Dim,
int HDim>
1307 template<
typename Other,
int Options,
int Dim,
int HDim>
1311 { transform->
matrix() = other.template block<Dim,HDim>(0,0); }
1318 template<
int LhsMode,
int RhsMode>
1331 template<
typename TransformType,
typename MatrixType,
int RhsCols>
1338 return T.matrix() *
other;
1342 template<
typename TransformType,
typename MatrixType,
int RhsCols>
1348 OtherRows = MatrixType::RowsAtCompileTime,
1349 OtherCols = MatrixType::ColsAtCompileTime
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);
1368 template<
typename TransformType,
typename MatrixType,
int RhsCols>
1374 OtherRows = MatrixType::RowsAtCompileTime,
1375 OtherCols = MatrixType::ColsAtCompileTime
1386 TopLeftLhs(res, 0, 0,
Dim, other.cols()).noalias() += T.linear() *
other;
1392 template<
typename TransformType,
typename MatrixType >
1399 OtherRows = MatrixType::RowsAtCompileTime,
1412 return res.template head<Dim>();
1421 template<
typename Other,
int Mode,
int Options,
int Dim,
int HDim>
1427 static ResultType
run(
const Other& other,
const TransformType& tr)
1428 {
return ResultType(other * tr.
matrix()); }
1432 template<
typename Other,
int Options,
int Dim,
int HDim>
1438 static ResultType
run(
const Other& other,
const TransformType& tr)
1441 res.
matrix().noalias() = other.template block<HDim,Dim>(0,0) * tr.
matrix();
1448 template<
typename Other,
int Mode,
int Options,
int Dim,
int HDim>
1454 static ResultType
run(
const Other& other,
const TransformType& tr)
1464 template<
typename Other,
int Options,
int Dim,
int HDim>
1470 static ResultType
run(
const Other& other,
const TransformType& tr)
1473 res.
matrix().noalias() = other.template block<Dim,Dim>(0,0) * tr.
matrix();
1480 template<
typename Other,
int Mode,
int Options,
int Dim,
int HDim>
1486 static ResultType
run(
const Other& other,
const TransformType& tr)
1491 res.
matrix().template topRows<Dim>().noalias()
1492 = other * tr.
matrix().template topRows<Dim>();
1501 template<
typename Scalar,
int Dim,
int LhsMode,
int LhsOptions,
int RhsMode,
int RhsOptions>
1508 static ResultType
run(
const Lhs& lhs,
const Rhs& rhs)
1518 template<
typename Scalar,
int Dim,
int LhsMode,
int LhsOptions,
int RhsMode,
int RhsOptions>
1524 static ResultType
run(
const Lhs& lhs,
const Rhs& rhs)
1530 template<
typename Scalar,
int Dim,
int LhsOptions,
int RhsOptions>
1536 static ResultType
run(
const Lhs& lhs,
const Rhs& rhs)
1545 template<
typename Scalar,
int Dim,
int LhsOptions,
int RhsOptions>
1551 static ResultType
run(
const Lhs& lhs,
const Rhs& rhs)
1563 #endif // EIGEN_TRANSFORM_H
cout<< "Here is the matrix m:"<< endl<< m<< endl;JacobiSVD< MatrixXf > svd(m, ComputeThinU|ComputeThinV)
#define EIGEN_STRONG_INLINE
Point3 translation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Transform< float, 3, Affine > Affine3f
Transform< double, 2, AffineCompact > AffineCompact2d
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
void determinant(const MatrixType &m)
const MatrixUType & matrixU() const
Represents a diagonal matrix with its storage.
Namespace containing all symbols from the Eigen library.
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
#define EIGEN_STATIC_ASSERT(CONDITION, MSG)
const unsigned int RowMajorBit
#define EIGEN_IMPLIES(a, b)
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
#define EIGEN_CONST_CONDITIONAL(cond)
Represents a translation transformation.
Transform< double, 2, Projective > Projective2d
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
Transform< float, 2, AffineCompact > AffineCompact2f
EIGEN_DEVICE_FUNC const VectorType & vector() const
EIGEN_DEVICE_FUNC const Derived & derived() const
Transform< double, 3, Affine > Affine3d
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.
P rotate(const T &r, const P &pt)
Expression of the multiple replication of a matrix or vector.
Common base class for compact rotation representations.
Transform< double, 3, Isometry > Isometry3d
EIGEN_DEVICE_FUNC void evalTo(Dest &dst) const
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE TensorUInt128< uint64_t, uint64_t > operator*(const TensorUInt128< HL, LL > &lhs, const TensorUInt128< HR, LR > &rhs)
Transform< float, 2, Projective > Projective2f
EIGEN_DEVICE_FUNC RotationMatrixType toRotationMatrix() const
EIGEN_DEVICE_FUNC const DiagonalWrapper< const Derived > asDiagonal() const
Transform< float, 3, Projective > Projective3f
#define EIGEN_DEVICE_FUNC
Transform< float, 3, AffineCompact > AffineCompact3f
Transform< double, 3, AffineCompact > AffineCompact3d
Transform< float, 2, Affine > Affine2f
Transform< float, 3, Isometry > Isometry3f
Expression of a fixed-size or dynamic-size block.
#define EIGEN_PLAIN_ENUM_MIN(a, b)
Transform< double, 2, Affine > Affine2d
Transform< double, 2, Isometry > Isometry2d
Two-sided Jacobi SVD decomposition of a rectangular matrix.
const SingularValuesType & singularValues() const
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 MatrixVType & matrixV() const
Generic expression where a coefficient-wise unary operator is applied to an expression.
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 EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
EIGEN_DEVICE_FUNC Derived & derived()
Base class for all dense matrices, vectors, and expressions.
Transform< float, 2, Isometry > Isometry2f
Transform< double, 3, Projective > Projective3d
#define EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(TYPE, SIZE)