Yoji KURODA / Eigen

Eigne Matrix Class Library

Dependents:   Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more

Eigen Matrix Class Library for mbed.

Finally, you can use Eigen on your mbed!!!

src/Householder/Householder.h@0:13a5d365ba16, 2016-10-13 (annotated)

Committer:
ykuroda
Date:
Thu Oct 13 04:07:23 2016 +0000
Revision:
0:13a5d365ba16
First commint, Eigne Matrix Class Library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ykuroda 0:13a5d365ba16 1 // This file is part of Eigen, a lightweight C++ template library
ykuroda 0:13a5d365ba16 2 // for linear algebra.
ykuroda 0:13a5d365ba16 3 //
ykuroda 0:13a5d365ba16 4 // Copyright (C) 2010 Benoit Jacob <jacob.benoit.1@gmail.com>
ykuroda 0:13a5d365ba16 5 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
ykuroda 0:13a5d365ba16 6 //
ykuroda 0:13a5d365ba16 7 // This Source Code Form is subject to the terms of the Mozilla
ykuroda 0:13a5d365ba16 8 // Public License v. 2.0. If a copy of the MPL was not distributed
ykuroda 0:13a5d365ba16 9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
ykuroda 0:13a5d365ba16 10
ykuroda 0:13a5d365ba16 11 #ifndef EIGEN_HOUSEHOLDER_H
ykuroda 0:13a5d365ba16 12 #define EIGEN_HOUSEHOLDER_H
ykuroda 0:13a5d365ba16 13
ykuroda 0:13a5d365ba16 14 namespace Eigen {
ykuroda 0:13a5d365ba16 15
ykuroda 0:13a5d365ba16 16 namespace internal {
ykuroda 0:13a5d365ba16 17 template<int n> struct decrement_size
ykuroda 0:13a5d365ba16 18 {
ykuroda 0:13a5d365ba16 19 enum {
ykuroda 0:13a5d365ba16 20 ret = n==Dynamic ? n : n-1
ykuroda 0:13a5d365ba16 21 };
ykuroda 0:13a5d365ba16 22 };
ykuroda 0:13a5d365ba16 23 }
ykuroda 0:13a5d365ba16 24
ykuroda 0:13a5d365ba16 25 /** Computes the elementary reflector H such that:
ykuroda 0:13a5d365ba16 26 * \f$ H *this = [ beta 0 ... 0]^T \f$
ykuroda 0:13a5d365ba16 27 * where the transformation H is:
ykuroda 0:13a5d365ba16 28 * \f$ H = I - tau v v^*\f$
ykuroda 0:13a5d365ba16 29 * and the vector v is:
ykuroda 0:13a5d365ba16 30 * \f$ v^T = [1 essential^T] \f$
ykuroda 0:13a5d365ba16 31 *
ykuroda 0:13a5d365ba16 32 * The essential part of the vector \c v is stored in *this.
ykuroda 0:13a5d365ba16 33 *
ykuroda 0:13a5d365ba16 34 * On output:
ykuroda 0:13a5d365ba16 35 * \param tau the scaling factor of the Householder transformation
ykuroda 0:13a5d365ba16 36 * \param beta the result of H * \c *this
ykuroda 0:13a5d365ba16 37 *
ykuroda 0:13a5d365ba16 38 * \sa MatrixBase::makeHouseholder(), MatrixBase::applyHouseholderOnTheLeft(),
ykuroda 0:13a5d365ba16 39 * MatrixBase::applyHouseholderOnTheRight()
ykuroda 0:13a5d365ba16 40 */
ykuroda 0:13a5d365ba16 41 template<typename Derived>
ykuroda 0:13a5d365ba16 42 void MatrixBase<Derived>::makeHouseholderInPlace(Scalar& tau, RealScalar& beta)
ykuroda 0:13a5d365ba16 43 {
ykuroda 0:13a5d365ba16 44 VectorBlock<Derived, internal::decrement_size<Base::SizeAtCompileTime>::ret> essentialPart(derived(), 1, size()-1);
ykuroda 0:13a5d365ba16 45 makeHouseholder(essentialPart, tau, beta);
ykuroda 0:13a5d365ba16 46 }
ykuroda 0:13a5d365ba16 47
ykuroda 0:13a5d365ba16 48 /** Computes the elementary reflector H such that:
ykuroda 0:13a5d365ba16 49 * \f$ H *this = [ beta 0 ... 0]^T \f$
ykuroda 0:13a5d365ba16 50 * where the transformation H is:
ykuroda 0:13a5d365ba16 51 * \f$ H = I - tau v v^*\f$
ykuroda 0:13a5d365ba16 52 * and the vector v is:
ykuroda 0:13a5d365ba16 53 * \f$ v^T = [1 essential^T] \f$
ykuroda 0:13a5d365ba16 54 *
ykuroda 0:13a5d365ba16 55 * On output:
ykuroda 0:13a5d365ba16 56 * \param essential the essential part of the vector \c v
ykuroda 0:13a5d365ba16 57 * \param tau the scaling factor of the Householder transformation
ykuroda 0:13a5d365ba16 58 * \param beta the result of H * \c *this
ykuroda 0:13a5d365ba16 59 *
ykuroda 0:13a5d365ba16 60 * \sa MatrixBase::makeHouseholderInPlace(), MatrixBase::applyHouseholderOnTheLeft(),
ykuroda 0:13a5d365ba16 61 * MatrixBase::applyHouseholderOnTheRight()
ykuroda 0:13a5d365ba16 62 */
ykuroda 0:13a5d365ba16 63 template<typename Derived>
ykuroda 0:13a5d365ba16 64 template<typename EssentialPart>
ykuroda 0:13a5d365ba16 65 void MatrixBase<Derived>::makeHouseholder(
ykuroda 0:13a5d365ba16 66 EssentialPart& essential,
ykuroda 0:13a5d365ba16 67 Scalar& tau,
ykuroda 0:13a5d365ba16 68 RealScalar& beta) const
ykuroda 0:13a5d365ba16 69 {
ykuroda 0:13a5d365ba16 70 using std::sqrt;
ykuroda 0:13a5d365ba16 71 using numext::conj;
ykuroda 0:13a5d365ba16 72
ykuroda 0:13a5d365ba16 73 EIGEN_STATIC_ASSERT_VECTOR_ONLY(EssentialPart)
ykuroda 0:13a5d365ba16 74 VectorBlock<const Derived, EssentialPart::SizeAtCompileTime> tail(derived(), 1, size()-1);
ykuroda 0:13a5d365ba16 75
ykuroda 0:13a5d365ba16 76 RealScalar tailSqNorm = size()==1 ? RealScalar(0) : tail.squaredNorm();
ykuroda 0:13a5d365ba16 77 Scalar c0 = coeff(0);
ykuroda 0:13a5d365ba16 78
ykuroda 0:13a5d365ba16 79 if(tailSqNorm == RealScalar(0) && numext::imag(c0)==RealScalar(0))
ykuroda 0:13a5d365ba16 80 {
ykuroda 0:13a5d365ba16 81 tau = RealScalar(0);
ykuroda 0:13a5d365ba16 82 beta = numext::real(c0);
ykuroda 0:13a5d365ba16 83 essential.setZero();
ykuroda 0:13a5d365ba16 84 }
ykuroda 0:13a5d365ba16 85 else
ykuroda 0:13a5d365ba16 86 {
ykuroda 0:13a5d365ba16 87 beta = sqrt(numext::abs2(c0) + tailSqNorm);
ykuroda 0:13a5d365ba16 88 if (numext::real(c0)>=RealScalar(0))
ykuroda 0:13a5d365ba16 89 beta = -beta;
ykuroda 0:13a5d365ba16 90 essential = tail / (c0 - beta);
ykuroda 0:13a5d365ba16 91 tau = conj((beta - c0) / beta);
ykuroda 0:13a5d365ba16 92 }
ykuroda 0:13a5d365ba16 93 }
ykuroda 0:13a5d365ba16 94
ykuroda 0:13a5d365ba16 95 /** Apply the elementary reflector H given by
ykuroda 0:13a5d365ba16 96 * \f$ H = I - tau v v^*\f$
ykuroda 0:13a5d365ba16 97 * with
ykuroda 0:13a5d365ba16 98 * \f$ v^T = [1 essential^T] \f$
ykuroda 0:13a5d365ba16 99 * from the left to a vector or matrix.
ykuroda 0:13a5d365ba16 100 *
ykuroda 0:13a5d365ba16 101 * On input:
ykuroda 0:13a5d365ba16 102 * \param essential the essential part of the vector \c v
ykuroda 0:13a5d365ba16 103 * \param tau the scaling factor of the Householder transformation
ykuroda 0:13a5d365ba16 104 * \param workspace a pointer to working space with at least
ykuroda 0:13a5d365ba16 105 * this->cols() * essential.size() entries
ykuroda 0:13a5d365ba16 106 *
ykuroda 0:13a5d365ba16 107 * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(),
ykuroda 0:13a5d365ba16 108 * MatrixBase::applyHouseholderOnTheRight()
ykuroda 0:13a5d365ba16 109 */
ykuroda 0:13a5d365ba16 110 template<typename Derived>
ykuroda 0:13a5d365ba16 111 template<typename EssentialPart>
ykuroda 0:13a5d365ba16 112 void MatrixBase<Derived>::applyHouseholderOnTheLeft(
ykuroda 0:13a5d365ba16 113 const EssentialPart& essential,
ykuroda 0:13a5d365ba16 114 const Scalar& tau,
ykuroda 0:13a5d365ba16 115 Scalar* workspace)
ykuroda 0:13a5d365ba16 116 {
ykuroda 0:13a5d365ba16 117 if(rows() == 1)
ykuroda 0:13a5d365ba16 118 {
ykuroda 0:13a5d365ba16 119 *this *= Scalar(1)-tau;
ykuroda 0:13a5d365ba16 120 }
ykuroda 0:13a5d365ba16 121 else
ykuroda 0:13a5d365ba16 122 {
ykuroda 0:13a5d365ba16 123 Map<typename internal::plain_row_type<PlainObject>::type> tmp(workspace,cols());
ykuroda 0:13a5d365ba16 124 Block<Derived, EssentialPart::SizeAtCompileTime, Derived::ColsAtCompileTime> bottom(derived(), 1, 0, rows()-1, cols());
ykuroda 0:13a5d365ba16 125 tmp.noalias() = essential.adjoint() * bottom;
ykuroda 0:13a5d365ba16 126 tmp += this->row(0);
ykuroda 0:13a5d365ba16 127 this->row(0) -= tau * tmp;
ykuroda 0:13a5d365ba16 128 bottom.noalias() -= tau * essential * tmp;
ykuroda 0:13a5d365ba16 129 }
ykuroda 0:13a5d365ba16 130 }
ykuroda 0:13a5d365ba16 131
ykuroda 0:13a5d365ba16 132 /** Apply the elementary reflector H given by
ykuroda 0:13a5d365ba16 133 * \f$ H = I - tau v v^*\f$
ykuroda 0:13a5d365ba16 134 * with
ykuroda 0:13a5d365ba16 135 * \f$ v^T = [1 essential^T] \f$
ykuroda 0:13a5d365ba16 136 * from the right to a vector or matrix.
ykuroda 0:13a5d365ba16 137 *
ykuroda 0:13a5d365ba16 138 * On input:
ykuroda 0:13a5d365ba16 139 * \param essential the essential part of the vector \c v
ykuroda 0:13a5d365ba16 140 * \param tau the scaling factor of the Householder transformation
ykuroda 0:13a5d365ba16 141 * \param workspace a pointer to working space with at least
ykuroda 0:13a5d365ba16 142 * this->cols() * essential.size() entries
ykuroda 0:13a5d365ba16 143 *
ykuroda 0:13a5d365ba16 144 * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(),
ykuroda 0:13a5d365ba16 145 * MatrixBase::applyHouseholderOnTheLeft()
ykuroda 0:13a5d365ba16 146 */
ykuroda 0:13a5d365ba16 147 template<typename Derived>
ykuroda 0:13a5d365ba16 148 template<typename EssentialPart>
ykuroda 0:13a5d365ba16 149 void MatrixBase<Derived>::applyHouseholderOnTheRight(
ykuroda 0:13a5d365ba16 150 const EssentialPart& essential,
ykuroda 0:13a5d365ba16 151 const Scalar& tau,
ykuroda 0:13a5d365ba16 152 Scalar* workspace)
ykuroda 0:13a5d365ba16 153 {
ykuroda 0:13a5d365ba16 154 if(cols() == 1)
ykuroda 0:13a5d365ba16 155 {
ykuroda 0:13a5d365ba16 156 *this *= Scalar(1)-tau;
ykuroda 0:13a5d365ba16 157 }
ykuroda 0:13a5d365ba16 158 else
ykuroda 0:13a5d365ba16 159 {
ykuroda 0:13a5d365ba16 160 Map<typename internal::plain_col_type<PlainObject>::type> tmp(workspace,rows());
ykuroda 0:13a5d365ba16 161 Block<Derived, Derived::RowsAtCompileTime, EssentialPart::SizeAtCompileTime> right(derived(), 0, 1, rows(), cols()-1);
ykuroda 0:13a5d365ba16 162 tmp.noalias() = right * essential.conjugate();
ykuroda 0:13a5d365ba16 163 tmp += this->col(0);
ykuroda 0:13a5d365ba16 164 this->col(0) -= tau * tmp;
ykuroda 0:13a5d365ba16 165 right.noalias() -= tau * tmp * essential.transpose();
ykuroda 0:13a5d365ba16 166 }
ykuroda 0:13a5d365ba16 167 }
ykuroda 0:13a5d365ba16 168
ykuroda 0:13a5d365ba16 169 } // end namespace Eigen
ykuroda 0:13a5d365ba16 170
ykuroda 0:13a5d365ba16 171 #endif // EIGEN_HOUSEHOLDER_H