Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
Householder.h
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2010 Benoit Jacob <jacob.benoit.1@gmail.com> 00005 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr> 00006 // 00007 // This Source Code Form is subject to the terms of the Mozilla 00008 // Public License v. 2.0. If a copy of the MPL was not distributed 00009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00010 00011 #ifndef EIGEN_HOUSEHOLDER_H 00012 #define EIGEN_HOUSEHOLDER_H 00013 00014 namespace Eigen { 00015 00016 namespace internal { 00017 template<int n> struct decrement_size 00018 { 00019 enum { 00020 ret = n==Dynamic ? n : n-1 00021 }; 00022 }; 00023 } 00024 00025 /** Computes the elementary reflector H such that: 00026 * \f$ H *this = [ beta 0 ... 0]^T \f$ 00027 * where the transformation H is: 00028 * \f$ H = I - tau v v^*\f$ 00029 * and the vector v is: 00030 * \f$ v^T = [1 essential^T] \f$ 00031 * 00032 * The essential part of the vector \c v is stored in *this. 00033 * 00034 * On output: 00035 * \param tau the scaling factor of the Householder transformation 00036 * \param beta the result of H * \c *this 00037 * 00038 * \sa MatrixBase::makeHouseholder(), MatrixBase::applyHouseholderOnTheLeft(), 00039 * MatrixBase::applyHouseholderOnTheRight() 00040 */ 00041 template<typename Derived> 00042 void MatrixBase<Derived>::makeHouseholderInPlace(Scalar& tau, RealScalar& beta) 00043 { 00044 VectorBlock<Derived, internal::decrement_size<Base::SizeAtCompileTime>::ret> essentialPart(derived(), 1, size()-1); 00045 makeHouseholder(essentialPart, tau, beta); 00046 } 00047 00048 /** Computes the elementary reflector H such that: 00049 * \f$ H *this = [ beta 0 ... 0]^T \f$ 00050 * where the transformation H is: 00051 * \f$ H = I - tau v v^*\f$ 00052 * and the vector v is: 00053 * \f$ v^T = [1 essential^T] \f$ 00054 * 00055 * On output: 00056 * \param essential the essential part of the vector \c v 00057 * \param tau the scaling factor of the Householder transformation 00058 * \param beta the result of H * \c *this 00059 * 00060 * \sa MatrixBase::makeHouseholderInPlace(), MatrixBase::applyHouseholderOnTheLeft(), 00061 * MatrixBase::applyHouseholderOnTheRight() 00062 */ 00063 template<typename Derived> 00064 template<typename EssentialPart> 00065 void MatrixBase<Derived>::makeHouseholder( 00066 EssentialPart& essential, 00067 Scalar& tau, 00068 RealScalar& beta) const 00069 { 00070 using std::sqrt; 00071 using numext::conj; 00072 00073 EIGEN_STATIC_ASSERT_VECTOR_ONLY(EssentialPart) 00074 VectorBlock<const Derived, EssentialPart::SizeAtCompileTime> tail(derived(), 1, size()-1); 00075 00076 RealScalar tailSqNorm = size()==1 ? RealScalar(0) : tail.squaredNorm(); 00077 Scalar c0 = coeff(0); 00078 00079 if(tailSqNorm == RealScalar(0) && numext::imag(c0)==RealScalar(0)) 00080 { 00081 tau = RealScalar(0); 00082 beta = numext::real(c0); 00083 essential.setZero(); 00084 } 00085 else 00086 { 00087 beta = sqrt(numext::abs2(c0) + tailSqNorm); 00088 if (numext::real(c0)>=RealScalar(0)) 00089 beta = -beta; 00090 essential = tail / (c0 - beta); 00091 tau = conj((beta - c0) / beta); 00092 } 00093 } 00094 00095 /** Apply the elementary reflector H given by 00096 * \f$ H = I - tau v v^*\f$ 00097 * with 00098 * \f$ v^T = [1 essential^T] \f$ 00099 * from the left to a vector or matrix. 00100 * 00101 * On input: 00102 * \param essential the essential part of the vector \c v 00103 * \param tau the scaling factor of the Householder transformation 00104 * \param workspace a pointer to working space with at least 00105 * this->cols() * essential.size() entries 00106 * 00107 * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(), 00108 * MatrixBase::applyHouseholderOnTheRight() 00109 */ 00110 template<typename Derived> 00111 template<typename EssentialPart> 00112 void MatrixBase<Derived>::applyHouseholderOnTheLeft( 00113 const EssentialPart& essential, 00114 const Scalar& tau, 00115 Scalar* workspace) 00116 { 00117 if(rows() == 1) 00118 { 00119 *this *= Scalar(1)-tau; 00120 } 00121 else 00122 { 00123 Map<typename internal::plain_row_type<PlainObject>::type> tmp(workspace,cols()); 00124 Block<Derived, EssentialPart::SizeAtCompileTime, Derived::ColsAtCompileTime> bottom(derived(), 1, 0, rows()-1, cols()); 00125 tmp.noalias() = essential.adjoint() * bottom; 00126 tmp += this->row(0); 00127 this->row(0) -= tau * tmp; 00128 bottom.noalias() -= tau * essential * tmp; 00129 } 00130 } 00131 00132 /** Apply the elementary reflector H given by 00133 * \f$ H = I - tau v v^*\f$ 00134 * with 00135 * \f$ v^T = [1 essential^T] \f$ 00136 * from the right to a vector or matrix. 00137 * 00138 * On input: 00139 * \param essential the essential part of the vector \c v 00140 * \param tau the scaling factor of the Householder transformation 00141 * \param workspace a pointer to working space with at least 00142 * this->cols() * essential.size() entries 00143 * 00144 * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(), 00145 * MatrixBase::applyHouseholderOnTheLeft() 00146 */ 00147 template<typename Derived> 00148 template<typename EssentialPart> 00149 void MatrixBase<Derived>::applyHouseholderOnTheRight( 00150 const EssentialPart& essential, 00151 const Scalar& tau, 00152 Scalar* workspace) 00153 { 00154 if(cols() == 1) 00155 { 00156 *this *= Scalar(1)-tau; 00157 } 00158 else 00159 { 00160 Map<typename internal::plain_col_type<PlainObject>::type> tmp(workspace,rows()); 00161 Block<Derived, Derived::RowsAtCompileTime, EssentialPart::SizeAtCompileTime> right(derived(), 0, 1, rows(), cols()-1); 00162 tmp.noalias() = right * essential.conjugate(); 00163 tmp += this->col(0); 00164 this->col(0) -= tau * tmp; 00165 right.noalias() -= tau * tmp * essential.transpose(); 00166 } 00167 } 00168 00169 } // end namespace Eigen 00170 00171 #endif // EIGEN_HOUSEHOLDER_H
Generated on Tue Jul 12 2022 17:46:55 by 1.7.2