gtsam 4.2.0
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
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
18#pragma once
19
23
24#include <boost/optional.hpp>
25#include <boost/shared_ptr.hpp>
26
27namespace gtsam
28{
29 namespace internal
30 {
31 namespace linearAlgorithms
32 {
33 /* ************************************************************************* */
34 struct OptimizeData {
35 boost::optional<OptimizeData&> parentData;
37 //VectorValues ancestorResults;
38 //VectorValues results;
39 };
40
41 /* ************************************************************************* */
48 template<class CLIQUE>
50 {
51 VectorValues collectedResult;
52
53 OptimizeData operator()(
54 const boost::shared_ptr<CLIQUE>& clique,
55 OptimizeData& parentData)
56 {
57 OptimizeData myData;
58 myData.parentData = parentData;
59 // Take any ancestor results we'll need
60 for(Key parent: clique->conditional_->parents())
61 myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent));
62
63 // Solve and store in our results
64 {
65 GaussianConditional& c = *clique->conditional();
66 // Solve matrix
67 Vector xS;
68 {
69 // Count dimensions of vector
70 DenseIndex dim = 0;
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();
76 }
77
78 // Fill parent vector
79 xS.resize(dim);
80 DenseIndex vectorPos = 0;
81 for(const VectorValues::const_iterator& parentPointer: parentPointers) {
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();
85 }
86 }
87
88 // NOTE(gareth): We can no longer write: xS = b - S * xS
89 // This is because Eigen (as of 3.3) no longer evaluates S * xS into
90 // a temporary, and the operation trashes valus in xS.
91 // See: http://eigen.tuxfamily.org/index.php?title=3.3
92 const Vector rhs = c.getb() - c.S() * xS;
93
94 // TODO(gareth): Inline instantiation of Eigen::Solve and check flag
95 const Vector solution = c.R().triangularView<Eigen::Upper>().solve(rhs);
96
97 // Check for indeterminant solution
98 if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
99
100 // Insert solution into a VectorValues
101 DenseIndex vectorPosition = 0;
102 for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
103 auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
104 if(!result.second)
105 throw std::runtime_error(
106 "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal)
107 + "' that exists.");
108
109 VectorValues::const_iterator r = result.first;
110 myData.cliqueResults.emplace(r->first, r);
111 vectorPosition += c.getDim(frontal);
112 }
113 }
114 return myData;
115 }
116 };
117
118 /* ************************************************************************* */
119 //OptimizeData OptimizePreVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& parentData)
120 //{
121 // // Create data - holds a pointer to our parent, a copy of parent solution, and our results
122 // OptimizeData myData;
123 // myData.parentData = parentData;
124 // // Take any ancestor results we'll need
125 // for(Key parent: clique->conditional_->parents())
126 // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
127 // // Solve and store in our results
128 // myData.results.insert(clique->conditional()->solve(myData.ancestorResults));
129 // myData.ancestorResults.insert(myData.results);
130 // return myData;
131 //}
132
133 /* ************************************************************************* */
134 //void OptimizePostVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& myData)
135 //{
136 // // Conglomerate our results to the parent
137 // myData.parentData->results.insert(myData.results);
138 //}
139
140 /* ************************************************************************* */
141 template<class BAYESTREE>
142 VectorValues optimizeBayesTree(const BAYESTREE& bayesTree)
143 {
144 gttic(linear_optimizeBayesTree);
145 //internal::OptimizeData rootData; // Will hold final solution
146 //treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor);
147 //return rootData.results;
148 OptimizeData rootData;
150 treeTraversal::no_op postVisitor;
151 TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
152 treeTraversal::DepthFirstForestParallel(bayesTree, rootData, preVisitor, postVisitor);
153 return preVisitor.collectedResult;
154 }
155 }
156 }
157}
Factor Graph Values.
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