GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-04-23 10:09:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

add a JacobiRotation class wrapping the cosine-sine pair with

Browse Source 
some convenient features (transpose, adjoint, product)
This commit is contained in:
Gael Guennebaud 2009-09-01 13:18:03 +02:00
parent 32f95ec267
commit 8392373d96
4 changed files with 123 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Eigen/src/Core/MatrixBase.h
View File

@ -804,10 +804,10 @@ template<typename Derived> class MatrixBase
///////// Jacobi module /////////
template<typename JacobiScalar>
void applyJacobiOnTheLeft(int p, int q, JacobiScalar c, JacobiScalar s);
void applyJacobiOnTheLeft(int p, int q, const JacobiRotation<JacobiScalar>& j);
template<typename JacobiScalar>
void applyJacobiOnTheRight(int p, int q, JacobiScalar c, JacobiScalar s);
bool makeJacobi(int p, int q, Scalar *c, Scalar *s) const;
void applyJacobiOnTheRight(int p, int q, const JacobiRotation<JacobiScalar>& j);
bool makeJacobi(int p, int q, JacobiRotation<Scalar> *j) const;
#ifdef EIGEN_MATRIXBASE_PLUGIN
#include EIGEN_MATRIXBASE_PLUGIN

1
Eigen/src/Core/util/ForwardDeclarations.h
View File

@ -123,6 +123,7 @@ template<typename MatrixType> class SVD;
template<typename MatrixType, unsigned int Options = 0> class JacobiSVD;
template<typename MatrixType, int UpLo = LowerTriangular> class LLT;
template<typename MatrixType> class LDLT;
template<typename Scalar> class JacobiRotation;
// Geometry module:
template<typename Derived, int _Dim> class RotationBase;

120
Eigen/src/Jacobi/Jacobi.h
View File

