24#include <boost/optional.hpp>
25#include <boost/shared_ptr.hpp>
31 namespace linearAlgorithms
35 boost::optional<OptimizeData&> parentData;
48 template<
class CLIQUE>
54 const boost::shared_ptr<CLIQUE>& clique,
58 myData.parentData = parentData;
60 for(
Key parent: clique->conditional_->parents())
61 myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent));
72 parentPointers.reserve(clique->conditional()->nrParents());
73 for(
Key parent: clique->conditional()->parents()) {
74 parentPointers.push_back(myData.cliqueResults.at(parent));
75 dim += parentPointers.
back()->second.size();
82 const Vector& parentVector = parentPointer->second;
83 xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1);
84 vectorPos += parentVector.size();
92 const Vector rhs = c.
getb() - c.
S() * xS;
95 const Vector solution = c.
R().triangularView<Eigen::Upper>().solve(rhs);
103 auto result = collectedResult.
emplace(*frontal, solution.segment(vectorPosition, c.
getDim(frontal)));
105 throw std::runtime_error(
106 "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal)
110 myData.cliqueResults.emplace(r->first, r);
111 vectorPosition += c.
getDim(frontal);
141 template<
class BAYESTREE>
142 VectorValues optimizeBayesTree(
const BAYESTREE& bayesTree)
144 gttic(linear_optimizeBayesTree);
150 treeTraversal::no_op postVisitor;
153 return preVisitor.collectedResult;
Conditional Gaussian Base class.
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition: FastVector.h:34
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:106
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
Traverse a forest depth-first with pre-order and post-order visits.
Definition: treeTraversal-inst.h:154
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition: types.h:192
FACTOR::const_iterator endFrontals() const
Iterator pointing past the last frontal key.
Definition: Conditional.h:163
FACTOR::const_iterator beginFrontals() const
Iterator pointing to first frontal key.
Definition: Conditional.h:160
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:140
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:80
Key back() const
Last key.
Definition: Factor.h:134
A GaussianConditional functions as the node in a Bayes network.
Definition: GaussianConditional.h:43
constABlock R() const
Return a view of the upper-triangular R block of the conditional.
Definition: GaussianConditional.h:218
constABlock S() const
Get a view of the parent blocks.
Definition: GaussianConditional.h:221
const constBVector getb() const
Get a view of the r.h.s.
Definition: JacobianFactor.h:297
DenseIndex getDim(const_iterator variable) const override
Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor ...
Definition: JacobianFactor.h:276
Definition: linearAlgorithms-inst.h:34
Pre-order visitor for back-substitution in a Bayes tree.
Definition: linearAlgorithms-inst.h:50
Thrown when a linear system is ill-posed.
Definition: linearExceptions.h:94
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition: VectorValues.h:74
Values::const_iterator const_iterator
Const iterator over vector values.
Definition: VectorValues.h:82
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition: VectorValues.h:185