SubgraphPreconditioner.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, 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 
19 
24 #include <gtsam/base/types.h>
25 #include <gtsam/base/Vector.h>
26 
27 #include <stdexcept>
28 
29 using std::cout;
30 using std::endl;
31 using std::vector;
32 using std::ostream;
33 
34 namespace gtsam {
35 
36 /* ************************************************************************* */
37 static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo,
38  const KeyVector &keys) {
39  /* a cache of starting index and dim */
40  vector<std::pair<size_t, size_t> > cache;
41  cache.reserve(3);
42 
43  /* figure out dimension by traversing the keys */
44  size_t dim = 0;
45  for (const Key &key : keys) {
46  const KeyInfoEntry &entry = keyInfo.find(key)->second;
47  cache.emplace_back(entry.start, entry.dim);
48  dim += entry.dim;
49  }
50 
51  /* use the cache to fill the result */
52  Vector result(dim);
53  size_t index = 0;
54  for (const auto &p : cache) {
55  result.segment(index, p.second) = src.segment(p.first, p.second);
56  index += p.second;
57  }
58 
59  return result;
60 }
61 
62 /*****************************************************************************/
63 static void setSubvector(const Vector &src, const KeyInfo &keyInfo,
64  const KeyVector &keys, Vector &dst) {
65  size_t index = 0;
66  for (const Key &key : keys) {
67  const KeyInfoEntry &entry = keyInfo.find(key)->second;
68  const size_t keyDim = entry.dim;
69  dst.segment(entry.start, keyDim) = src.segment(index, keyDim);
70  index += keyDim;
71  }
72 }
73 
74 /* ************************************************************************* */
75 // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with
76 // Cholesky)
78  const GaussianFactorGraph &gfg) {
80  for (const auto &factor : gfg)
81  if (factor) {
82  auto jf = std::dynamic_pointer_cast<JacobianFactor>(factor);
83  if (!jf) {
84  jf = std::make_shared<JacobianFactor>(*factor);
85  }
86  result.push_back(jf);
87  }
88  return result;
89 }
90 
91 /* ************************************************************************* */
93  parameters_(p) {}
94 
95 /* ************************************************************************* */
98  Ab2_(convertToJacobianFactors(Ab2)), Rc1_(Rc1), xbar_(xbar),
99  b2bar_(-Ab2_.gaussianErrors(xbar)), parameters_(p) {
100 }
101 
102 /* ************************************************************************* */
103 // x = xbar + inv(R1)*y
105  return xbar_ + Rc1_.backSubstitute(y);
106 }
107 
108 /* ************************************************************************* */
110  Errors e = createErrors(y);
111  VectorValues x = this->x(y);
112  Errors e2 = Ab2_.gaussianErrors(x);
113  return 0.5 * (dot(e, e) + dot(e2,e2));
114 }
115 
116 /* ************************************************************************* */
117 // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
119  VectorValues x = Rc1_.backSubstitute(y); /* inv(R1)*y */
120  Errors e = Ab2_ * x - b2bar_; /* (A2*inv(R1)*y-b2bar) */
122  Ab2_.transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
123  return y + Rc1_.backSubstituteTranspose(v);
124 }
125 
126 /* ************************************************************************* */
127 // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
129  Errors e = createErrors(y);
130  VectorValues x = Rc1_.backSubstitute(y); /* x=inv(R1)*y */
131  Errors e2 = Ab2_ * x; /* A2*x */
132  e.splice(e.end(), e2);
133  return e;
134 }
135 
136 /* ************************************************************************* */
137 // In-place version that overwrites e
139 
140  Errors::iterator ei = e.begin();
141  for(const auto& key_value: y) {
142  *ei = key_value.second;
143  ++ei;
144  }
145 
146  // Add A2 contribution
147  VectorValues x = Rc1_.backSubstitute(y); // x=inv(R1)*y
148  Ab2_.multiplyInPlace(x, ei); // use iterator version
149 }
150 
151 /* ************************************************************************* */
152 // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
154 
155  Errors::const_iterator it = e.begin();
156  VectorValues y = zero();
157  for(auto& key_value: y) {
158  key_value.second = *it;
159  ++it;
160  }
161  transposeMultiplyAdd2(1.0, it, e.end(), y);
162  return y;
163 }
164 
165 /* ************************************************************************* */
166 // y += alpha*A'*e
168 (double alpha, const Errors& e, VectorValues& y) const {
169 
170  Errors::const_iterator it = e.begin();
171  for(auto& key_value: y) {
172  const Vector& ei = *it;
173  key_value.second += alpha * ei;
174  ++it;
175  }
176  transposeMultiplyAdd2(alpha, it, e.end(), y);
177 }
178 
179 /* ************************************************************************* */
180 // y += alpha*inv(R1')*A2'*e2
182  Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {
183 
184  // create e2 with what's left of e
185  // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
186  Errors e2;
187  while (it != end) e2.push_back(*(it++));
188 
189  VectorValues x = VectorValues::Zero(y); // x = 0
190  Ab2_.transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2
191  y += alpha * Rc1_.backSubstituteTranspose(x); // y += alpha*inv(R1')*x
192 }
193 
194 /* ************************************************************************* */
195 void SubgraphPreconditioner::print(const std::string& s) const {
196  cout << s << endl;
197  Ab2_.print();
198 }
199 
200 /*****************************************************************************/
202  assert(x.size() == y.size());
203 
204  /* back substitute */
205  for (auto it = std::make_reverse_iterator(Rc1_.end()); it != std::make_reverse_iterator(Rc1_.begin()); ++it) {
206  auto& cg = *it;
207  /* collect a subvector of x that consists of the parents of cg (S) */
208  const KeyVector parentKeys(cg->beginParents(), cg->endParents());
209  const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
210  const Vector xParent = getSubvector(x, keyInfo_, parentKeys);
211  const Vector rhsFrontal = getSubvector(y, keyInfo_, frontalKeys);
212 
213  /* compute the solution for the current pivot */
214  const Vector solFrontal = cg->R().triangularView<Eigen::Upper>().solve(
215  rhsFrontal - cg->S() * xParent);
216 
217  /* assign subvector of sol to the frontal variables */
218  setSubvector(solFrontal, keyInfo_, frontalKeys, x);
219  }
220 }
221 
222 /*****************************************************************************/
224  /* copy first */
225  assert(x.size() == y.size());
226  std::copy(y.data(), y.data() + y.rows(), x.data());
227 
228  /* in place back substitute */
229  for (const auto &cg : Rc1_) {
230  const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
231  const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
232  const Vector solFrontal =
233  cg->R().transpose().triangularView<Eigen::Lower>().solve(
234  rhsFrontal);
235 
236  // Check for indeterminant solution
237  if (solFrontal.hasNaN())
238  throw IndeterminantLinearSystemException(cg->keys().front());
239 
240  /* assign subvector of sol to the frontal variables */
241  setSubvector(solFrontal, keyInfo_, frontalKeys, x);
242 
243  /* substract from parent variables */
244  for (auto it = cg->beginParents(); it != cg->endParents(); it++) {
245  const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
246  Eigen::Map<Vector> rhsParent(x.data() + entry.start, entry.dim, 1);
247  rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
248  }
249  }
250 }
251 
252 /*****************************************************************************/
253 void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
254 {
255  /* identify the subgraph structure */
257  auto subgraph = builder(gfg);
258 
259  keyInfo_ = keyInfo;
260 
261  /* build factor subgraph */
262  auto gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true);
263 
264  /* factorize and cache BayesNet */
265  Rc1_ = *gfg_subgraph.eliminateSequential();
266 }
267 
268 /*****************************************************************************/
269 
270 } // nsamespace gtsam
const gtsam::Symbol key('X', 0)
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
Typedefs for easier changing of types.
Scalar * y
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
static void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst)
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
Errors gaussianErrors(const VectorValues &x) const
void transposeMultiplyAdd(double alpha, const Errors &e, VectorValues &x) const
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
VectorValues operator^(const Errors &e) const
double error(const VectorValues &y) const
void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, Errors::const_iterator end, VectorValues &y) const
Errors operator*(const VectorValues &y) const
static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys)
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
std::reverse_iterator< Iterator > make_reverse_iterator(Iterator i)
void transposeMultiplyAdd(double alpha, const Errors &e, VectorValues &y) const
SubgraphPreconditioner(const SubgraphPreconditionerParameters &p=SubgraphPreconditionerParameters())
const GaussianBayesNet & Rc1() const
const_iterator begin() const
Definition: FactorGraph.h:361
void multiplyInPlace(const VectorValues &x, Errors &e) const
SubgraphPreconditionerParameters parameters_
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
static VectorValues Zero(const VectorValues &other)
Array< int, Dynamic, 1 > v
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
VectorValues backSubstituteTranspose(const VectorValues &gx) const
Linear Factor Graph where all factors are Gaussians.
void transposeSolve(const Vector &y, Vector &x) const override
implement x = R^{-T} y
GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
const_iterator end() const
Definition: FactorGraph.h:364
static GaussianFactorGraph convertToJacobianFactors(const GaussianFactorGraph &gfg)
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
Errors createErrors(const VectorValues &V)
Break V into pieces according to its start indices.
Definition: Errors.cpp:28
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
static EIGEN_DEPRECATED const end_t end
float * p
Chordal Bayes Net, the result of eliminating a factor graph.
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
VectorValues backSubstitute(const VectorValues &gx) const
void solve(const Vector &y, Vector &x) const override
implement x = R^{-1} y
VectorValues gradient(const VectorValues &y) const
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
void print(const std::string &s="SubgraphPreconditioner") const
VectorValues x(const VectorValues &y) const
const GaussianFactorGraph & Ab2() const
void multiplyInPlace(const VectorValues &y, Errors &e) const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:36:21