@ -26,56 +26,98 @@
#ifndef EIGEN_JACOBI_H
#define EIGEN_JACOBI_H
/** Applies the counter clock wise 2D rotation of angle \c theta given by its
* cosine \a c and sine \a s to the set of 2D vectors of cordinates \a x and \a y:
* \f$ x = c x - s' y \f$
* \f$ y = s x + c y \f$
/** \ingroup Jacobi
* \class JacobiRotation
* \brief Represents a rotation in the plane from a cosine-sine pair.
*
* This class represents a Jacobi rotation which is also known as a Givens rotation.
* This is a 2D clock-wise rotation in the plane \c J of angle \f$ \theta \f$ defined by
* its cosine \c c and sine \c s as follow:
* \f$ J = \left ( \begin{array}{cc} c & \overline s \\ -s & \overline c \end{array} \right ) \f$
*
* \sa MatrixBase::makeJacobi(), MatrixBase::applyJacobiOnTheLeft(), MatrixBase::applyJacobiOnTheRight()
*/
template<typename Scalar> class JacobiRotation
{
public:
/** Default constructor without any initialization. */
JacobiRotation() {}
/** Construct a Jacobi rotation from a cosine-sine pair (\a c, \c s). */
JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
Scalar& c() { return m_c; }
Scalar c() const { return m_c; }
Scalar& s() { return m_s; }
Scalar s() const { return m_s; }
/** Concatenates two Jacobi rotation */
JacobiRotation operator*(const JacobiRotation& other)
{
return JacobiRotation(m_c * other.m_c - ei_conj(m_s) * other.m_s,
ei_conj(m_c * ei_conj(other.m_s) + ei_conj(m_s) * ei_conj(other.m_c)));
}
/** Returns the transposed transformation */
JacobiRotation transpose() const { return JacobiRotation(m_c, -ei_conj(m_s)); }
/** Returns the adjoint transformation */
JacobiRotation adjoint() const { return JacobiRotation(ei_conj(m_c), -m_s); }
protected:
Scalar m_c, m_s;
};
/** Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y:
* \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right ) = J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
*
* \sa MatrixBase::applyJacobiOnTheLeft(), MatrixBase::applyJacobiOnTheRight()
*/
template<typename VectorX, typename VectorY, typename JacobiScalar>
void ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, JacobiScalar c, JacobiScalar s);
void ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<JacobiScalar>& j);
/** Applies a rotation in the plane defined by \a c, \a s to the rows \a p and \a q of \c *this.
* More precisely, it computes B = J' * B, with J = [c s ; -s' c] and B = [ *this.row(p) ; *this.row(q) ]
* \sa MatrixBase::applyJacobiOnTheRight(), ei_apply_rotation_in_the_plane()
/** Applies the rotation in the plane \a j to the rows \a p and \a q of \c *this, i.e., it computes B = J * B,
* with \f$ B = \left ( \begin{array}{cc} \text{*this.row}(p) \\ \text{*this.row}(q) \end{array} \right ) \f$.
*
* \sa class JacobiRotation, MatrixBase::applyJacobiOnTheRight(), ei_apply_rotation_in_the_plane()
*/
template<typename Derived>
template<typename JacobiScalar>
inline void MatrixBase<Derived>::applyJacobiOnTheLeft(int p, int q, JacobiScalar c, JacobiScalar s)
inline void MatrixBase<Derived>::applyJacobiOnTheLeft(int p, int q, const JacobiRotation<JacobiScalar>& j)
{
RowXpr x(row(p));
RowXpr y(row(q));
ei_apply_rotation_in_the_plane(x, y, c, s);
ei_apply_rotation_in_the_plane(x, y, j);
}
/** Applies a rotation in the plane defined by \a c, \a s to the columns \a p and \a q of \c *this.
* More precisely, it computes B = B * J, with J = [c s ; -s' c] and B = [ *this.col(p) ; *this.col(q) ]
* \sa MatrixBase::applyJacobiOnTheLeft(), ei_apply_rotation_in_the_plane()
/** Applies the rotation in the plane \a j to the columns \a p and \a q of \c *this, i.e., it computes B = B * J
* with \f$ B = \left ( \begin{array}{cc} \text{*this.col}(p) & \text{*this.col}(q) \end{array} \right ) \f$.
*
* \sa class JacobiRotation, MatrixBase::applyJacobiOnTheLeft(), ei_apply_rotation_in_the_plane()
*/
template<typename Derived>
template<typename JacobiScalar>
inline void MatrixBase<Derived>::applyJacobiOnTheRight(int p, int q, JacobiScalar c, JacobiScalar s)
inline void MatrixBase<Derived>::applyJacobiOnTheRight(int p, int q, const JacobiRotation<JacobiScalar>& j)
{
ColXpr x(col(p));
ColXpr y(col(q));
ei_apply_rotation_in_the_plane(x, y, c, -ei_conj(s));
ei_apply_rotation_in_the_plane(x, y, j.transpose());
}
/** Computes the cosine-sine pair (\a c, \a s) such that its associated
* rotation \f$ J = ( \begin{array}{cc} c & \overline s \\ -s & \overline c \end{array} )\f$
* applied to both the right and left of the 2x2 matrix
* \f$ B = ( \begin{array}{cc} x & y \\ * & z \end{array} )\f$ yields
* a diagonal matrix A: \f$ A = J^* B J \f$
/** Computes the Jacobi rotation \a J such that applying \a J on both the right and left sides of the 2x2 matrix
* \f$ B = \left ( \begin{array}{cc} x & y \\ * & z \end{array} \right )\f$ yields
* a diagonal matrix \f$ A = J^* B J \f$
*
* \sa MatrixBase::makeJacobi(), MatrixBase::applyJacobiOnTheLeft(), MatrixBase::applyJacobiOnTheRight()
*/
template<typename Scalar>
bool ei_makeJacobi(typename NumTraits<Scalar>::Real x, Scalar y, typename NumTraits<Scalar>::Real z, Scalar *c, Scalar *s)
bool ei_makeJacobi(typename NumTraits<Scalar>::Real x, Scalar y, typename NumTraits<Scalar>::Real z, JacobiRotation<Scalar> *j)
{
typedef typename NumTraits<Scalar>::Real RealScalar;
if(y == Scalar(0))
{
*c = Scalar(1);
*s = Scalar(0);
j->c() = Scalar(1);
j->s() = Scalar(0);
return false;
}
else
@ -93,20 +135,26 @@ bool ei_makeJacobi(typename NumTraits<Scalar>::Real x, Scalar y, typename NumTra
}
RealScalar sign_t = t > 0 ? 1 : -1;
RealScalar n = RealScalar(1) / ei_sqrt(ei_abs2(t)+1);
*s = - sign_t * (ei_conj(y) / ei_abs(y)) * ei_abs(t) * n;
*c = n;
j->s() = - sign_t * (ei_conj(y) / ei_abs(y)) * ei_abs(t) * n;
j->c() = n;
return true;
}
}
/** Computes the Jacobi rotation \a J such that applying \a J on both the right and left sides of the 2x2 matrix
* \f$ B = \left ( \begin{array}{cc} \text{this}_{pp} & \text{this}_{pq} \\ * & \text{this}_{qq} \end{array} \right )\f$ yields
* a diagonal matrix \f$ A = J^* B J \f$
*
* \sa MatrixBase::ei_make_jacobi(), MatrixBase::applyJacobiOnTheLeft(), MatrixBase::applyJacobiOnTheRight()
*/
template<typename Derived>
inline bool MatrixBase<Derived>::makeJacobi(int p, int q, Scalar *c, Scalar *s) const
inline bool MatrixBase<Derived>::makeJacobi(int p, int q, JacobiRotation<Scalar> *j) const
{
return ei_makeJacobi(ei_real(coeff(p,p)), coeff(p,q), ei_real(coeff(q,q)), c, s);
return ei_makeJacobi(ei_real(coeff(p,p)), coeff(p,q), ei_real(coeff(q,q)), j);
}
template<typename VectorX, typename VectorY, typename JacobiScalar>
void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, JacobiScalar c, JacobiScalar s)
void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY& _y, const JacobiRotation<JacobiScalar>& j)
{
typedef typename VectorX::Scalar Scalar;
ei_assert(_x.size() == _y.size());
@ -126,16 +174,16 @@ void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY&
int alignedStart = ei_alignmentOffset(y, size);
int alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
const Packet pc = ei_pset1(Scalar(c));
const Packet ps = ei_pset1(Scalar(s));
const Packet pc = ei_pset1(Scalar(j.c()));
const Packet ps = ei_pset1(Scalar(j.s()));
ei_conj_helper<NumTraits<Scalar>::IsComplex,false> cj;
for(int i=0; i<alignedStart; ++i)
{
Scalar xi = x[i];
Scalar yi = y[i];
x[i] = c * xi + ei_conj(s) * yi;
y[i] = - s * xi + ei_conj(c) * yi;
x[i] = j.c() * xi + ei_conj(j.s()) * yi;
y[i] = -j.s() * xi + ei_conj(j.c()) * yi;
}
Scalar* px = x + alignedStart;
@ -182,8 +230,8 @@ void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY&
{
Scalar xi = x[i];
Scalar yi = y[i];
x[i] = c * xi + ei_conj(s) * yi;
y[i] = -s * xi + ei_conj(c) * yi;
x[i] = j.c() * xi + ei_conj(j.s()) * yi;
y[i] = -j.s() * xi + ei_conj(j.c()) * yi;
}
}
else
@ -192,8 +240,8 @@ void /*EIGEN_DONT_INLINE*/ ei_apply_rotation_in_the_plane(VectorX& _x, VectorY&
{
Scalar xi = *x;
Scalar yi = *y;
*x = c * xi + ei_conj(s) * yi;
*y = -s * xi + ei_conj(c) * yi;
*x = j.c() * xi + ei_conj(j.s()) * yi;
*y = -j.s() * xi + ei_conj(j.c()) * yi;
x += incrx;
y += incry;
}

73
Eigen/src/SVD/JacobiSVD.h
View File

@ -54,7 +54,7 @@ template<typename MatrixType, unsigned int Options> class JacobiSVD
MaxDiagSizeAtCompileTime = EIGEN_ENUM_MIN(MaxRowsAtCompileTime,MaxColsAtCompileTime),
MatrixOptions = MatrixType::Options
};
typedef Matrix<Scalar, Dynamic, Dynamic, MatrixOptions> DummyMatrixType;
typedef typename ei_meta_if<ComputeU,
Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime,
@ -73,13 +73,13 @@ template<typename MatrixType, unsigned int Options> class JacobiSVD
JacobiSVD() : m_isInitialized(false) {}
JacobiSVD(const MatrixType& matrix) : m_isInitialized(false)
JacobiSVD(const MatrixType& matrix) : m_isInitialized(false)
{
compute(matrix);
}
JacobiSVD& compute(const MatrixType& matrix);
const MatrixUType& matrixU() const
{
ei_assert(m_isInitialized && "JacobiSVD is not initialized.");
@ -103,7 +103,7 @@ template<typename MatrixType, unsigned int Options> class JacobiSVD
MatrixVType m_matrixV;
SingularValuesType m_singularValues;
bool m_isInitialized;
template<typename _MatrixType, unsigned int _Options, bool _IsComplex>
friend struct ei_svd_precondition_2x2_block_to_be_real;
};
@ -120,11 +120,12 @@ struct ei_svd_precondition_2x2_block_to_be_real<MatrixType, Options, true>
typedef JacobiSVD<MatrixType, Options> SVD;
typedef typename MatrixType::Scalar Scalar;
typedef typename MatrixType::RealScalar RealScalar;
enum { ComputeU = SVD::ComputeU, ComputeV = SVD::ComputeV };
static void run(MatrixType& work_matrix, JacobiSVD<MatrixType, Options>& svd, int p, int q)
{
Scalar c, s, z;
Scalar z;
JacobiRotation<Scalar> rot;
RealScalar n = ei_sqrt(ei_abs2(work_matrix.coeff(p,p)) + ei_abs2(work_matrix.coeff(q,p)));
if(n==0)
{
@ -137,10 +138,10 @@ struct ei_svd_precondition_2x2_block_to_be_real<MatrixType, Options, true>
}
else
{
c = ei_conj(work_matrix.coeff(p,p)) / n;
s = work_matrix.coeff(q,p) / n;
work_matrix.applyJacobiOnTheLeft(p,q,c,s);
if(ComputeU) svd.m_matrixU.applyJacobiOnTheRight(p,q,ei_conj(c),-s);
rot.c() = ei_conj(work_matrix.coeff(p,p)) / n;
rot.s() = work_matrix.coeff(q,p) / n;
work_matrix.applyJacobiOnTheLeft(p,q,rot);
if(ComputeU) svd.m_matrixU.applyJacobiOnTheRight(p,q,rot.adjoint());
if(work_matrix.coeff(p,q) != Scalar(0))
{
Scalar z = ei_abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q);
@ -154,38 +155,34 @@ struct ei_svd_precondition_2x2_block_to_be_real<MatrixType, Options, true>
if(ComputeU) svd.m_matrixU.col(q) *= ei_conj(z);
}
}
}
}
};
template<typename MatrixType, typename RealScalar>
void ei_real_2x2_jacobi_svd(const MatrixType& matrix, int p, int q,
RealScalar *c_left, RealScalar *s_left,
RealScalar *c_right, RealScalar *s_right)
JacobiRotation<RealScalar> *j_left,
JacobiRotation<RealScalar> *j_right)
{
Matrix<RealScalar,2,2> m;
m << ei_real(matrix.coeff(p,p)), ei_real(matrix.coeff(p,q)),
ei_real(matrix.coeff(q,p)), ei_real(matrix.coeff(q,q));
RealScalar c1, s1;
ei_real(matrix.coeff(q,p)), ei_real(matrix.coeff(q,q));
JacobiRotation<RealScalar> rot1;
RealScalar t = m.coeff(0,0) + m.coeff(1,1);
RealScalar d = m.coeff(1,0) - m.coeff(0,1);
if(t == RealScalar(0))
{
c1 = 0;
s1 = d > 0 ? 1 : -1;
rot1.c() = 0;
rot1.s() = d > 0 ? 1 : -1;
}
else
{
RealScalar u = d / t;
c1 = RealScalar(1) / ei_sqrt(1 + ei_abs2(u));
s1 = c1 * u;
rot1.c() = RealScalar(1) / ei_sqrt(1 + ei_abs2(u));
rot1.s() = rot1.c() * u;
}
m.applyJacobiOnTheLeft(0,1,c1,s1);
RealScalar c2, s2;
m.makeJacobi(0,1,&c2,&s2);
*c_left = c1*c2 + s1*s2;
*s_left = s1*c2 - c1*s2;
*c_right = c2;
*s_right = s2;
m.applyJacobiOnTheLeft(0,1,rot1);
m.makeJacobi(0,1,j_right);
*j_left = rot1 * j_right->transpose();
}
template<typename MatrixType, unsigned int Options>
@ -208,18 +205,18 @@ sweep_again:
{
ei_svd_precondition_2x2_block_to_be_real<MatrixType, Options>::run(work_matrix, *this, p, q);
RealScalar c_left, s_left, c_right, s_right;
ei_real_2x2_jacobi_svd(work_matrix, p, q, &c_left, &s_left, &c_right, &s_right);
work_matrix.applyJacobiOnTheLeft(p,q,c_left,s_left);
if(ComputeU) m_matrixU.applyJacobiOnTheRight(p,q,c_left,-s_left);
work_matrix.applyJacobiOnTheRight(p,q,c_right,s_right);
if(ComputeV) m_matrixV.applyJacobiOnTheRight(p,q,c_right,s_right);
JacobiRotation<RealScalar> j_left, j_right;
ei_real_2x2_jacobi_svd(work_matrix, p, q, &j_left, &j_right);
work_matrix.applyJacobiOnTheLeft(p,q,j_left);
if(ComputeU) m_matrixU.applyJacobiOnTheRight(p,q,j_left.transpose());
work_matrix.applyJacobiOnTheRight(p,q,j_right);
if(ComputeV) m_matrixV.applyJacobiOnTheRight(p,q,j_right);
}
}
}
RealScalar biggestOnDiag = work_matrix.diagonal().cwise().abs().maxCoeff();
RealScalar maxAllowedOffDiag = biggestOnDiag * precision;
for(int p = 0; p < size; ++p)
@ -231,7 +228,7 @@ sweep_again:
if(ei_abs(work_matrix.coeff(p,q)) > maxAllowedOffDiag)
goto sweep_again;
}
for(int i = 0; i < size; ++i)
{
RealScalar a = ei_abs(work_matrix.coeff(i,i));
@ -251,7 +248,7 @@ sweep_again:
if(ComputeV) m_matrixV.col(pos).swap(m_matrixV.col(i));
}
}
m_isInitialized = true;
return *this;
}