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::print(const std::string &s) const {
47  std::cout << s << std::endl;
48  std::ostringstream os;
49  print(os);
50  std::cout << os.str() << std::endl;
51 }
52 
53 /*****************************************************************************/
55  const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
56  const VectorValues &initial) {
57  /* build preconditioner */
58  preconditioner_->build(gfg, keyInfo, lambda);
59 
60  /* apply pcg */
61  GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda);
62  Vector x0 = initial.vector(keyInfo.ordering());
63  const Vector sol = preconditionedConjugateGradient(system, x0, parameters_);
64 
65  return buildVectorValues(sol, keyInfo);
66 }
67 
68 /*****************************************************************************/
69 GaussianFactorGraphSystem::GaussianFactorGraphSystem(
70  const GaussianFactorGraph &gfg, const Preconditioner &preconditioner,
71  const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) :
72  gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(
73  lambda) {
74 }
75 
76 /*****************************************************************************/
78  /* implement b-Ax, assume x and r are pre-allocated */
79 
80  /* reset r to b */
81  getb(r);
82 
83  /* substract A*x */
84  Vector Ax = Vector::Zero(r.rows(), 1);
85  multiply(x, Ax);
86  r -= Ax;
87 }
88 
89 /*****************************************************************************/
91  /* implement A^T*(A*x), assume x and AtAx are pre-allocated */
92 
93  // Build a VectorValues for Vector x
95 
96  // VectorValues form of A'Ax for multiplyHessianAdd
97  VectorValues vvAtAx = keyInfo_.x0(); // crucial for performance
98 
99  // vvAtAx += 1.0 * A'Ax for each factor
100  gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx);
101 
102  // Make the result as Vector form
103  AtAx = vvAtAx.vector(keyInfo_.ordering());
104 }
105 
106 /*****************************************************************************/
108  /* compute rhs, assume b pre-allocated */
109 
110  // Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues
112 
113  // Make the result as Vector form
114  b = -vvb.vector(keyInfo_.ordering());
115 }
116 
117 /**********************************************************************************/
119  Vector &y) const {
120  // For a preconditioner M = L*L^T
121  // Calculate y = L^{-1} x
123 }
124 
125 /**********************************************************************************/
127  Vector &y) const {
128  // For a preconditioner M = L*L^T
129  // Calculate y = L^{-T} x
131 }
132 
133 /**********************************************************************************/
134 void GaussianFactorGraphSystem::scal(const double alpha, Vector &x) const {
135  x *= alpha;
136 }
137 double GaussianFactorGraphSystem::dot(const Vector &x, const Vector &y) const {
138  return x.dot(y);
139 }
140 void GaussianFactorGraphSystem::axpy(const double alpha, const Vector &x,
141  Vector &y) const {
142  y += alpha * x;
143 }
144 /**********************************************************************************/
146  const map<Key, size_t> & dimensions) {
148 
149  DenseIndex offset = 0;
150  for (size_t i = 0; i < ordering.size(); ++i) {
151  const Key key = ordering[i];
152  map<Key, size_t>::const_iterator it = dimensions.find(key);
153  if (it == dimensions.end()) {
154  throw invalid_argument(
155  "buildVectorValues: inconsistent ordering and dimensions");
156  }
157  const size_t dim = it->second;
158  result.emplace(key, v.segment(offset, dim));
159  offset += dim;
160  }
161 
162  return result;
163 }
164 
165 /**********************************************************************************/
166 VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
168  for ( const KeyInfo::value_type &item: keyInfo ) {
169  result.emplace(item.first, v.segment(item.second.start, item.second.dim));
170  }
171  return result;
172 }
173 
174 }
gtsam::GaussianFactorGraphSystem::rightPrecondition
void rightPrecondition(const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:126
gtsam::KeyInfo::ordering
const Ordering & ordering() const
Return the ordering.
Definition: IterativeSolver.h:159
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.
dimensions
const std::vector< size_t > dimensions
Definition: testVerticalBlockMatrix.cpp:27
gtsam::GaussianFactorGraphSystem::residual
void residual(const Vector &x, Vector &r) const
Definition: PCGSolver.cpp:77
gtsam::KeyInfo::x0
VectorValues x0() const
Return VectorValues with zeros, of correct dimension.
Definition: IterativeSolver.cpp:144
gtsam::GaussianFactorGraph::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const
‍** y += alpha*A'A*x *‍/
Definition: GaussianFactorGraph.cpp:420
gtsam::GaussianFactorGraphSystem::keyInfo_
KeyInfo keyInfo_
Definition: PCGSolver.h:85
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::GaussianFactorGraphSystem::getb
void getb(Vector &b) const
Definition: PCGSolver.cpp:107
optimize
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
Definition: timeSFMBAL.h:64
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::GaussianFactorGraphSystem::dot
double dot(const Vector &x, const Vector &y) const
Definition: PCGSolver.cpp:137
gtsam::GaussianFactorGraphSystem::multiply
void multiply(const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:90
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::buildVectorValues
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo)
Create VectorValues from a Vector and a KeyInfo class.
Definition: PCGSolver.cpp:166
gtsam::KeyInfo
Definition: IterativeSolver.h:127
gtsam::PCGSolverParameters
Definition: PCGSolver.h:36
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::PCGSolverParameters::print
void print(const std::string &s) const
Definition: PCGSolver.cpp:46
gtsam::GaussianFactorGraph::gradientAtZero
virtual VectorValues gradientAtZero() const
Definition: GaussianFactorGraph.cpp:369
PCGSolver.h
gtsam::preconditionedConjugateGradient
V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters)
Definition: ConjugateGradientSolver.h:107
x0
static Symbol x0('x', 0)
gtsam::Preconditioner::solve
virtual void solve(const Vector &y, Vector &x) const =0
implement x = L^{-1} y
VectorValues.h
Factor Graph Values.
gtsam::GaussianFactorGraphSystem::gfg_
const GaussianFactorGraph & gfg_
Definition: PCGSolver.h:83
gtsam::Preconditioner::transposeSolve
virtual void transposeSolve(const Vector &y, Vector &x) const =0
implement x = L^{-T} y
lambda
static double lambda[]
Definition: jv.c:524
ordering
static enum @1096 ordering
y
Scalar * y
Definition: level1_cplx_impl.h:124
offset
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: gnuplot_common_settings.hh:64
key
const gtsam::Symbol key('X', 0)
gtsam::b
const G & b
Definition: Group.h:79
gtsam::GaussianFactorGraphSystem::leftPrecondition
void leftPrecondition(const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:118
gtsam
traits
Definition: SFMdata.h:40
gtsam::GaussianFactorGraphSystem
Definition: PCGSolver.h:82
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
std
Definition: BFloat16.h:88
gtsam::GaussianFactorGraphSystem::axpy
void axpy(const double alpha, const Vector &x, Vector &y) const
Definition: PCGSolver.cpp:140
p
float * p
Definition: Tutorial_Map_using.cpp:9
Preconditioner.h
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
initial
Definition: testScenarioRunner.cpp:148
gtsam::buildVectorValues
VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const map< Key, size_t > &dimensions)
Create VectorValues from a Vector.
Definition: PCGSolver.cpp:145
gtsam::GaussianFactorGraphSystem::preconditioner_
const Preconditioner & preconditioner_
Definition: PCGSolver.h:84
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::GaussianFactorGraphSystem::scal
void scal(const double alpha, Vector &x) const
Definition: PCGSolver.cpp:134
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::createPreconditioner
std::shared_ptr< Preconditioner > createPreconditioner(const std::shared_ptr< PreconditionerParameters > params)
Definition: Preconditioner.cpp:189
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:173


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:19