Go to the documentation of this file.
40 vector<std::pair<size_t, size_t> > cache;
47 cache.emplace_back(entry.
start, entry.
dim);
54 for (
const auto &
p : cache) {
55 result.segment(index,
p.second) = src.segment(
p.first,
p.second);
68 const size_t keyDim = entry.
dim;
69 dst.segment(entry.
start, keyDim) = src.segment(index, keyDim);
80 for (
const auto &factor : gfg)
82 auto jf = std::dynamic_pointer_cast<JacobianFactor>(factor);
84 jf = std::make_shared<JacobianFactor>(*factor);
99 b2bar_(-Ab2_.gaussianErrors(xbar)), parameters_(
p) {
113 return 0.5 * (
dot(
e,
e) +
dot(e2,e2));
132 e.splice(
e.end(), e2);
140 Errors::iterator ei =
e.begin();
141 for(
const auto& key_value:
y) {
142 *ei = key_value.second;
155 Errors::const_iterator it =
e.begin();
157 for(
auto& key_value:
y) {
158 key_value.second = *it;
170 Errors::const_iterator it =
e.begin();
171 for(
auto& key_value:
y) {
173 key_value.second +=
alpha * ei;
176 transposeMultiplyAdd2(
alpha, it,
e.end(),
y);
182 Errors::const_iterator it, Errors::const_iterator
end,
VectorValues&
y)
const {
187 while (it !=
end) e2.push_back(*(it++));
202 assert(
x.
size() ==
y.size());
208 const KeyVector parentKeys(cg->beginParents(), cg->endParents());
209 const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
215 rhsFrontal - cg->S() * xParent);
225 assert(
x.
size() ==
y.size());
226 std::copy(
y.data(),
y.data() +
y.rows(),
x.data());
229 for (
const auto &cg :
Rc1_) {
230 const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
237 if (solFrontal.hasNaN())
244 for (
auto it = cg->beginParents(); it != cg->endParents(); it++) {
247 rhsParent -=
Matrix(cg->getA(it)).transpose() * solFrontal;
257 auto subgraph = builder(gfg);
265 Rc1_ = *gfg_subgraph.eliminateSequential();
GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
Errors operator*(const VectorValues &y) const
Linear Factor Graph where all factors are Gaussians.
typedef and functions to augment Eigen's VectorXd
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Typedefs for easier changing of types.
VectorValues operator^(const Errors &e) const
SubgraphPreconditioner(const SubgraphPreconditionerParameters &p=SubgraphPreconditionerParameters())
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
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
double error(const VectorValues &y) const
Errors b2bar_
A2*xbar - b2.
Chordal Bayes Net, the result of eliminating a factor graph.
static void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst)
static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys)
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
VectorValues x(const VectorValues &y) const
void transposeMultiplyAdd(double alpha, const Errors &e, VectorValues &y) const
VectorValues gradient(const VectorValues &y) const
void print(const std::string &s="SubgraphPreconditioner") const
void transposeSolve(const Vector &y, Vector &x) const override
implement x = R^{-T} y
Errors createErrors(const VectorValues &V)
Break V into pieces according to its start indices.
void multiplyInPlace(const VectorValues &y, Errors &e) const
double dot(const V1 &a, const V2 &b)
static VectorValues Zero(const VectorValues &other)
VectorValues backSubstituteTranspose(const VectorValues &gx) const
A matrix or vector expression mapping an existing array of data.
SubgraphBuilderParameters builderParams
Errors gaussianErrors(const VectorValues &x) const
const gtsam::Symbol key('X', 0)
void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, Errors::const_iterator end, VectorValues &y) const
void solve(const Vector &y, Vector &x) const override
implement x = R^{-1} y
static GaussianFactorGraph convertToJacobianFactors(const GaussianFactorGraph &gfg)
void transposeMultiplyAdd(double alpha, const Errors &e, VectorValues &x) const
std::reverse_iterator< Iterator > make_reverse_iterator(Iterator i)
const_iterator end() const
VectorValues backSubstitute(const VectorValues &gx) const
Array< int, Dynamic, 1 > v
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
const_iterator begin() const
SubgraphPreconditionerParameters parameters_
static const EIGEN_DEPRECATED end_t end
void multiplyInPlace(const VectorValues &x, Errors &e) const
** In-place version e <- A*x that overwrites e. */
std::uint64_t Key
Integer nonlinear key type.
VectorValues xbar_
A1 \ b1.
VectorValues zero() const
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:36:27