|
1 | 1 | /* |
2 | | - * Copyright 2017, Justin Carpentier, LAAS-CNRS |
| 2 | + * Copyright 2017-2018, Justin Carpentier, LAAS-CNRS |
3 | 3 | * |
4 | 4 | * This file is part of eigenpy. |
5 | 5 | * eigenpy is free software: you can redistribute it and/or |
|
22 | 22 |
|
23 | 23 | #include "eigenpy/solvers/IterativeSolverBase.hpp" |
24 | 24 |
|
25 | | -namespace Eigen |
26 | | -{ |
27 | | - template <typename _Scalar> |
28 | | - class LeastSquareDiagonalPreconditionerFix |
29 | | - : public LeastSquareDiagonalPreconditioner<_Scalar> |
30 | | - { |
31 | | - typedef _Scalar Scalar; |
32 | | - typedef typename NumTraits<Scalar>::Real RealScalar; |
33 | | - typedef LeastSquareDiagonalPreconditioner<_Scalar> Base; |
34 | | - using DiagonalPreconditioner<_Scalar>::m_invdiag; |
35 | | - public: |
36 | | - |
37 | | - LeastSquareDiagonalPreconditionerFix() : Base() {} |
38 | | - |
39 | | - template<typename MatType> |
40 | | - explicit LeastSquareDiagonalPreconditionerFix(const MatType& mat) : Base() |
41 | | - { |
42 | | - compute(mat); |
43 | | - } |
44 | | - |
45 | | - template<typename MatType> |
46 | | - LeastSquareDiagonalPreconditionerFix& analyzePattern(const MatType& ) |
47 | | - { |
48 | | - return *this; |
49 | | - } |
50 | | - |
51 | | - template<typename MatType> |
52 | | - LeastSquareDiagonalPreconditionerFix& factorize(const MatType& mat) |
53 | | - { |
54 | | - // Compute the inverse squared-norm of each column of mat |
55 | | - m_invdiag.resize(mat.cols()); |
56 | | - if(MatType::IsRowMajor) |
57 | | - { |
58 | | - m_invdiag.setZero(); |
59 | | - for(Index j=0; j<mat.outerSize(); ++j) |
60 | | - { |
61 | | - for(typename MatType::InnerIterator it(mat,j); it; ++it) |
62 | | - m_invdiag(it.index()) += numext::abs2(it.value()); |
63 | | - } |
64 | | - for(Index j=0; j<mat.cols(); ++j) |
65 | | - if(numext::real(m_invdiag(j))>RealScalar(0)) |
66 | | - m_invdiag(j) = RealScalar(1)/numext::real(m_invdiag(j)); |
67 | | - } |
68 | | - else |
69 | | - { |
70 | | - for(Index j=0; j<mat.outerSize(); ++j) |
71 | | - { |
72 | | - RealScalar sum = mat.col(j).squaredNorm(); |
73 | | - if(sum>RealScalar(0)) |
74 | | - m_invdiag(j) = RealScalar(1)/sum; |
75 | | - else |
76 | | - m_invdiag(j) = RealScalar(1); |
77 | | - } |
78 | | - } |
79 | | - Base::m_isInitialized = true; |
80 | | - return *this; |
81 | | - } |
82 | | - |
83 | | - }; |
84 | | -} |
85 | | - |
86 | 25 | namespace eigenpy |
87 | 26 | { |
88 | 27 |
|
|
0 commit comments