$treeview $search $mathjax
Eigen
3.2.5
$projectbrief
|
$projectbrief
|
$searchbox |
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com> 00005 // 00006 // This Source Code Form is subject to the terms of the Mozilla 00007 // Public License v. 2.0. If a copy of the MPL was not distributed 00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00009 00010 #ifndef EIGEN_UMEYAMA_H 00011 #define EIGEN_UMEYAMA_H 00012 00013 // This file requires the user to include 00014 // * Eigen/Core 00015 // * Eigen/LU 00016 // * Eigen/SVD 00017 // * Eigen/Array 00018 00019 namespace Eigen { 00020 00021 #ifndef EIGEN_PARSED_BY_DOXYGEN 00022 00023 // These helpers are required since it allows to use mixed types as parameters 00024 // for the Umeyama. The problem with mixed parameters is that the return type 00025 // cannot trivially be deduced when float and double types are mixed. 00026 namespace internal { 00027 00028 // Compile time return type deduction for different MatrixBase types. 00029 // Different means here different alignment and parameters but the same underlying 00030 // real scalar type. 00031 template<typename MatrixType, typename OtherMatrixType> 00032 struct umeyama_transform_matrix_type 00033 { 00034 enum { 00035 MinRowsAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime), 00036 00037 // When possible we want to choose some small fixed size value since the result 00038 // is likely to fit on the stack. So here, EIGEN_SIZE_MIN_PREFER_DYNAMIC is not what we want. 00039 HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime)+1 00040 }; 00041 00042 typedef Matrix<typename traits<MatrixType>::Scalar, 00043 HomogeneousDimension, 00044 HomogeneousDimension, 00045 AutoAlign | (traits<MatrixType>::Flags & RowMajorBit ? RowMajor : ColMajor), 00046 HomogeneousDimension, 00047 HomogeneousDimension 00048 > type; 00049 }; 00050 00051 } 00052 00053 #endif 00054 00093 template <typename Derived, typename OtherDerived> 00094 typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type 00095 umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true) 00096 { 00097 typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType; 00098 typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar; 00099 typedef typename NumTraits<Scalar>::Real RealScalar; 00100 typedef typename Derived::Index Index; 00101 00102 EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL) 00103 EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value), 00104 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 00105 00106 enum { Dimension = EIGEN_SIZE_MIN_PREFER_DYNAMIC(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) }; 00107 00108 typedef Matrix<Scalar, Dimension, 1> VectorType; 00109 typedef Matrix<Scalar, Dimension, Dimension> MatrixType; 00110 typedef typename internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType; 00111 00112 const Index m = src.rows(); // dimension 00113 const Index n = src.cols(); // number of measurements 00114 00115 // required for demeaning ... 00116 const RealScalar one_over_n = RealScalar(1) / static_cast<RealScalar>(n); 00117 00118 // computation of mean 00119 const VectorType src_mean = src.rowwise().sum() * one_over_n; 00120 const VectorType dst_mean = dst.rowwise().sum() * one_over_n; 00121 00122 // demeaning of src and dst points 00123 const RowMajorMatrixType src_demean = src.colwise() - src_mean; 00124 const RowMajorMatrixType dst_demean = dst.colwise() - dst_mean; 00125 00126 // Eq. (36)-(37) 00127 const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n; 00128 00129 // Eq. (38) 00130 const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose(); 00131 00132 JacobiSVD<MatrixType> svd(sigma, ComputeFullU | ComputeFullV); 00133 00134 // Initialize the resulting transformation with an identity matrix... 00135 TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1); 00136 00137 // Eq. (39) 00138 VectorType S = VectorType::Ones(m); 00139 if (sigma.determinant()<Scalar(0)) S(m-1) = Scalar(-1); 00140 00141 // Eq. (40) and (43) 00142 const VectorType& d = svd.singularValues(); 00143 Index rank = 0; for (Index i=0; i<m; ++i) if (!internal::isMuchSmallerThan(d.coeff(i),d.coeff(0))) ++rank; 00144 if (rank == m-1) { 00145 if ( svd.matrixU().determinant() * svd.matrixV().determinant() > Scalar(0) ) { 00146 Rt.block(0,0,m,m).noalias() = svd.matrixU()*svd.matrixV().transpose(); 00147 } else { 00148 const Scalar s = S(m-1); S(m-1) = Scalar(-1); 00149 Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); 00150 S(m-1) = s; 00151 } 00152 } else { 00153 Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose(); 00154 } 00155 00156 if (with_scaling) 00157 { 00158 // Eq. (42) 00159 const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S); 00160 00161 // Eq. (41) 00162 Rt.col(m).head(m) = dst_mean; 00163 Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean; 00164 Rt.block(0,0,m,m) *= c; 00165 } 00166 else 00167 { 00168 Rt.col(m).head(m) = dst_mean; 00169 Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean; 00170 } 00171 00172 return Rt; 00173 } 00174 00175 } // end namespace Eigen 00176 00177 #endif // EIGEN_UMEYAMA_H