Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef EIGEN_UMEYAMA_H
00026 #define EIGEN_UMEYAMA_H
00027
00028
00029
00030
00031
00032
00033
00034 #ifndef EIGEN_PARSED_BY_DOXYGEN
00035
00036
00037
00038
00039 namespace internal {
00040
00041
00042
00043
00044 template<typename MatrixType, typename OtherMatrixType>
00045 struct umeyama_transform_matrix_type
00046 {
00047 enum {
00048 MinRowsAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime),
00049
00050
00051
00052 HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime)+1
00053 };
00054
00055 typedef Matrix<typename traits<MatrixType>::Scalar,
00056 HomogeneousDimension,
00057 HomogeneousDimension,
00058 AutoAlign | (traits<MatrixType>::Flags & RowMajorBit ? RowMajor : ColMajor),
00059 HomogeneousDimension,
00060 HomogeneousDimension
00061 > type;
00062 };
00063
00064 }
00065
00066 #endif
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106 template <typename Derived, typename OtherDerived>
00107 typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
00108 umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true)
00109 {
00110 typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
00111 typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar;
00112 typedef typename NumTraits<Scalar>::Real RealScalar;
00113 typedef typename Derived::Index Index;
00114
00115 EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL)
00116 EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value),
00117 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00118
00119 enum { Dimension = EIGEN_SIZE_MIN_PREFER_DYNAMIC(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
00120
00121 typedef Matrix<Scalar, Dimension, 1> VectorType;
00122 typedef Matrix<Scalar, Dimension, Dimension> MatrixType;
00123 typedef typename internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType;
00124
00125 const Index m = src.rows();
00126 const Index n = src.cols();
00127
00128
00129 const RealScalar one_over_n = 1 / static_cast<RealScalar>(n);
00130
00131
00132 const VectorType src_mean = src.rowwise().sum() * one_over_n;
00133 const VectorType dst_mean = dst.rowwise().sum() * one_over_n;
00134
00135
00136 const RowMajorMatrixType src_demean = src.colwise() - src_mean;
00137 const RowMajorMatrixType dst_demean = dst.colwise() - dst_mean;
00138
00139
00140 const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n;
00141
00142
00143 const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose();
00144
00145 JacobiSVD<MatrixType> svd(sigma, ComputeFullU | ComputeFullV);
00146
00147
00148 TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1);
00149
00150
00151 VectorType S = VectorType::Ones(m);
00152 if (sigma.determinant()<0) S(m-1) = -1;
00153
00154
00155 const VectorType& d = svd.singularValues();
00156 Index rank = 0; for (Index i=0; i<m; ++i) if (!internal::isMuchSmallerThan(d.coeff(i),d.coeff(0))) ++rank;
00157 if (rank == m-1) {
00158 if ( svd.matrixU().determinant() * svd.matrixV().determinant() > 0 ) {
00159 Rt.block(0,0,m,m).noalias() = svd.matrixU()*svd.matrixV().transpose();
00160 } else {
00161 const Scalar s = S(m-1); S(m-1) = -1;
00162 Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
00163 S(m-1) = s;
00164 }
00165 } else {
00166 Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
00167 }
00168
00169
00170 const Scalar c = 1/src_var * svd.singularValues().dot(S);
00171
00172
00173
00174
00175 Rt.col(m).head(m) = dst_mean;
00176 Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean;
00177
00178 if (with_scaling) Rt.block(0,0,m,m) *= c;
00179
00180 return Rt;
00181 }
00182
00183 #endif // EIGEN_UMEYAMA_H