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


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:44:50