Preconditioner.cpp
Go to the documentation of this file.
1 /*
2  * Preconditioner.cpp
3  *
4  * Created on: Jun 2, 2014
5  * Author: Yong-Dian Jian
6  * Author: Sungtae An
7  */
8 
11 #include <gtsam/linear/PCGSolver.h>
15 #include <memory>
16 #include <iostream>
17 #include <vector>
18 
19 using namespace std;
20 
21 namespace gtsam {
22 
23 /*****************************************************************************/
25  print(cout);
26 }
27 
28 /***************************************************************************************/
29 void PreconditionerParameters::print(ostream &os) const {
30  os << "PreconditionerParameters" << endl
31  << "kernel: " << kernelTranslator(kernel_) << endl
32  << "verbosity: " << verbosityTranslator(verbosity_) << endl;
33 }
34 
35 /*****************************************************************************/
36  ostream& operator<<(ostream &os, const PreconditionerParameters &p) {
37  p.print(os);
38  return os;
39 }
40 
41 /***************************************************************************************/
42 PreconditionerParameters::Kernel PreconditionerParameters::kernelTranslator(const std::string &src) {
43  std::string s = src;
44  // convert string s to upper case
45  std::transform(s.begin(), s.end(), s.begin(), ::toupper);
46  if (s == "GTSAM") return PreconditionerParameters::GTSAM;
47  else if (s == "CHOLMOD") return PreconditionerParameters::CHOLMOD;
48  /* default is cholmod */
49  else return PreconditionerParameters::CHOLMOD;
50 }
51 
52 /***************************************************************************************/
53 PreconditionerParameters::Verbosity PreconditionerParameters::verbosityTranslator(const std::string &src) {
54  std::string s = src;
55  // convert string to upper case
56  std::transform(s.begin(), s.end(), s.begin(), ::toupper);
57  if (s == "SILENT") return PreconditionerParameters::SILENT;
58  else if (s == "COMPLEXITY") return PreconditionerParameters::COMPLEXITY;
59  else if (s == "ERROR") return PreconditionerParameters::ERROR;
60  /* default is default */
61  else return PreconditionerParameters::SILENT;
62 }
63 
64 /***************************************************************************************/
65 std::string PreconditionerParameters::kernelTranslator(PreconditionerParameters::Kernel k) {
66  if ( k == GTSAM ) return "GTSAM";
67  if ( k == CHOLMOD ) return "CHOLMOD";
68  else return "UNKNOWN";
69 }
70 
71 /***************************************************************************************/
72 std::string PreconditionerParameters::verbosityTranslator(PreconditionerParameters::Verbosity verbosity) {
73  if (verbosity == SILENT) return "SILENT";
74  else if (verbosity == COMPLEXITY) return "COMPLEXITY";
75  else if (verbosity == ERROR) return "ERROR";
76  else return "UNKNOWN";
77 }
78 
79 /***************************************************************************************/
80 BlockJacobiPreconditioner::BlockJacobiPreconditioner()
81  : Base(), buffer_(0), bufferSize_(0), nnz_(0) {}
82 
83 /***************************************************************************************/
85 
86 /***************************************************************************************/
88 
89  const size_t n = dims_.size();
90  double *ptr = buffer_, *dst = x.data();
91 
92  std::copy(y.data(), y.data() + y.rows(), x.data());
93 
94  for ( size_t i = 0 ; i < n ; ++i ) {
95  const size_t d = dims_[i];
96  const size_t sz = d*d;
97 
98  const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
99  Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
100  R.triangularView<Eigen::Lower>().solveInPlace(b);
101 
102  dst += d;
103  ptr += sz;
104  }
105 }
106 
107 /***************************************************************************************/
109  const size_t n = dims_.size();
110  double *ptr = buffer_, *dst = x.data();
111 
112  std::copy(y.data(), y.data() + y.rows(), x.data());
113 
114  for ( size_t i = 0 ; i < n ; ++i ) {
115  const size_t d = dims_[i];
116  const size_t sz = d*d;
117 
118  const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
119  Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
120  R.transpose().triangularView<Eigen::Upper>().solveInPlace(b);
121 
122  dst += d;
123  ptr += sz;
124  }
125 }
126 
127 /***************************************************************************************/
129  const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
130 {
131  // n is the number of keys
132  const size_t n = keyInfo.size();
133  // dims_ is a vector that contains the dimension of keys
134  dims_ = keyInfo.colSpec();
135 
136  /* prepare the buffer of block diagonals */
137  std::vector<Matrix> blocks; blocks.reserve(n);
138 
139  /* allocate memory for the factorization of block diagonals */
140  size_t nnz = 0;
141  for ( size_t i = 0 ; i < n ; ++i ) {
142  const size_t dim = dims_[i];
143  // blocks.push_back(Matrix::Zero(dim, dim));
144  // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ;
145  nnz += dim*dim;
146  }
147 
148  /* getting the block diagonals over the factors */
149  std::map<Key, Matrix> hessianMap =gfg.hessianBlockDiagonal();
150  for (const auto& [key, hessian]: hessianMap) {
151  blocks.push_back(hessian);
152  }
153 
154  /* if necessary, allocating the memory for cacheing the factorization results */
155  if ( nnz > bufferSize_ ) {
156  clean();
157  buffer_ = new double [nnz];
158  bufferSize_ = nnz;
159  }
160  nnz_ = nnz;
161 
162  /* factorizing the blocks respectively */
163  double *ptr = buffer_;
164  for ( size_t i = 0 ; i < n ; ++i ) {
165  /* use eigen to decompose Di */
166  /* It is same as L = chol(M,'lower') in MATLAB where M is full preconditioner */
167  const Matrix L = blocks[i].llt().matrixL();
168 
169  /* store the data in the buffer */
170  size_t sz = dims_[i]*dims_[i] ;
171  std::copy(L.data(), L.data() + sz, ptr);
172 
173  /* advance the pointer */
174  ptr += sz;
175  }
176 }
177 
178 /*****************************************************************************/
180  if ( buffer_ ) {
181  delete [] buffer_;
182  buffer_ = 0;
183  bufferSize_ = 0;
184  nnz_ = 0;
185  }
186 }
187 
188 /***************************************************************************************/
189 std::shared_ptr<Preconditioner> createPreconditioner(
190  const std::shared_ptr<PreconditionerParameters> params) {
191  using std::dynamic_pointer_cast;
192  if (dynamic_pointer_cast<DummyPreconditionerParameters>(params)) {
193  return std::make_shared<DummyPreconditioner>();
194  } else if (dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(
195  params)) {
196  return std::make_shared<BlockJacobiPreconditioner>();
197  } else if (auto subgraph =
198  dynamic_pointer_cast<SubgraphPreconditionerParameters>(
199  params)) {
200  return std::make_shared<SubgraphPreconditioner>(*subgraph);
201  }
202 
203  throw invalid_argument(
204  "createPreconditioner: unexpected preconditioner parameter type");
205 }
206 } // namespace gtsam
const gtsam::Symbol key('X', 0)
Scalar * y
void transposeSolve(const Vector &y, Vector &x) const override
implement x = L^{-T} y
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Rot2 R(Rot2::fromAngle(0.1))
MatrixXd L
Definition: LLT_example.cpp:6
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
static const SmartProjectionParams params
Eigen::VectorXd Vector
Definition: Vector.h:38
virtual std::map< Key, Matrix > hessianBlockDiagonal() const
RealScalar s
Linear Factor Graph where all factors are Gaussians.
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
const G & b
Definition: Group.h:86
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
traits
Definition: chartTesting.h:28
ofstream os("timeSchurFactors.csv")
float * p
ostream & operator<<(ostream &os, const PreconditionerParameters &p)
void solve(const Vector &y, Vector &x) const override
implement x = L^{-1} y
std::vector< size_t > colSpec() const
Return a vector of dimensions ordered by ordering()
Factor Graph Base Class.
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
std::shared_ptr< Preconditioner > createPreconditioner(const std::shared_ptr< PreconditionerParameters > params)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:15