Loading...
Searching...
No Matches
linearAlgorithms-inst.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>
namespace gtsam
{
namespace internal
{
namespace linearAlgorithms
{
/* ************************************************************************* */
struct OptimizeData {
boost::optional<OptimizeData&> parentData;
FastMap<Key, VectorValues::const_iterator> cliqueResults;
//VectorValues ancestorResults;
//VectorValues results;
};
/* ************************************************************************* */
template<class CLIQUE>
struct OptimizeClique
{
VectorValues collectedResult;
OptimizeData operator()(
const boost::shared_ptr<CLIQUE>& clique,
OptimizeData& parentData)
{
OptimizeData myData;
myData.parentData = parentData;
// Take any ancestor results we'll need
for(Key parent: clique->conditional_->parents())
myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent));
// Solve and store in our results
{
GaussianConditional& c = *clique->conditional();
// Solve matrix
Vector xS;
{
// Count dimensions of vector
DenseIndex dim = 0;
FastVector<VectorValues::const_iterator> parentPointers;
parentPointers.reserve(clique->conditional()->nrParents());
for(Key parent: clique->conditional()->parents()) {
parentPointers.push_back(myData.cliqueResults.at(parent));
dim += parentPointers.back()->second.size();
}
// Fill parent vector
xS.resize(dim);
DenseIndex vectorPos = 0;
for(const VectorValues::const_iterator& parentPointer: parentPointers) {
const Vector& parentVector = parentPointer->second;
xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1);
vectorPos += parentVector.size();
}
}
// NOTE(gareth): We can no longer write: xS = b - S * xS
// This is because Eigen (as of 3.3) no longer evaluates S * xS into
// a temporary, and the operation trashes valus in xS.
// See: http://eigen.tuxfamily.org/index.php?title=3.3
const Vector rhs = c.getb() - c.S() * xS;
// TODO(gareth): Inline instantiation of Eigen::Solve and check flag
const Vector solution = c.R().triangularView<Eigen::Upper>().solve(rhs);
// Check for indeterminant solution
if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
// Insert solution into a VectorValues
DenseIndex vectorPosition = 0;
for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) {
auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal)));
if(!result.second)
throw std::runtime_error(
"Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal)
+ "' that exists.");
VectorValues::const_iterator r = result.first;
myData.cliqueResults.emplace(r->first, r);
vectorPosition += c.getDim(frontal);
}
}
return myData;
}
};
/* ************************************************************************* */
//OptimizeData OptimizePreVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& parentData)
//{
// // Create data - holds a pointer to our parent, a copy of parent solution, and our results
// OptimizeData myData;
// myData.parentData = parentData;
// // Take any ancestor results we'll need
// for(Key parent: clique->conditional_->parents())
// myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// // Solve and store in our results
// myData.results.insert(clique->conditional()->solve(myData.ancestorResults));
// myData.ancestorResults.insert(myData.results);
// return myData;
//}
/* ************************************************************************* */
//void OptimizePostVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& myData)
//{
// // Conglomerate our results to the parent
// myData.parentData->results.insert(myData.results);
//}
/* ************************************************************************* */
template<class BAYESTREE>
VectorValues optimizeBayesTree(const BAYESTREE& bayesTree)
{
gttic(linear_optimizeBayesTree);
//internal::OptimizeData rootData; // Will hold final solution
//treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor);
//return rootData.results;
OptimizeData rootData;
OptimizeClique<typename BAYESTREE::Clique> preVisitor;
treeTraversal::no_op postVisitor;
TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
treeTraversal::DepthFirstForestParallel(bayesTree, rootData, preVisitor, postVisitor);
return preVisitor.collectedResult;
}
}
}
}
treeTraversal-inst.h
VectorValues.h
Factor Graph Values.
GaussianConditional.h
Conditional Gaussian Base class.
gtsam::FastVector
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:33
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:106
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
gtsam::TbbOpenMPMixedScope
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition types.h:192
gtsam::Conditional::endFrontals
FACTOR::const_iterator endFrontals() const
Iterator pointing past the last frontal key.
Definition Conditional.h:163
gtsam::Conditional::beginFrontals
FACTOR::const_iterator beginFrontals() const
Iterator pointing to first frontal key.
Definition Conditional.h:160
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
gtsam::GaussianConditional::R
constABlock R() const
Return a view of the upper-triangular R block of the conditional.
Definition GaussianConditional.h:218
gtsam::GaussianConditional::S
constABlock S() const
Get a view of the parent blocks.
Definition GaussianConditional.h:221
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
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:276
gtsam::internal::linearAlgorithms::OptimizeData
Definition linearAlgorithms-inst.h:34
gtsam::internal::linearAlgorithms::OptimizeClique
Pre-order visitor for back-substitution in a Bayes tree.
Definition linearAlgorithms-inst.h:50
gtsam::IndeterminantLinearSystemException
Thrown when a linear system is ill-posed.
Definition linearExceptions.h:94
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::VectorValues::const_iterator
Values::const_iterator const_iterator
Const iterator over vector values.
Definition VectorValues.h:82