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 /* ************************************************************************* */
97  const GaussianBayesNet& Rc1, const VectorValues& xbar, const SubgraphPreconditionerParameters &p) :
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
gtsam::buildFactorSubgraph
GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
Definition: SubgraphBuilder.cpp:407
gtsam::SubgraphPreconditioner::operator*
Errors operator*(const VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:128
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
Vector.h
typedef and functions to augment Eigen's VectorXd
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
types.h
Typedefs for easier changing of types.
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::SubgraphPreconditioner::operator^
VectorValues operator^(const Errors &e) const
Definition: SubgraphPreconditioner.cpp:153
gtsam::SubgraphPreconditioner::SubgraphPreconditioner
SubgraphPreconditioner(const SubgraphPreconditionerParameters &p=SubgraphPreconditionerParameters())
Definition: SubgraphPreconditioner.cpp:92
gtsam::FactorGraph::print
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Definition: FactorGraph-inst.h:37
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::KeyInfoEntry::dim
size_t dim
Definition: IterativeSolver.h:116
gtsam::SubgraphPreconditioner::error
double error(const VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:109
gtsam::SubgraphPreconditioner::b2bar_
Errors b2bar_
A2*xbar - b2.
Definition: SubgraphPreconditioner.h:63
Eigen::Upper
@ Upper
Definition: Constants.h:211
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
gtsam::setSubvector
static void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst)
Definition: SubgraphPreconditioner.cpp:63
gtsam::getSubvector
static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys)
Definition: SubgraphPreconditioner.cpp:37
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::KeyInfo
Definition: IterativeSolver.h:127
gtsam::SubgraphPreconditioner::x
VectorValues x(const VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:104
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::SubgraphPreconditioner::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Errors &e, VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:168
gtsam::SubgraphPreconditioner::Rc1_
GaussianBayesNet Rc1_
Definition: SubgraphPreconditioner.h:61
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::SubgraphPreconditioner::gradient
VectorValues gradient(const VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:118
gtsam::SubgraphPreconditioner::Ab2_
GaussianFactorGraph Ab2_
Definition: SubgraphPreconditioner.h:60
gtsam::SubgraphPreconditioner::print
void print(const std::string &s="SubgraphPreconditioner") const
Definition: SubgraphPreconditioner.cpp:195
gtsam::SubgraphPreconditioner::transposeSolve
void transposeSolve(const Vector &y, Vector &x) const override
implement x = R^{-T} y
Definition: SubgraphPreconditioner.cpp:223
gtsam::createErrors
Errors createErrors(const VectorValues &V)
Break V into pieces according to its start indices.
Definition: Errors.cpp:28
gtsam::SubgraphPreconditioner::multiplyInPlace
void multiplyInPlace(const VectorValues &y, Errors &e) const
Definition: SubgraphPreconditioner.cpp:138
gtsam::dot
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
gtsam::VectorValues::Zero
static VectorValues Zero(const VectorValues &other)
Definition: VectorValues.cpp:75
gtsam::SubgraphPreconditioner::keyInfo_
KeyInfo keyInfo_
Definition: SubgraphPreconditioner.h:65
gtsam::SubgraphBuilder
Definition: SubgraphBuilder.h:153
gtsam::GaussianBayesNet::backSubstituteTranspose
VectorValues backSubstituteTranspose(const VectorValues &gx) const
Definition: GaussianBayesNet.cpp:145
gtsam::FastList< Vector >
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::SubgraphPreconditionerParameters::builderParams
SubgraphBuilderParameters builderParams
Definition: SubgraphPreconditioner.h:44
lambda
static double lambda[]
Definition: jv.c:524
gtsam::GaussianFactorGraph::gaussianErrors
Errors gaussianErrors(const VectorValues &x) const
Definition: GaussianFactorGraph.cpp:512
y
Scalar * y
Definition: level1_cplx_impl.h:124
key
const gtsam::Symbol key('X', 0)
JacobianFactor.h
gtsam::SubgraphPreconditionerParameters
Definition: SubgraphPreconditioner.h:40
gtsam::SubgraphPreconditioner::transposeMultiplyAdd2
void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, Errors::const_iterator end, VectorValues &y) const
Definition: SubgraphPreconditioner.cpp:181
gtsam::SubgraphPreconditioner::solve
void solve(const Vector &y, Vector &x) const override
implement x = R^{-1} y
Definition: SubgraphPreconditioner.cpp:201
gtsam::convertToJacobianFactors
static GaussianFactorGraph convertToJacobianFactors(const GaussianFactorGraph &gfg)
Definition: SubgraphPreconditioner.cpp:77
gtsam::GaussianFactorGraph::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Errors &e, VectorValues &x) const
Definition: GaussianFactorGraph.cpp:455
gtsam
traits
Definition: SFMdata.h:40
make_reverse_iterator
std::reverse_iterator< Iterator > make_reverse_iterator(Iterator i)
Definition: stl_iterators.cpp:16
gtsam::FactorGraph::end
const_iterator end() const
Definition: FactorGraph.h:342
SubgraphPreconditioner.h
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::GaussianBayesNet::backSubstitute
VectorValues backSubstitute(const VectorValues &gx) const
Definition: GaussianBayesNet.cpp:129
gtsam::VectorValues::size
size_t size() const
Definition: VectorValues.h:130
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
SubgraphBuilder.h
gtsam::SubgraphPreconditioner::build
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
Definition: SubgraphPreconditioner.cpp:253
gtsam::FactorGraph::begin
const_iterator begin() const
Definition: FactorGraph.h:339
gtsam::SubgraphPreconditioner::parameters_
SubgraphPreconditionerParameters parameters_
Definition: SubgraphPreconditioner.h:66
gtsam::IndeterminantLinearSystemException
Definition: linearExceptions.h:94
Eigen::placeholders::end
static const EIGEN_DEPRECATED end_t end
Definition: IndexedViewHelper.h:181
gtsam::KeyInfoEntry::start
size_t start
Definition: IterativeSolver.h:116
gtsam::GaussianFactorGraph::multiplyInPlace
void multiplyInPlace(const VectorValues &x, Errors &e) const
‍** In-place version e <- A*x that overwrites e. *‍/
Definition: GaussianFactorGraph.cpp:427
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SubgraphPreconditioner::xbar_
VectorValues xbar_
A1 \ b1.
Definition: SubgraphPreconditioner.h:62
gtsam::KeyInfoEntry
Definition: IterativeSolver.h:115
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35
gtsam::SubgraphPreconditioner::zero
VectorValues zero() const
Definition: SubgraphPreconditioner.h:104


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:36:27