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 
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);
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
gtsam::Preconditioner
Definition: Preconditioner.h:64
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::GaussianFactorGraph::hessianBlockDiagonal
virtual std::map< Key, Matrix > hessianBlockDiagonal() const
Definition: GaussianFactorGraph.cpp:290
s
RealScalar s
Definition: level1_cplx_impl.h:126
d
static const double d[K][N]
Definition: igam.h:11
gtsam::BlockJacobiPreconditioner::bufferSize_
size_t bufferSize_
Definition: Preconditioner.h:152
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::operator<<
ostream & operator<<(ostream &os, const PreconditionerParameters &p)
Definition: Preconditioner.cpp:36
gtsam::BlockJacobiPreconditioner::solve
void solve(const Vector &y, Vector &x) const override
implement x = L^{-1} y
Definition: Preconditioner.cpp:87
Eigen::Upper
@ Upper
Definition: Constants.h:211
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::BlockJacobiPreconditioner::build
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
Definition: Preconditioner.cpp:128
os
ofstream os("timeSchurFactors.csv")
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::PreconditionerParameters
Definition: Preconditioner.h:24
gtsam::KeyInfo
Definition: IterativeSolver.h:127
FactorGraph-inst.h
Factor Graph Base Class.
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::PreconditionerParameters::Verbosity
Verbosity
Definition: Preconditioner.h:33
gtsam::KeyInfo::colSpec
std::vector< size_t > colSpec() const
Return a vector of dimensions ordered by ordering()
Definition: IterativeSolver.cpp:135
PCGSolver.h
gtsam::BlockJacobiPreconditioner::nnz_
size_t nnz_
Definition: Preconditioner.h:153
L
MatrixXd L
Definition: LLT_example.cpp:6
gtsam::BlockJacobiPreconditioner::dims_
std::vector< size_t > dims_
Definition: Preconditioner.h:150
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
Eigen::Lower
@ Lower
Definition: Constants.h:209
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
gtsam::PreconditionerParameters::Kernel
Kernel
Definition: Preconditioner.h:28
lambda
static double lambda[]
Definition: jv.c:524
y
Scalar * y
Definition: level1_cplx_impl.h:124
key
const gtsam::Symbol key('X', 0)
gtsam::BlockJacobiPreconditioner::~BlockJacobiPreconditioner
~BlockJacobiPreconditioner() override
Definition: Preconditioner.cpp:84
gtsam::b
const G & b
Definition: Group.h:79
gtsam
traits
Definition: SFMdata.h:40
NoiseModel.h
std
Definition: BFloat16.h:88
SubgraphPreconditioner.h
p
float * p
Definition: Tutorial_Map_using.cpp:9
Preconditioner.h
gtsam::BlockJacobiPreconditioner::clean
void clean()
Definition: Preconditioner.cpp:179
gtsam::BlockJacobiPreconditioner::buffer_
double * buffer_
Definition: Preconditioner.h:151
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::createPreconditioner
std::shared_ptr< Preconditioner > createPreconditioner(const std::shared_ptr< PreconditionerParameters > params)
Definition: Preconditioner.cpp:189
gtsam::BlockJacobiPreconditioner::transposeSolve
void transposeSolve(const Vector &y, Vector &x) const override
implement x = L^{-T} y
Definition: Preconditioner.cpp:108


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:37