Jacobi.h
Go to the documentation of this file.
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
5 // Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
6 //
7 // This Source Code Form is subject to the terms of the Mozilla
8 // Public License v. 2.0. If a copy of the MPL was not distributed
9 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10 
11 #ifndef EIGEN_JACOBI_H
12 #define EIGEN_JACOBI_H
13 
14 namespace Eigen {
15 
34 template<typename Scalar> class JacobiRotation
35 {
36  public:
38 
42 
45  JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
46 
47  EIGEN_DEVICE_FUNC Scalar& c() { return m_c; }
48  EIGEN_DEVICE_FUNC Scalar c() const { return m_c; }
49  EIGEN_DEVICE_FUNC Scalar& s() { return m_s; }
50  EIGEN_DEVICE_FUNC Scalar s() const { return m_s; }
51 
55  {
56  using numext::conj;
57  return JacobiRotation(m_c * other.m_c - conj(m_s) * other.m_s,
58  conj(m_c * conj(other.m_s) + conj(m_s) * conj(other.m_c)));
59  }
60 
64 
67  JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); }
68 
69  template<typename Derived>
73  bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z);
74 
76  void makeGivens(const Scalar& p, const Scalar& q, Scalar* r=0);
77 
78  protected:
80  void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type);
82  void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type);
83 
85 };
86 
92 template<typename Scalar>
95 {
96  using std::sqrt;
97  using std::abs;
98 
99  RealScalar deno = RealScalar(2)*abs(y);
101  {
102  m_c = Scalar(1);
103  m_s = Scalar(0);
104  return false;
105  }
106  else
107  {
108  RealScalar tau = (x-z)/deno;
109  RealScalar w = sqrt(numext::abs2(tau) + RealScalar(1));
110  RealScalar t;
111  if(tau>RealScalar(0))
112  {
113  t = RealScalar(1) / (tau + w);
114  }
115  else
116  {
117  t = RealScalar(1) / (tau - w);
118  }
119  RealScalar sign_t = t > RealScalar(0) ? RealScalar(1) : RealScalar(-1);
120  RealScalar n = RealScalar(1) / sqrt(numext::abs2(t)+RealScalar(1));
121  m_s = - sign_t * (numext::conj(y) / abs(y)) * abs(t) * n;
122  m_c = n;
123  return true;
124  }
125 }
126 
136 template<typename Scalar>
137 template<typename Derived>
140 {
141  return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
142 }
143 
160 template<typename Scalar>
163 {
165 }
166 
167 
168 // specialization for complexes
169 template<typename Scalar>
172 {
173  using std::sqrt;
174  using std::abs;
175  using numext::conj;
176 
177  if(q==Scalar(0))
178  {
179  m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1);
180  m_s = 0;
181  if(r) *r = m_c * p;
182  }
183  else if(p==Scalar(0))
184  {
185  m_c = 0;
186  m_s = -q/abs(q);
187  if(r) *r = abs(q);
188  }
189  else
190  {
191  RealScalar p1 = numext::norm1(p);
192  RealScalar q1 = numext::norm1(q);
193  if(p1>=q1)
194  {
195  Scalar ps = p / p1;
196  RealScalar p2 = numext::abs2(ps);
197  Scalar qs = q / p1;
198  RealScalar q2 = numext::abs2(qs);
199 
200  RealScalar u = sqrt(RealScalar(1) + q2/p2);
201  if(numext::real(p)<RealScalar(0))
202  u = -u;
203 
204  m_c = Scalar(1)/u;
205  m_s = -qs*conj(ps)*(m_c/p2);
206  if(r) *r = p * u;
207  }
208  else
209  {
210  Scalar ps = p / q1;
211  RealScalar p2 = numext::abs2(ps);
212  Scalar qs = q / q1;
213  RealScalar q2 = numext::abs2(qs);
214 
215  RealScalar u = q1 * sqrt(p2 + q2);
216  if(numext::real(p)<RealScalar(0))
217  u = -u;
218 
219  p1 = abs(p);
220  ps = p/p1;
221  m_c = p1/u;
222  m_s = -conj(ps) * (q/u);
223  if(r) *r = ps * u;
224  }
225  }
226 }
227 
228 // specialization for reals
229 template<typename Scalar>
232 {
233  using std::sqrt;
234  using std::abs;
235  if(q==Scalar(0))
236  {
237  m_c = p<Scalar(0) ? Scalar(-1) : Scalar(1);
238  m_s = Scalar(0);
239  if(r) *r = abs(p);
240  }
241  else if(p==Scalar(0))
242  {
243  m_c = Scalar(0);
244  m_s = q<Scalar(0) ? Scalar(1) : Scalar(-1);
245  if(r) *r = abs(q);
246  }
247  else if(abs(p) > abs(q))
248  {
249  Scalar t = q/p;
250  Scalar u = sqrt(Scalar(1) + numext::abs2(t));
251  if(p<Scalar(0))
252  u = -u;
253  m_c = Scalar(1)/u;
254  m_s = -t * m_c;
255  if(r) *r = p * u;
256  }
257  else
258  {
259  Scalar t = p/q;
260  Scalar u = sqrt(Scalar(1) + numext::abs2(t));
261  if(q<Scalar(0))
262  u = -u;
263  m_s = -Scalar(1)/u;
264  m_c = -t * m_s;
265  if(r) *r = q * u;
266  }
267 
268 }
269 
270 /****************************************************************************************
271 * Implementation of MatrixBase methods
272 ****************************************************************************************/
273 
274 namespace internal {
281 template<typename VectorX, typename VectorY, typename OtherScalar>
284 }
285 
292 template<typename Derived>
293 template<typename OtherScalar>
296 {
297  RowXpr x(this->row(p));
298  RowXpr y(this->row(q));
300 }
301 
308 template<typename Derived>
309 template<typename OtherScalar>
312 {
313  ColXpr x(this->col(p));
314  ColXpr y(this->col(q));
315  internal::apply_rotation_in_the_plane(x, y, j.transpose());
316 }
317 
318 namespace internal {
319 
320 template<typename Scalar, typename OtherScalar,
321  int SizeAtCompileTime, int MinAlignment, bool Vectorizable>
323 {
324  static EIGEN_DEVICE_FUNC
325  inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
326  {
327  for(Index i=0; i<size; ++i)
328  {
329  Scalar xi = *x;
330  Scalar yi = *y;
331  *x = c * xi + numext::conj(s) * yi;
332  *y = -s * xi + numext::conj(c) * yi;
333  x += incrx;
334  y += incry;
335  }
336  }
337 };
338 
339 template<typename Scalar, typename OtherScalar,
340  int SizeAtCompileTime, int MinAlignment>
341 struct apply_rotation_in_the_plane_selector<Scalar,OtherScalar,SizeAtCompileTime,MinAlignment,true /* vectorizable */>
342 {
343  static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
344  {
345  enum {
346  PacketSize = packet_traits<Scalar>::size,
347  OtherPacketSize = packet_traits<OtherScalar>::size
348  };
349  typedef typename packet_traits<Scalar>::type Packet;
350  typedef typename packet_traits<OtherScalar>::type OtherPacket;
351 
352  /*** dynamic-size vectorized paths ***/
353  if(SizeAtCompileTime == Dynamic && ((incrx==1 && incry==1) || PacketSize == 1))
354  {
355  // both vectors are sequentially stored in memory => vectorization
356  enum { Peeling = 2 };
357 
358  Index alignedStart = internal::first_default_aligned(y, size);
359  Index alignedEnd = alignedStart + ((size-alignedStart)/PacketSize)*PacketSize;
360 
361  const OtherPacket pc = pset1<OtherPacket>(c);
362  const OtherPacket ps = pset1<OtherPacket>(s);
365 
366  for(Index i=0; i<alignedStart; ++i)
367  {
368  Scalar xi = x[i];
369  Scalar yi = y[i];
370  x[i] = c * xi + numext::conj(s) * yi;
371  y[i] = -s * xi + numext::conj(c) * yi;
372  }
373 
374  Scalar* EIGEN_RESTRICT px = x + alignedStart;
375  Scalar* EIGEN_RESTRICT py = y + alignedStart;
376 
377  if(internal::first_default_aligned(x, size)==alignedStart)
378  {
379  for(Index i=alignedStart; i<alignedEnd; i+=PacketSize)
380  {
381  Packet xi = pload<Packet>(px);
382  Packet yi = pload<Packet>(py);
383  pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
384  pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
385  px += PacketSize;
386  py += PacketSize;
387  }
388  }
389  else
390  {
391  Index peelingEnd = alignedStart + ((size-alignedStart)/(Peeling*PacketSize))*(Peeling*PacketSize);
392  for(Index i=alignedStart; i<peelingEnd; i+=Peeling*PacketSize)
393  {
394  Packet xi = ploadu<Packet>(px);
395  Packet xi1 = ploadu<Packet>(px+PacketSize);
396  Packet yi = pload <Packet>(py);
397  Packet yi1 = pload <Packet>(py+PacketSize);
398  pstoreu(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
399  pstoreu(px+PacketSize, padd(pm.pmul(pc,xi1),pcj.pmul(ps,yi1)));
400  pstore (py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
401  pstore (py+PacketSize, psub(pcj.pmul(pc,yi1),pm.pmul(ps,xi1)));
402  px += Peeling*PacketSize;
403  py += Peeling*PacketSize;
404  }
405  if(alignedEnd!=peelingEnd)
406  {
407  Packet xi = ploadu<Packet>(x+peelingEnd);
408  Packet yi = pload <Packet>(y+peelingEnd);
409  pstoreu(x+peelingEnd, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
410  pstore (y+peelingEnd, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
411  }
412  }
413 
414  for(Index i=alignedEnd; i<size; ++i)
415  {
416  Scalar xi = x[i];
417  Scalar yi = y[i];
418  x[i] = c * xi + numext::conj(s) * yi;
419  y[i] = -s * xi + numext::conj(c) * yi;
420  }
421  }
422 
423  /*** fixed-size vectorized path ***/
424  else if(SizeAtCompileTime != Dynamic && MinAlignment>0) // FIXME should be compared to the required alignment
425  {
426  const OtherPacket pc = pset1<OtherPacket>(c);
427  const OtherPacket ps = pset1<OtherPacket>(s);
430  Scalar* EIGEN_RESTRICT px = x;
431  Scalar* EIGEN_RESTRICT py = y;
432  for(Index i=0; i<size; i+=PacketSize)
433  {
434  Packet xi = pload<Packet>(px);
435  Packet yi = pload<Packet>(py);
436  pstore(px, padd(pm.pmul(pc,xi),pcj.pmul(ps,yi)));
437  pstore(py, psub(pcj.pmul(pc,yi),pm.pmul(ps,xi)));
438  px += PacketSize;
439  py += PacketSize;
440  }
441  }
442 
443  /*** non-vectorized path ***/
444  else
445  {
447  }
448  }
449 };
450 
451 template<typename VectorX, typename VectorY, typename OtherScalar>
454 {
455  typedef typename VectorX::Scalar Scalar;
456  const bool Vectorizable = (int(VectorX::Flags) & int(VectorY::Flags) & PacketAccessBit)
458 
459  eigen_assert(xpr_x.size() == xpr_y.size());
460  Index size = xpr_x.size();
461  Index incrx = xpr_x.derived().innerStride();
462  Index incry = xpr_y.derived().innerStride();
463 
464  Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0);
465  Scalar* EIGEN_RESTRICT y = &xpr_y.derived().coeffRef(0);
466 
467  OtherScalar c = j.c();
468  OtherScalar s = j.s();
469  if (c==OtherScalar(1) && s==OtherScalar(0))
470  return;
471 
473  Scalar,OtherScalar,
474  VectorX::SizeAtCompileTime,
476  Vectorizable>::run(x,incrx,y,incry,size,c,s);
477 }
478 
479 } // end namespace internal
480 
481 } // end namespace Eigen
482 
483 #endif // EIGEN_JACOBI_H
Matrix3f m
EIGEN_DEVICE_FUNC JacobiRotation()
Definition: Jacobi.h:41
internal::packet_traits< Scalar >::type Packet
SCALAR Scalar
Definition: bench_gemm.cpp:46
const AutoDiffScalar< DerType > & conj(const AutoDiffScalar< DerType > &x)
float real
Definition: datatypes.h:10
Scalar * y
void applyOnTheLeft(const EigenBase< OtherDerived > &other)
Definition: MatrixBase.h:540
Vector3f p1
EIGEN_DEVICE_FUNC Scalar & c()
Definition: Jacobi.h:47
#define min(a, b)
Definition: datatypes.h:19
int RealScalar int RealScalar int RealScalar * pc
EIGEN_DEVICE_FUNC void apply_rotation_in_the_plane(DenseBase< VectorX > &xpr_x, DenseBase< VectorY > &xpr_y, const JacobiRotation< OtherScalar > &j)
Definition: Jacobi.h:453
static void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
Definition: Jacobi.h:343
int n
void applyOnTheRight(const EigenBase< OtherDerived > &other)
Definition: MatrixBase.h:528
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Rotation given by a cosine-sine pair.
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:232
Base class for all dense matrices, vectors, and arrays.
Definition: DenseBase.h:41
const unsigned int PacketAccessBit
Definition: Constants.h:94
int RealScalar int RealScalar int RealScalar RealScalar * ps
AnnoyingScalar conj(const AnnoyingScalar &x)
static Index first_default_aligned(const DenseBase< Derived > &m)
EIGEN_DEVICE_FUNC Packet padd(const Packet &a, const Packet &b)
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
EIGEN_DEVICE_FUNC JacobiRotation(const Scalar &c, const Scalar &s)
Definition: Jacobi.h:45
EIGEN_DEVICE_FUNC void pstoreu(Scalar *to, const Packet &from)
EIGEN_DEVICE_FUNC JacobiRotation operator*(const JacobiRotation &other)
Definition: Jacobi.h:54
m row(1)
EIGEN_DEVICE_FUNC void makeGivens(const Scalar &p, const Scalar &q, Scalar *r=0)
Definition: Jacobi.h:162
int RealScalar int RealScalar * py
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
#define eigen_assert(x)
Definition: Macros.h:1037
RealScalar RealScalar * px
EIGEN_DEVICE_FUNC Scalar s() const
Definition: Jacobi.h:50
EIGEN_DEVICE_FUNC const Scalar & q
#define EIGEN_RESTRICT
Definition: Macros.h:1160
EIGEN_DEVICE_FUNC bool makeJacobi(const MatrixBase< Derived > &, Index p, Index q)
Definition: Jacobi.h:139
RowVector3d w
EIGEN_DEVICE_FUNC JacobiRotation adjoint() const
Definition: Jacobi.h:67
EIGEN_DEVICE_FUNC void pstore(Scalar *to, const Packet &from)
Vector xi
Definition: testPose2.cpp:148
#define EIGEN_DEVICE_FUNC
Definition: Macros.h:976
EIGEN_DEVICE_FUNC Scalar & s()
Definition: Jacobi.h:49
NumTraits< Scalar >::Real RealScalar
Definition: Jacobi.h:37
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
static EIGEN_DEVICE_FUNC void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
Definition: Jacobi.h:325
#define EIGEN_PLAIN_ENUM_MIN(a, b)
Definition: Macros.h:1288
EIGEN_DEVICE_FUNC JacobiRotation transpose() const
Definition: Jacobi.h:63
float * p
EIGEN_DEVICE_FUNC Packet psub(const Packet &a, const Packet &b)
m col(1)
static Point3 p2
EIGEN_DEVICE_FUNC Scalar c() const
Definition: Jacobi.h:48
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const int Dynamic
Definition: Constants.h:22
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType pmul(const LhsType &x, const RhsType &y) const
Definition: ConjHelper.h:71
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
#define abs(x)
Definition: datatypes.h:17
EIGEN_DEVICE_FUNC bool abs2(bool x)
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
std::ptrdiff_t j
Point2 t(10, 10)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:27