gtsam 4.1.1 gtsam
linearAlgorithms-inst.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10 * -------------------------------------------------------------------------- */
11
21
22#include <boost/optional.hpp>
23#include <boost/shared_ptr.hpp>
24
25namespace gtsam
26{
27 namespace internal
28 {
29 namespace linearAlgorithms
30 {
31 /* ************************************************************************* */
32 struct OptimizeData {
33 boost::optional<OptimizeData&> parentData;
35 //VectorValues ancestorResults;
36 //VectorValues results;
37 };
38
39 /* ************************************************************************* */
46 template<class CLIQUE>
48 {
49 VectorValues collectedResult;
50
51 OptimizeData operator()(
52 const boost::shared_ptr<CLIQUE>& clique,
53 OptimizeData& parentData)
54 {
55 OptimizeData myData;
56 myData.parentData = parentData;
57 // Take any ancestor results we'll need
58 for(Key parent: clique->conditional_->parents())
59 myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent));
60
61 // Solve and store in our results
62 {
63 GaussianConditional& c = *clique->conditional();
64 // Solve matrix
65 Vector xS;
66 {
67 // Count dimensions of vector
68 DenseIndex dim = 0;
69 FastVector<VectorValues::const_iterator> parentPointers;
70 parentPointers.reserve(clique->conditional()->nrParents());
71 for(Key parent: clique->conditional()->parents()) {
72 parentPointers.push_back(myData.cliqueResults.at(parent));
73 dim += parentPointers.back()->second.size();
74 }
75
76 // Fill parent vector
77 xS.resize(dim);
78 DenseIndex vectorPos = 0;
79 for(const VectorValues::const_iterator& parentPointer: parentPointers) {
80 const Vector& parentVector = parentPointer->second;
81 xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1);
82 vectorPos += parentVector.size();
83 }
84 }
85
86 // NOTE(gareth): We can no longer write: xS = b - S * xS
87 // This is because Eigen (as of 3.3) no longer evaluates S * xS into
88 // a temporary, and the operation trashes valus in xS.
89 // See: http://eigen.tuxfamily.org/index.php?title=3.3
90 const Vector rhs = c.getb() - c.S() * xS;
91
92 // TODO(gareth): Inline instantiation of Eigen::Solve and check flag
93 const Vector solution = c.R().triangularView<Eigen::Upper>().solve(rhs);
94
95 // Check for indeterminant solution
96 if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
97
98 // Insert solution into a VectorValues
99 DenseIndex vectorPosition = 0;
100 for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
101 auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
102 if(!result.second)
103 throw std::runtime_error(
104 "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal)
105 + "' that exists.");
106
107 VectorValues::const_iterator r = result.first;
108 myData.cliqueResults.emplace(r->first, r);
109 vectorPosition += c.getDim(frontal);
110 }
111 }
112 return myData;
113 }
114 };
115
116 /* ************************************************************************* */
117 //OptimizeData OptimizePreVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& parentData)
118 //{
119 // // Create data - holds a pointer to our parent, a copy of parent solution, and our results
120 // OptimizeData myData;
121 // myData.parentData = parentData;
122 // // Take any ancestor results we'll need
123 // for(Key parent: clique->conditional_->parents())
124 // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
125 // // Solve and store in our results
126 // myData.results.insert(clique->conditional()->solve(myData.ancestorResults));
127 // myData.ancestorResults.insert(myData.results);
128 // return myData;
129 //}
130
131 /* ************************************************************************* */
132 //void OptimizePostVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& myData)
133 //{
134 // // Conglomerate our results to the parent
135 // myData.parentData->results.insert(myData.results);
136 //}
137
138 /* ************************************************************************* */
139 template<class BAYESTREE>
140 VectorValues optimizeBayesTree(const BAYESTREE& bayesTree)
141 {
142 gttic(linear_optimizeBayesTree);
143 //internal::OptimizeData rootData; // Will hold final solution
144 //treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor);
145 //return rootData.results;
146 OptimizeData rootData;
148 treeTraversal::no_op postVisitor;
149 TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
150 treeTraversal::DepthFirstForestParallel(bayesTree, rootData, preVisitor, postVisitor);
151 return preVisitor.collectedResult;
152 }
153 }
154 }
155}
Conditional Gaussian Base class.
Factor Graph Values.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:75
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
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:161
FACTOR::const_iterator endFrontals() const
Iterator pointing past the last frontal key.
Definition: Conditional.h:108
FACTOR::const_iterator beginFrontals() const
Iterator pointing to first frontal key.
Definition: Conditional.h:105
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:125
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:68
Key back() const
Last key.
Definition: Factor.h:119
A conditional Gaussian functions as the node in a Bayes network It has a set of parents y,...
Definition: GaussianConditional.h:39
constABlock R() const
Return a view of the upper-triangular R block of the conditional.
Definition: GaussianConditional.h:97
constABlock S() const
Get a view of the parent blocks.
Definition: GaussianConditional.h:100
const constBVector getb() const
Get a view of the r.h.s.
Definition: JacobianFactor.h:295
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:274
Definition: linearAlgorithms-inst.h:32
Pre-order visitor for back-substitution in a Bayes tree.
Definition: linearAlgorithms-inst.h:48
Thrown when a linear system is ill-posed.
Definition: linearExceptions.h:94
This class represents a collection of vector-valued variables associated each with a unique integer i...
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:183