PCGSolver.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
12 /*
13  * @file PCGSolver.cpp
14  * @brief Preconditioned Conjugate Gradient Solver for linear systems
15  * @date Feb 14, 2012
16  * @author Yong-Dian Jian
17  * @author Sungtae An
18  */
19 
20 #include <gtsam/linear/PCGSolver.h>
24 
25 #include <algorithm>
26 #include <iostream>
27 #include <stdexcept>
28 
29 using namespace std;
30 
31 namespace gtsam {
32 
33 /*****************************************************************************/
34 void PCGSolverParameters::print(ostream &os) const {
35  Base::print(os);
36  os << "PCGSolverParameters:" << endl;
37  preconditioner_->print(os);
38 }
39 
40 /*****************************************************************************/
41 PCGSolver::PCGSolver(const PCGSolverParameters &p) {
42  parameters_ = p;
43  preconditioner_ = createPreconditioner(p.preconditioner_);
44 }
45 
46 void PCGSolverParameters::setPreconditionerParams(const std::shared_ptr<PreconditionerParameters> preconditioner) {
47  preconditioner_ = preconditioner;
48 }
49 
50 void PCGSolverParameters::print(const std::string &s) const {
51  std::cout << s << std::endl;
52  std::ostringstream os;
53  print(os);
54  std::cout << os.str() << std::endl;
55 }
56 
57 /*****************************************************************************/
59  const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
60  const VectorValues &initial) {
61  /* build preconditioner */
62  preconditioner_->build(gfg, keyInfo, lambda);
63 
64  /* apply pcg */
65  GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda);
66  Vector x0 = initial.vector(keyInfo.ordering());
67  const Vector sol = preconditionedConjugateGradient(system, x0, parameters_);
68 
69  return buildVectorValues(sol, keyInfo);
70 }
71 
72 /*****************************************************************************/
73 GaussianFactorGraphSystem::GaussianFactorGraphSystem(
74  const GaussianFactorGraph &gfg, const Preconditioner &preconditioner,
75  const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) :
76  gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(
77  lambda) {
78 }
79 
80 /*****************************************************************************/
82  /* implement b-Ax, assume x and r are pre-allocated */
83 
84  /* reset r to b */
85  getb(r);
86 
87  /* substract A*x */
88  Vector Ax = Vector::Zero(r.rows(), 1);
89  multiply(x, Ax);
90  r -= Ax;
91 }
92 
93 /*****************************************************************************/
95  /* implement A^T*(A*x), assume x and AtAx are pre-allocated */
96 
97  // Build a VectorValues for Vector x
99 
100  // VectorValues form of A'Ax for multiplyHessianAdd
101  VectorValues vvAtAx = keyInfo_.x0(); // crucial for performance
102 
103  // vvAtAx += 1.0 * A'Ax for each factor
104  gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
105 
106  // Make the result as Vector form
107  AtAx = vvAtAx.vector(keyInfo_.ordering());
108 }
109 
110 /*****************************************************************************/
112  /* compute rhs, assume b pre-allocated */
113 
114  // Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues
116 
117  // Make the result as Vector form
118  b = -vvb.vector(keyInfo_.ordering());
119 }
120 
121 /**********************************************************************************/
123  Vector &y) const {
124  // For a preconditioner M = L*L^T
125  // Calculate y = L^{-1} x
126  preconditioner_.solve(x, y);
127 }
128 
129 /**********************************************************************************/
131  Vector &y) const {
132  // For a preconditioner M = L*L^T
133  // Calculate y = L^{-T} x
135 }
136 
137 /**********************************************************************************/
138 void GaussianFactorGraphSystem::scal(const double alpha, Vector &x) const {
139  x *= alpha;
140 }
141 double GaussianFactorGraphSystem::dot(const Vector &x, const Vector &y) const {
142  return x.dot(y);
143 }
144 void GaussianFactorGraphSystem::axpy(const double alpha, const Vector &x,
145  Vector &y) const {
146  y += alpha * x;
147 }
148 /**********************************************************************************/
150  const map<Key, size_t> & dimensions) {
152 
153  DenseIndex offset = 0;
154  for (size_t i = 0; i < ordering.size(); ++i) {
155  const Key key = ordering[i];
156  map<Key, size_t>::const_iterator it = dimensions.find(key);
157  if (it == dimensions.end()) {
158  throw invalid_argument(
159  "buildVectorValues: inconsistent ordering and dimensions");
160  }
161  const size_t dim = it->second;
162  result.emplace(key, v.segment(offset, dim));
163  offset += dim;
164  }
165 
166  return result;
167 }
168 
169 /**********************************************************************************/
170 VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
172  for ( const KeyInfo::value_type &item: keyInfo ) {
173  result.emplace(item.first, v.segment(item.second.start, item.second.dim));
174  }
175  return result;
176 }
177 
178 }
const gtsam::Symbol key('X', 0)
void rightPrecondition(const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:130
double dot(const Vector &x, const Vector &y) const
Definition: PCGSolver.cpp:141
Scalar * y
void multiply(const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:94
const Preconditioner & preconditioner_
Definition: PCGSolver.h:97
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
void scal(const double alpha, Vector &x) const
Definition: PCGSolver.cpp:138
static enum @1107 ordering
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo)
Create VectorValues from a Vector and a KeyInfo class.
Definition: PCGSolver.cpp:170
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Definition: VectorValues.h:185
Factor Graph Values.
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters)
virtual void solve(const Vector &y, Vector &x) const =0
implement x = L^{-1} y
virtual void transposeSolve(const Vector &y, Vector &x) const =0
implement x = L^{-T} y
Array< int, Dynamic, 1 > v
RealScalar alpha
RealScalar s
void leftPrecondition(const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:122
const Ordering & ordering() const
Return the ordering.
Linear Factor Graph where all factors are Gaussians.
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const
const G & b
Definition: Group.h:86
std::shared_ptr< PreconditionerParameters > preconditioner_
Definition: PCGSolver.h:54
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
static Symbol x0('x', 0)
ofstream os("timeSchurFactors.csv")
void axpy(const double alpha, const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:144
float * p
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
VectorValues x0() const
Return VectorValues with zeros, of correct dimension.
Vector vector() const
const GaussianFactorGraph & gfg_
Definition: PCGSolver.h:96
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
const std::vector< size_t > dimensions
virtual VectorValues gradientAtZero() const
std::shared_ptr< Preconditioner > createPreconditioner(const std::shared_ptr< PreconditionerParameters > params)
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
void residual(const Vector &x, Vector &r) const
Definition: PCGSolver.cpp:81
VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const map< Key, size_t > &dimensions)
Create VectorValues from a Vector.
Definition: PCGSolver.cpp:149
void getb(Vector &b) const
Definition: PCGSolver.cpp:111


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