gtsam  4.1.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 
21 
22 #include <boost/optional.hpp>
23 #include <boost/shared_ptr.hpp>
24 
25 namespace 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 }
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition: JacobianFactor.h:295
gtsam::GaussianConditional::S
constABlock S() const
Get a view of the parent blocks.
Definition: GaussianConditional.h:100
gtsam::VectorValues::emplace
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition: VectorValues.h:183
gtsam::IndeterminantLinearSystemException
Thrown when a linear system is ill-posed.
Definition: linearExceptions.h:94
gtsam::GaussianConditional
A conditional Gaussian functions as the node in a Bayes network It has a set of parents y,...
Definition: GaussianConditional.h:39
gtsam::GaussianConditional::R
constABlock R() const
Return a view of the upper-triangular R block of the conditional.
Definition: GaussianConditional.h:97
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
GaussianConditional.h
Conditional Gaussian Base class.
gtsam::TbbOpenMPMixedScope
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition: types.h:153
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::Conditional::beginFrontals
FACTOR::const_iterator beginFrontals() const
Iterator pointing to first frontal key.
Definition: Conditional.h:105
gtsam::internal::linearAlgorithms::OptimizeClique
Pre-order visitor for back-substitution in a Bayes tree.
Definition: linearAlgorithms-inst.h:48
gtsam::JacobianFactor::getDim
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
VectorValues.h
Factor Graph Values.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
treeTraversal-inst.h
gtsam::VectorValues::const_iterator
Values::const_iterator const_iterator
Const iterator over vector values.
Definition: VectorValues.h:82
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:67
gtsam::FastMap< Key, VectorValues::const_iterator >
gtsam::internal::linearAlgorithms::OptimizeData
Definition: linearAlgorithms-inst.h:32
gtsam::Conditional::endFrontals
FACTOR::const_iterator endFrontals() const
Iterator pointing past the last frontal key.
Definition: Conditional.h:108
gtsam::treeTraversal::DepthFirstForestParallel
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