#include <SubgraphPreconditioner.h>
Public Types | |
typedef std::shared_ptr< SubgraphPreconditioner > | shared_ptr |
Public Types inherited from gtsam::Preconditioner | |
typedef std::vector< size_t > | Dimensions |
typedef std::shared_ptr< Preconditioner > | shared_ptr |
Public Member Functions | |
const GaussianFactorGraph & | Ab2 () const |
const Errors | b2bar () const |
void | build (const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override |
build/factorize the preconditioner More... | |
double | error (const VectorValues &y) const |
VectorValues | gradient (const VectorValues &y) const |
void | multiplyInPlace (const VectorValues &y, Errors &e) const |
Errors | operator* (const VectorValues &y) const |
VectorValues | operator^ (const Errors &e) const |
void | print (const std::string &s="SubgraphPreconditioner") const |
const GaussianBayesNet & | Rc1 () const |
void | solve (const Vector &y, Vector &x) const override |
implement x = R^{-1} y More... | |
SubgraphPreconditioner (const GaussianFactorGraph &Ab2, const GaussianBayesNet &Rc1, const VectorValues &xbar, const SubgraphPreconditionerParameters &p=SubgraphPreconditionerParameters()) | |
SubgraphPreconditioner (const SubgraphPreconditionerParameters &p=SubgraphPreconditionerParameters()) | |
void | transposeMultiplyAdd (double alpha, const Errors &e, VectorValues &y) const |
void | transposeMultiplyAdd2 (double alpha, Errors::const_iterator begin, Errors::const_iterator end, VectorValues &y) const |
void | transposeSolve (const Vector &y, Vector &x) const override |
implement x = R^{-T} y More... | |
VectorValues | x (const VectorValues &y) const |
VectorValues | zero () const |
~SubgraphPreconditioner () override | |
Public Member Functions inherited from gtsam::Preconditioner | |
Preconditioner () | |
virtual | ~Preconditioner () |
Private Attributes | |
GaussianFactorGraph | Ab2_ |
Errors | b2bar_ |
A2*xbar - b2. More... | |
KeyInfo | keyInfo_ |
SubgraphPreconditionerParameters | parameters_ |
GaussianBayesNet | Rc1_ |
VectorValues | xbar_ |
A1 \ b1. More... | |
Subgraph conditioner class, as explained in the RSS 2010 submission. Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2 We solve R1*x=c1, and make the substitution y=R1*x-c1. To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2. Then solve for yhat using CG, and solve for xhat = system.x(yhat).
Definition at line 54 of file SubgraphPreconditioner.h.
typedef std::shared_ptr<SubgraphPreconditioner> gtsam::SubgraphPreconditioner::shared_ptr |
Definition at line 57 of file SubgraphPreconditioner.h.
gtsam::SubgraphPreconditioner::SubgraphPreconditioner | ( | const SubgraphPreconditionerParameters & | p = SubgraphPreconditionerParameters() | ) |
Definition at line 92 of file SubgraphPreconditioner.cpp.
gtsam::SubgraphPreconditioner::SubgraphPreconditioner | ( | const GaussianFactorGraph & | Ab2, |
const GaussianBayesNet & | Rc1, | ||
const VectorValues & | xbar, | ||
const SubgraphPreconditionerParameters & | p = SubgraphPreconditionerParameters() |
||
) |
Constructor
Ab2 | the Graph A2*x=b2 |
Rc1 | the Bayes Net R1*x=c1 |
xbar | the solution to R1*x=c1 |
Definition at line 96 of file SubgraphPreconditioner.cpp.
|
inlineoverride |
Definition at line 81 of file SubgraphPreconditioner.h.
|
inline |
Access Ab2
Definition at line 87 of file SubgraphPreconditioner.h.
|
inline |
Access b2bar
Definition at line 93 of file SubgraphPreconditioner.h.
|
overridevirtual |
build/factorize the preconditioner
Implements gtsam::Preconditioner.
Definition at line 253 of file SubgraphPreconditioner.cpp.
double gtsam::SubgraphPreconditioner::error | ( | const VectorValues & | y | ) | const |
Definition at line 109 of file SubgraphPreconditioner.cpp.
VectorValues gtsam::SubgraphPreconditioner::gradient | ( | const VectorValues & | y | ) | const |
gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar)
Definition at line 118 of file SubgraphPreconditioner.cpp.
void gtsam::SubgraphPreconditioner::multiplyInPlace | ( | const VectorValues & | y, |
Errors & | e | ||
) | const |
Apply operator A in place: needs e allocated already
Definition at line 138 of file SubgraphPreconditioner.cpp.
Errors gtsam::SubgraphPreconditioner::operator* | ( | const VectorValues & | y | ) | const |
Apply operator A
Definition at line 128 of file SubgraphPreconditioner.cpp.
VectorValues gtsam::SubgraphPreconditioner::operator^ | ( | const Errors & | e | ) | const |
Apply operator A'
Definition at line 153 of file SubgraphPreconditioner.cpp.
void gtsam::SubgraphPreconditioner::print | ( | const std::string & | s = "SubgraphPreconditioner" | ) | const |
print the object
Definition at line 195 of file SubgraphPreconditioner.cpp.
|
inline |
Access Rc1
Definition at line 90 of file SubgraphPreconditioner.h.
implement x = R^{-1} y
Implements gtsam::Preconditioner.
Definition at line 201 of file SubgraphPreconditioner.cpp.
void gtsam::SubgraphPreconditioner::transposeMultiplyAdd | ( | double | alpha, |
const Errors & | e, | ||
VectorValues & | y | ||
) | const |
Add A'e to y y += alpha*A'[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2]
Definition at line 168 of file SubgraphPreconditioner.cpp.
void gtsam::SubgraphPreconditioner::transposeMultiplyAdd2 | ( | double | alpha, |
Errors::const_iterator | begin, | ||
Errors::const_iterator | end, | ||
VectorValues & | y | ||
) | const |
Add constraint part of the error only y += alpha*inv(R1')*A2'*e2 Takes a range indicating e2 !!!!
Definition at line 181 of file SubgraphPreconditioner.cpp.
|
overridevirtual |
implement x = R^{-T} y
Implements gtsam::Preconditioner.
Definition at line 223 of file SubgraphPreconditioner.cpp.
VectorValues gtsam::SubgraphPreconditioner::x | ( | const VectorValues & | y | ) | const |
Add zero-mean i.i.d. Gaussian prior terms to each variable
sigma | Standard deviation of Gaussian |
Definition at line 104 of file SubgraphPreconditioner.cpp.
|
inline |
Definition at line 104 of file SubgraphPreconditioner.h.
|
private |
Definition at line 60 of file SubgraphPreconditioner.h.
|
private |
A2*xbar - b2.
Definition at line 63 of file SubgraphPreconditioner.h.
|
private |
Definition at line 65 of file SubgraphPreconditioner.h.
|
private |
Definition at line 66 of file SubgraphPreconditioner.h.
|
private |
Definition at line 61 of file SubgraphPreconditioner.h.
|
private |
A1 \ b1.
Definition at line 62 of file SubgraphPreconditioner.h.