Go to the documentation of this file.
10 #ifndef EIGEN_UMEYAMA_H
11 #define EIGEN_UMEYAMA_H
21 #ifndef EIGEN_PARSED_BY_DOXYGEN
31 template<
typename MatrixType,
typename OtherMatrixType>
93 template <
typename Derived,
typename OtherDerived>
103 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
111 const Index m = src.rows();
112 const Index n = src.cols();
122 const RowMajorMatrixType src_demean = src.
colwise() - src_mean;
123 const RowMajorMatrixType dst_demean = dst.
colwise() - dst_mean;
126 const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n;
129 const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose();
134 TransformationMatrixType Rt = TransformationMatrixType::Identity(
m+1,
m+1);
139 if (
svd.matrixU().determinant() *
svd.matrixV().determinant() < 0 )
143 Rt.block(0,0,
m,
m).noalias() =
svd.matrixU() *
S.asDiagonal() *
svd.matrixV().transpose();
151 Rt.col(
m).head(
m) = dst_mean;
152 Rt.col(
m).head(
m).noalias() -=
c*Rt.topLeftCorner(
m,
m)*src_mean;
153 Rt.block(0,0,
m,
m) *=
c;
157 Rt.col(
m).head(
m) = dst_mean;
158 Rt.col(
m).head(
m).noalias() -= Rt.topLeftCorner(
m,
m)*src_mean;
166 #endif // EIGEN_UMEYAMA_H
Namespace containing all symbols from the Eigen library.
const unsigned int RowMajorBit
cout<< "Here is the matrix m:"<< endl<< m<< endl;JacobiSVD< MatrixXf > svd(m, ComputeThinU|ComputeThinV)
static const double sigma
#define EIGEN_SIZE_MIN_PREFER_DYNAMIC(a, b)
EIGEN_DEVICE_FUNC ConstColwiseReturnType colwise() const
NumTraits< Scalar >::Real RealScalar
Two-sided Jacobi SVD decomposition of a rectangular matrix.
#define EIGEN_STATIC_ASSERT(CONDITION, MSG)
EIGEN_DEVICE_FUNC ConstRowwiseReturnType rowwise() const
The matrix class, also used for vectors and row-vectors.
Base class for all dense matrices, vectors, and expressions.
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
internal::umeyama_transform_matrix_type< Derived, OtherDerived >::type umeyama(const MatrixBase< Derived > &src, const MatrixBase< OtherDerived > &dst, bool with_scaling=true)
Returns the transformation between two point sets.
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:43:02