Added linear tools (including summarization) from MastSLAM
Alex Cunningham
parent
a0b55c3ff7
commit
da334ed8a2
|
|
@ -2412,6 +2412,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check.linear_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>check.linear_unstable</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ set (gtsam_unstable_subdirs
|
|||
base
|
||||
geometry
|
||||
discrete
|
||||
# linear
|
||||
linear
|
||||
dynamics
|
||||
nonlinear
|
||||
slam
|
||||
|
|
@ -38,7 +38,7 @@ set(gtsam_unstable_srcs
|
|||
${geometry_srcs}
|
||||
${discrete_srcs}
|
||||
${dynamics_srcs}
|
||||
#${linear_srcs}
|
||||
${linear_srcs}
|
||||
${nonlinear_srcs}
|
||||
${slam_srcs}
|
||||
${navigation_srcs}
|
||||
|
|
|
|||
|
|
@ -3,10 +3,15 @@ file(GLOB base_headers "*.h")
|
|||
install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear)
|
||||
|
||||
# Components to link tests in this subfolder against
|
||||
set(linear_local_libs
|
||||
linear
|
||||
linear_unstable
|
||||
base)
|
||||
set(linear_local_libs
|
||||
linear_unstable
|
||||
nonlinear
|
||||
linear
|
||||
inference
|
||||
geometry
|
||||
base
|
||||
ccolamd
|
||||
)
|
||||
|
||||
set (linear_full_libs
|
||||
${gtsam-default}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,152 @@
|
|||
/**
|
||||
* @file bayesTreeOperations.cpp
|
||||
*
|
||||
* @date Jun 22, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/linear/bayesTreeOperations.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::set<Index> keysToIndices(const KeySet& keys, const Ordering& ordering) {
|
||||
std::set<Index> result;
|
||||
BOOST_FOREACH(const Key& key, keys)
|
||||
result.insert(ordering[key]);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph) {
|
||||
GaussianFactorGraph result;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) {
|
||||
GaussianFactorGraph split = splitFactor(factor);
|
||||
result.push_back(split);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) {
|
||||
GaussianFactorGraph result;
|
||||
if (!factor) return result;
|
||||
|
||||
// Needs to be a jacobian factor - just pass along hessians
|
||||
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
|
||||
if (!jf) {
|
||||
result.push_back(factor);
|
||||
return result;
|
||||
}
|
||||
|
||||
// Loop over variables and strip off factors using split conditionals
|
||||
// Assumes upper triangular structure
|
||||
JacobianFactor::const_iterator rowIt, colIt;
|
||||
const size_t totalRows = jf->rows();
|
||||
size_t rowsRemaining = totalRows;
|
||||
for (rowIt = jf->begin(); rowIt != jf->end(); ++rowIt) {
|
||||
// get dim of current variable
|
||||
size_t varDim = jf->getDim(rowIt);
|
||||
size_t startRow = totalRows - rowsRemaining;
|
||||
size_t nrRows = std::min(rowsRemaining, varDim);
|
||||
|
||||
// Extract submatrices
|
||||
std::vector<std::pair<Index, Matrix> > matrices;
|
||||
for (colIt = rowIt; colIt != jf->end(); ++colIt) {
|
||||
Index idx = *colIt;
|
||||
const Matrix subA = jf->getA(colIt).middleRows(startRow, nrRows);
|
||||
matrices.push_back(make_pair(idx, subA));
|
||||
}
|
||||
|
||||
Vector subB = jf->getb().segment(startRow, nrRows);
|
||||
Vector sigmas = jf->get_model()->sigmas().segment(startRow, nrRows);
|
||||
SharedDiagonal model;
|
||||
if (jf->get_model()->isConstrained())
|
||||
model = noiseModel::Constrained::MixedSigmas(sigmas);
|
||||
else
|
||||
model = noiseModel::Diagonal::Sigmas(sigmas);
|
||||
|
||||
// extract matrices from each
|
||||
// assemble into new JacobianFactor
|
||||
result.add(matrices, subB, model);
|
||||
|
||||
rowsRemaining -= nrRows;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph) {
|
||||
GaussianFactorGraph result;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) {
|
||||
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
|
||||
if (factor && (!jf || jf->size() > 1))
|
||||
result.push_back(factor);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void findCliques(const GaussianBayesTree::sharedClique& current_clique,
|
||||
std::set<GaussianConditional::shared_ptr>& result) {
|
||||
// Add the current clique
|
||||
result.insert(current_clique->conditional());
|
||||
|
||||
// Add the parent if not root
|
||||
if (!current_clique->isRoot())
|
||||
findCliques(current_clique->parent(), result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
|
||||
const GaussianBayesTree& bayesTree, const std::set<Index>& savedIndices) {
|
||||
std::set<GaussianConditional::shared_ptr> affected_cliques;
|
||||
// FIXME: track previously found keys more efficiently
|
||||
BOOST_FOREACH(const Index& index, savedIndices) {
|
||||
GaussianBayesTree::sharedClique clique = bayesTree.nodes()[index];
|
||||
|
||||
// add path back to root to affected set
|
||||
findCliques(clique, affected_cliques);
|
||||
}
|
||||
return affected_cliques;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::deque<GaussianBayesTree::sharedClique>
|
||||
findPathCliques(const GaussianBayesTree::sharedClique& initial) {
|
||||
std::deque<GaussianBayesTree::sharedClique> result, parents;
|
||||
if (initial->isRoot())
|
||||
return result;
|
||||
result.push_back(initial->parent());
|
||||
parents = findPathCliques(initial->parent());
|
||||
result.insert(result.end(), parents.begin(), parents.end());
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph liquefy(const GaussianBayesTree::sharedClique& root, bool splitConditionals) {
|
||||
GaussianFactorGraph result;
|
||||
if (root && root->conditional()) {
|
||||
GaussianConditional::shared_ptr conditional = root->conditional();
|
||||
if (!splitConditionals)
|
||||
result.push_back(conditional->toFactor());
|
||||
else
|
||||
result.push_back(splitFactor(conditional->toFactor()));
|
||||
}
|
||||
BOOST_FOREACH(const GaussianBayesTree::sharedClique& child, root->children())
|
||||
result.push_back(liquefy(child, splitConditionals));
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph liquefy(const GaussianBayesTree& bayesTree, bool splitConditionals) {
|
||||
return liquefy(bayesTree.root(), splitConditionals);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
/**
|
||||
* @file bayesTreeOperations.h
|
||||
*
|
||||
* @brief Types and utility functions for operating on linear systems
|
||||
*
|
||||
* @date Jun 22, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Managing orderings
|
||||
|
||||
/** Converts sets of keys to indices by way of orderings */
|
||||
std::set<Index> keysToIndices(const KeySet& keys, const Ordering& ordering);
|
||||
|
||||
// Linear Graph Operations
|
||||
|
||||
/**
|
||||
* Given a graph, splits each factor into factors where the dimension is
|
||||
* that of the first variable.
|
||||
*/
|
||||
GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph);
|
||||
|
||||
/**
|
||||
* Splits a factor into factors where the dimension is
|
||||
* that of the first variable.
|
||||
*/
|
||||
GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor);
|
||||
|
||||
/** Removes prior jacobian factors from the graph */
|
||||
GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph);
|
||||
|
||||
// Bayes Tree / Conditional operations
|
||||
|
||||
/**
|
||||
* Given a Bayes Tree, return conditionals corresponding to cliques that have or
|
||||
* are connected to a set of wanted variables.
|
||||
*
|
||||
* @return the set of conditionals extracted from cliques.
|
||||
*/
|
||||
std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
|
||||
const GaussianBayesTree& bayesTree, const std::set<Index>& savedIndices);
|
||||
|
||||
/**
|
||||
* Recursively traverses from a given clique in a Bayes Tree and collects all of the conditionals
|
||||
* Adds any new cliques from path to root to the result set.
|
||||
*
|
||||
* Note the use of a set of shared_ptr: this will sort/filter on unique *pointer* locations,
|
||||
* which ensures unique cliques, but the order of the cliques is meaningless
|
||||
*/
|
||||
void findCliqueConditionals(const GaussianBayesTree::sharedClique& current_clique,
|
||||
std::set<GaussianConditional::shared_ptr>& result);
|
||||
|
||||
/**
|
||||
* Given a clique, returns a sequence of clique parents to the root, not including the
|
||||
* given clique.
|
||||
*/
|
||||
std::deque<GaussianBayesTree::sharedClique>
|
||||
findPathCliques(const GaussianBayesTree::sharedClique& initial);
|
||||
|
||||
/**
|
||||
* Liquefies a GaussianBayesTree into a GaussianFactorGraph recursively, given either a
|
||||
* root clique or a full bayes tree.
|
||||
*
|
||||
* @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors
|
||||
*/
|
||||
GaussianFactorGraph liquefy(const GaussianBayesTree::sharedClique& root, bool splitConditionals = false);
|
||||
GaussianFactorGraph liquefy(const GaussianBayesTree& bayesTree, bool splitConditionals = false);
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,205 @@
|
|||
/**
|
||||
* @file summarization.cpp
|
||||
*
|
||||
* @date Jun 22, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/linear/conditioning.h>
|
||||
#include <gtsam_unstable/linear/bayesTreeOperations.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shared_ptr& initConditional,
|
||||
const std::set<Index>& saved_indices, const VectorValues& solution) {
|
||||
const bool verbose = false;
|
||||
|
||||
if (!initConditional)
|
||||
return initConditional;
|
||||
|
||||
if (verbose) {
|
||||
cout << "backsubSummarize() Starting" << endl;
|
||||
initConditional->print("Full initial conditional");
|
||||
}
|
||||
|
||||
// Check for presence of variables to remove
|
||||
std::set<Index> frontalsToRemove, parentsToRemove;
|
||||
BOOST_FOREACH(const Index& frontal, initConditional->frontals())
|
||||
if (!saved_indices.count(frontal))
|
||||
frontalsToRemove.insert(frontal);
|
||||
|
||||
BOOST_FOREACH(const Index& parent, initConditional->parents())
|
||||
if (!saved_indices.count(parent))
|
||||
parentsToRemove.insert(parent);
|
||||
|
||||
// If all variables in this conditional are to be saved, just return initial conditional
|
||||
if (frontalsToRemove.empty() && parentsToRemove.empty())
|
||||
return initConditional;
|
||||
|
||||
// If none of the frontal variables are to be saved, return empty pointer
|
||||
if (frontalsToRemove.size() == initConditional->nrFrontals())
|
||||
return GaussianConditional::shared_ptr();
|
||||
|
||||
// Collect dimensions of the new conditional
|
||||
if (verbose) cout << " Collecting dimensions" << endl;
|
||||
size_t newTotalRows = 0, newTotalCols = 1; // Need spacing for RHS
|
||||
size_t newNrFrontals = 0;
|
||||
size_t oldOffset = 0;
|
||||
vector<size_t> newDims, oldDims;
|
||||
vector<size_t> oldColOffsets;
|
||||
vector<Index> newIndices;
|
||||
vector<size_t> newIdxToOldIdx; // Access to arrays, maps from new var to old var
|
||||
const vector<Index>& oldIndices = initConditional->keys();
|
||||
const size_t oldNrFrontals = initConditional->nrFrontals();
|
||||
GaussianConditional::const_iterator varIt = initConditional->beginFrontals();
|
||||
size_t oldIdx = 0;
|
||||
for (; varIt != initConditional->endFrontals(); ++varIt) {
|
||||
size_t varDim = initConditional->dim(varIt);
|
||||
oldDims += varDim;
|
||||
if (!frontalsToRemove.count(*varIt)) {
|
||||
newTotalCols += varDim;
|
||||
newTotalRows += varDim;
|
||||
newDims += varDim;
|
||||
newIndices += *varIt;
|
||||
++newNrFrontals;
|
||||
newIdxToOldIdx += oldIdx;
|
||||
oldColOffsets += oldOffset;
|
||||
}
|
||||
oldOffset += varDim;
|
||||
++oldIdx;
|
||||
}
|
||||
varIt = initConditional->beginParents();
|
||||
for (; varIt != initConditional->endParents(); ++varIt) {
|
||||
size_t varDim = initConditional->dim(varIt);
|
||||
oldDims += varDim;
|
||||
if (!parentsToRemove.count(*varIt)) {
|
||||
newTotalCols += varDim;
|
||||
newDims += varDim;
|
||||
newIndices += *varIt;
|
||||
}
|
||||
}
|
||||
newDims += 1; // For the RHS
|
||||
|
||||
// Initialize new conditional
|
||||
Matrix full_matrix = Matrix::Zero(newTotalRows, newTotalCols);
|
||||
Vector sigmas = zero(newTotalRows);
|
||||
if (verbose) cout << " Initializing new conditional\nfull_matrix:\n" << full_matrix << endl;
|
||||
|
||||
// Fill in full matrix - iterate over rows for each sub-conditional
|
||||
const size_t oldRNrCols = initConditional->get_R().cols();
|
||||
size_t newColOffset = 0;
|
||||
for (size_t newfrontalIdx=0; newfrontalIdx<newNrFrontals; ++newfrontalIdx) {
|
||||
const size_t& dim = newDims.at(newfrontalIdx);
|
||||
if (verbose) cout << " Filling in Matrix: newfrontalIdx " << newfrontalIdx
|
||||
<< " frontalKey: " << newIndices[newfrontalIdx] << " dim: " << dim << endl;
|
||||
|
||||
size_t oldColOffset = oldColOffsets.at(newfrontalIdx);
|
||||
|
||||
// get R block, sliced by row
|
||||
Eigen::Block<GaussianConditional::rsd_type::constBlock> rblock =
|
||||
initConditional->get_R().block(oldColOffset, oldColOffset, dim, oldRNrCols-oldColOffset);
|
||||
if (verbose) cout << " rblock\n" << rblock << endl;
|
||||
|
||||
|
||||
// set the R matrix for this var
|
||||
full_matrix.block(newColOffset, newColOffset, dim, dim) = rblock.leftCols(dim);
|
||||
if (verbose) cout << " full_matrix: set R\n" << full_matrix << endl;
|
||||
|
||||
// set RHS
|
||||
full_matrix.block(newColOffset, newTotalCols-1, dim, 1) = initConditional->get_d().segment(oldColOffset, dim);
|
||||
if (verbose) cout << " full_matrix: set rhs\n" << full_matrix << endl;
|
||||
|
||||
// set sigmas
|
||||
sigmas.segment(newColOffset, dim) = initConditional->get_sigmas().segment(oldColOffset, dim);
|
||||
|
||||
// add parents in R block, while updating rhs
|
||||
// Loop through old variable list
|
||||
size_t newParentStartCol = newColOffset + dim;
|
||||
size_t oldParentStartCol = dim; // Copying from Rblock - offset already accounted for
|
||||
for (size_t oldIdx = newIdxToOldIdx[newfrontalIdx]+1; oldIdx<oldNrFrontals; ++oldIdx) {
|
||||
Index parentKey = oldIndices[oldIdx];
|
||||
size_t parentDim = oldDims[oldIdx];
|
||||
if (verbose) cout << " Adding parents from R: parentKey: " << parentKey << " parentDim: " << parentDim << endl;
|
||||
if (!frontalsToRemove.count(parentKey)) {
|
||||
if (verbose) {
|
||||
cout << " Copying block (from): oldParentStartCol " << oldParentStartCol << endl;
|
||||
cout << " Copying block (to): newColOffset " << newColOffset << ", newParentStartCol " << newParentStartCol << endl;
|
||||
}
|
||||
full_matrix.block(newColOffset, newParentStartCol, dim, parentDim)
|
||||
= rblock.middleCols(oldParentStartCol, parentDim);
|
||||
newParentStartCol += parentDim;
|
||||
if (verbose) cout << " full_matrix: add parent from R\n" << full_matrix << endl;
|
||||
} else {
|
||||
const Vector& parentSol = solution.at(parentKey);
|
||||
full_matrix.block(newColOffset, newTotalCols-1, dim, 1) -=
|
||||
rblock.middleCols(oldParentStartCol, parentDim) * parentSol;
|
||||
if (verbose) cout << " full_matrix: update rhs from parent in R\n" << full_matrix << endl;
|
||||
}
|
||||
oldParentStartCol += parentDim;
|
||||
}
|
||||
|
||||
// add parents (looping over original block structure), while updating rhs
|
||||
GaussianConditional::const_iterator oldParent = initConditional->beginParents();
|
||||
for (; oldParent != initConditional->endParents(); ++oldParent) {
|
||||
Index parentKey = *oldParent;
|
||||
size_t parentDim = initConditional->dim(oldParent);
|
||||
if (verbose) cout << " Adding parents from S: parentKey: " << parentKey << " parentDim: " << parentDim << endl;
|
||||
if (parentsToRemove.count(parentKey)) {
|
||||
// Solve out the variable
|
||||
const Vector& parentSol = solution.at(parentKey);
|
||||
assert((size_t)parentSol.size() == parentDim);
|
||||
full_matrix.block(newColOffset, newTotalCols-1, dim, 1) -=
|
||||
initConditional->get_S(oldParent).middleRows(oldColOffset, dim) * parentSol;
|
||||
if (verbose) cout << " full_matrix: update rhs from parent in S\n" << full_matrix << endl;
|
||||
} else {
|
||||
// Copy the matrix block
|
||||
full_matrix.block(newColOffset, newParentStartCol, dim, parentDim) =
|
||||
initConditional->get_S(oldParent).middleRows(oldColOffset, dim);
|
||||
if (verbose) cout << " full_matrix: add parent from S\n" << full_matrix << endl;
|
||||
}
|
||||
}
|
||||
|
||||
// Increment the rows
|
||||
newColOffset += dim;
|
||||
oldColOffset += dim;
|
||||
}
|
||||
|
||||
// Construct a new conditional
|
||||
if (verbose) cout << "backsubSummarize() Complete!" << endl;
|
||||
GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end());
|
||||
return GaussianConditional::shared_ptr(new
|
||||
GaussianConditional(newIndices.begin(), newIndices.end(), newNrFrontals, matrices, sigmas));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree,
|
||||
const std::set<Index>& saved_indices) {
|
||||
const bool verbose = false;
|
||||
|
||||
VectorValues solution = optimize(bayesTree);
|
||||
|
||||
// FIXME: set of conditionals does not manage possibility of solving out whole separators
|
||||
std::set<GaussianConditional::shared_ptr> affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices);
|
||||
|
||||
// Summarize conditionals separately
|
||||
GaussianFactorGraph summarized_graph;
|
||||
BOOST_FOREACH(const GaussianConditional::shared_ptr& conditional, affected_cliques) {
|
||||
if (verbose) conditional->print("Initial conditional");
|
||||
GaussianConditional::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution);
|
||||
if (reducedConditional) {
|
||||
if (verbose) reducedConditional->print("Final conditional");
|
||||
summarized_graph.push_back(reducedConditional->toFactor());
|
||||
} else if (verbose) {
|
||||
cout << "Conditional removed after summarization!" << endl;
|
||||
}
|
||||
}
|
||||
return summarized_graph;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
/**
|
||||
* @file summarization.h
|
||||
*
|
||||
* @brief Types and utility functions for summarization
|
||||
*
|
||||
* @date Jun 22, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Backsubstitution-based conditioning - reduces conditionals to
|
||||
* densities on a sub-set of variables.
|
||||
*
|
||||
* Corner cases:
|
||||
* - If no frontal vars are saved, returns a null pointer
|
||||
*
|
||||
* @param initConditional the conditional from which to remove a variable
|
||||
* @param saved_indices is the set of indices that should appear in the result
|
||||
* @param solution is a full solution for the system
|
||||
*/
|
||||
gtsam::GaussianConditional::shared_ptr conditionDensity(const gtsam::GaussianConditional::shared_ptr& initConditional,
|
||||
const std::set<gtsam::Index>& saved_indices, const gtsam::VectorValues& solution);
|
||||
|
||||
/**
|
||||
* Backsubstitution-based conditioning for a complete Bayes Tree - reduces
|
||||
* conditionals by solving out variables to eliminate. Traverses the tree to
|
||||
* add the correct dummy factors whenever a separator is eliminated.
|
||||
*/
|
||||
gtsam::GaussianFactorGraph conditionDensity(const gtsam::GaussianBayesTree& bayesTree,
|
||||
const std::set<gtsam::Index>& saved_indices);
|
||||
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
/**
|
||||
* @file corruptInitialization.h
|
||||
*
|
||||
* @brief Utilities for using noisemodels to corrupt given initialization value
|
||||
*
|
||||
* @date Jun 22, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/Sampler.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** given a noisemodel and a measurement, add noise to the measurement */
|
||||
template<typename T>
|
||||
T corruptWithNoise(const T& init,
|
||||
const noiseModel::Base::shared_ptr& model, Sampler& sampler) {
|
||||
Vector n = zero(model->dim());
|
||||
noiseModel::Diagonal::shared_ptr
|
||||
diag_model = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (diag_model)
|
||||
n = sampler.sampleNewModel(diag_model);
|
||||
return init.retract(n);
|
||||
}
|
||||
|
||||
// specialization for doubles - just adds, rather than retract
|
||||
template<>
|
||||
inline double corruptWithNoise<double>(const double& init,
|
||||
const noiseModel::Base::shared_ptr& model, Sampler& sampler) {
|
||||
double n = 0.0;
|
||||
noiseModel::Diagonal::shared_ptr
|
||||
diag_model = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (diag_model)
|
||||
n = sampler.sampleNewModel(diag_model)(0);
|
||||
return init + n;
|
||||
}
|
||||
|
||||
// specialization for doubles - just adds, rather than retract
|
||||
template<>
|
||||
inline Vector corruptWithNoise<Vector>(const Vector& init,
|
||||
const noiseModel::Base::shared_ptr& model, Sampler& sampler) {
|
||||
Vector n = zero(init.size());
|
||||
noiseModel::Diagonal::shared_ptr
|
||||
diag_model = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (diag_model)
|
||||
n = sampler.sampleNewModel(diag_model);
|
||||
return init + n;
|
||||
}
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
/**
|
||||
* @file testLinearTools.cpp
|
||||
*
|
||||
* @brief
|
||||
*
|
||||
* @date Aug 27, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam_unstable/linear/bayesTreeOperations.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(ones(2));
|
||||
SharedDiagonal model4 = noiseModel::Diagonal::Sigmas(ones(4));
|
||||
SharedDiagonal model6 = noiseModel::Diagonal::Sigmas(ones(6));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testLinearTools, splitFactor ) {
|
||||
|
||||
// Build upper-triangular system
|
||||
JacobianFactor initFactor(
|
||||
0,Matrix_(4, 2,
|
||||
1.0, 2.0,
|
||||
0.0, 3.0,
|
||||
0.0, 0.0,
|
||||
0.0, 0.0),
|
||||
1,Matrix_(4, 2,
|
||||
1.0, 2.0,
|
||||
9.0, 3.0,
|
||||
6.0, 8.0,
|
||||
0.0, 7.0),
|
||||
Vector_(4, 0.1, 0.2, 0.3, 0.4),
|
||||
model4);
|
||||
|
||||
GaussianFactorGraph actSplit = splitFactor(initFactor.clone());
|
||||
GaussianFactorGraph expSplit;
|
||||
|
||||
expSplit.add(
|
||||
0,Matrix_(2, 2,
|
||||
1.0, 2.0,
|
||||
0.0, 3.0),
|
||||
1,Matrix_(2, 2,
|
||||
1.0, 2.0,
|
||||
9.0, 3.0),
|
||||
Vector_(2, 0.1, 0.2),
|
||||
model2);
|
||||
expSplit.add(
|
||||
1,Matrix_(2, 2,
|
||||
6.0, 8.0,
|
||||
0.0, 7.0),
|
||||
Vector_(2, 0.3, 0.4),
|
||||
model2);
|
||||
|
||||
EXPECT(assert_equal(expSplit, actSplit));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST_UNSAFE( testLinearTools, splitFactor2 ) {
|
||||
|
||||
// Build upper-triangular system
|
||||
JacobianFactor initFactor(
|
||||
0,Matrix_(6, 2,
|
||||
1.0, 2.0,
|
||||
0.0, 3.0,
|
||||
0.0, 0.0,
|
||||
0.0, 0.0,
|
||||
0.0, 0.0,
|
||||
0.0, 0.0),
|
||||
1,Matrix_(6, 2,
|
||||
1.0, 2.0,
|
||||
9.0, 3.0,
|
||||
6.0, 8.0,
|
||||
0.0, 7.0,
|
||||
0.0, 0.0,
|
||||
0.0, 0.0),
|
||||
2,Matrix_(6, 2,
|
||||
1.1, 2.2,
|
||||
9.1, 3.2,
|
||||
6.1, 8.2,
|
||||
0.1, 7.2,
|
||||
0.1, 3.2,
|
||||
0.0, 1.2),
|
||||
Vector_(6, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6),
|
||||
model6);
|
||||
|
||||
GaussianFactorGraph actSplit = splitFactor(initFactor.clone());
|
||||
GaussianFactorGraph expSplit;
|
||||
|
||||
expSplit.add(
|
||||
0,Matrix_(2, 2,
|
||||
1.0, 2.0,
|
||||
0.0, 3.0),
|
||||
1,Matrix_(2, 2,
|
||||
1.0, 2.0,
|
||||
9.0, 3.0),
|
||||
2,Matrix_(2, 2,
|
||||
1.1, 2.2,
|
||||
9.1, 3.2),
|
||||
Vector_(2, 0.1, 0.2),
|
||||
model2);
|
||||
expSplit.add(
|
||||
1,Matrix_(2, 2,
|
||||
6.0, 8.0,
|
||||
0.0, 7.0),
|
||||
2,Matrix_(2, 2,
|
||||
6.1, 8.2,
|
||||
0.1, 7.2),
|
||||
Vector_(2, 0.3, 0.4),
|
||||
model2);
|
||||
expSplit.add(
|
||||
2,Matrix_(2, 2,
|
||||
0.1, 3.2,
|
||||
0.0, 1.2),
|
||||
Vector_(2, 0.5, 0.6),
|
||||
model2);
|
||||
|
||||
EXPECT(assert_equal(expSplit, actSplit));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,250 @@
|
|||
/**
|
||||
* @file testConditioning.cpp
|
||||
*
|
||||
* @brief Experiments using backsubstitution for conditioning (not summarization, it turns out)
|
||||
*
|
||||
* @date Sep 3, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
||||
#include <boost/assign/std/set.hpp>
|
||||
|
||||
#include <gtsam_unstable/linear/conditioning.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace boost::assign;
|
||||
using namespace gtsam;
|
||||
|
||||
const double tol = 1e-5;
|
||||
|
||||
// Simple example
|
||||
Matrix R = Matrix_(3,3,
|
||||
1.0,-2.0,-3.0,
|
||||
0.0, 3.0,-5.0,
|
||||
0.0, 0.0, 6.0);
|
||||
Vector d = Vector_(3,
|
||||
0.1, 0.2, 0.3);
|
||||
Vector x = Vector_(3,
|
||||
0.55,
|
||||
0.15,
|
||||
0.05);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testConditioning, directed_elimination_example ) {
|
||||
|
||||
// create a 3-variable system from which to eliminate variables
|
||||
// Scalar variables, pre-factorized into R,d system
|
||||
// Use multifrontal representation
|
||||
// Variables 0, 1, 2 - want to summarize out 1
|
||||
Vector expx = R.triangularView<Eigen::Upper>().solve(d);
|
||||
EXPECT(assert_equal(x, expx, tol));
|
||||
EXPECT(assert_equal(Vector(R*x), d, tol));
|
||||
|
||||
// backsub-summarized version
|
||||
Matrix Rprime = Matrix_(2,2,
|
||||
1.0,-3.0,
|
||||
0.0, 6.0);
|
||||
Vector dprime = Vector_(2,
|
||||
d(0) - R(0,1)*x(1),
|
||||
d(2));
|
||||
Vector xprime = Vector_(2,
|
||||
x(0), // Same solution, just smaller
|
||||
x(2));
|
||||
EXPECT(assert_equal(Vector(Rprime*xprime), dprime, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testConditioning, directed_elimination_singlefrontal ) {
|
||||
// Gaussian conditional with a single frontal variable, parent is to be removed
|
||||
// Top row from above example
|
||||
|
||||
Index root_key = 0, removed_key = 1, remaining_parent = 2;
|
||||
Matrix R11 = Matrix_(1,1, 1.0), R22 = Matrix_(1,1, 3.0), S = Matrix_(1,1,-2.0), T = Matrix_(1,1,-3.0);
|
||||
Vector d0 = d.segment(0,1), d1 = d.segment(1,1), sigmas = Vector_(1, 1.0);
|
||||
GaussianConditional::shared_ptr initConditional(new
|
||||
GaussianConditional(root_key, d0, R11, removed_key, S, remaining_parent, T, sigmas));
|
||||
|
||||
VectorValues solution;
|
||||
solution.insert(0, x.segment(0,1));
|
||||
solution.insert(1, x.segment(1,1));
|
||||
solution.insert(2, x.segment(2,1));
|
||||
|
||||
std::set<Index> saved_indices;
|
||||
saved_indices += root_key, remaining_parent;
|
||||
|
||||
GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
|
||||
GaussianConditional::shared_ptr expSummarized(new
|
||||
GaussianConditional(root_key, d0 - S*x(1), R11, remaining_parent, T, sigmas));
|
||||
|
||||
CHECK(actSummarized);
|
||||
EXPECT(assert_equal(*expSummarized, *actSummarized, tol));
|
||||
|
||||
// Simple test of base case: if target index isn't present, return clone
|
||||
GaussianConditional::shared_ptr actSummarizedSimple = conditionDensity(expSummarized, saved_indices, solution);
|
||||
CHECK(actSummarizedSimple);
|
||||
EXPECT(assert_equal(*expSummarized, *actSummarizedSimple, tol));
|
||||
|
||||
// case where frontal variable is to be eliminated - return null
|
||||
GaussianConditional::shared_ptr removeFrontalInit(new
|
||||
GaussianConditional(removed_key, d1, R22, remaining_parent, T, sigmas));
|
||||
GaussianConditional::shared_ptr actRemoveFrontal = conditionDensity(removeFrontalInit, saved_indices, solution);
|
||||
EXPECT(!actRemoveFrontal);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testConditioning, directed_elimination_multifrontal ) {
|
||||
// Use top two rows from the previous example
|
||||
Index root_key = 0, removed_key = 1, remaining_parent = 2;
|
||||
Matrix R11 = R.topLeftCorner(2,2), S = R.block(0,2,2,1),
|
||||
Sprime = Matrix_(1,1,-2.0), R11prime = Matrix_(1,1, 1.0);
|
||||
Vector d1 = d.segment(0,2), sigmas1 = Vector_(1, 1.0), sigmas2 = Vector_(2, 1.0, 1.0);
|
||||
|
||||
|
||||
std::list<std::pair<Index, Matrix> > terms;
|
||||
terms += make_pair(root_key, Matrix(R11.col(0)));
|
||||
terms += make_pair(removed_key, Matrix(R11.col(1)));
|
||||
terms += make_pair(remaining_parent, S);
|
||||
GaussianConditional::shared_ptr initConditional(new GaussianConditional(terms, 2, d1, sigmas2));
|
||||
|
||||
VectorValues solution;
|
||||
solution.insert(0, x.segment(0,1));
|
||||
solution.insert(1, x.segment(1,1));
|
||||
solution.insert(2, x.segment(2,1));
|
||||
|
||||
std::set<Index> saved_indices;
|
||||
saved_indices += root_key, remaining_parent;
|
||||
|
||||
GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
|
||||
GaussianConditional::shared_ptr expSummarized(new
|
||||
GaussianConditional(root_key, d.segment(0,1) - Sprime*x(1), R11prime, remaining_parent, R.block(0,2,1,1), sigmas1));
|
||||
|
||||
CHECK(actSummarized);
|
||||
EXPECT(assert_equal(*expSummarized, *actSummarized, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testConditioning, directed_elimination_multifrontal_multidim ) {
|
||||
// use larger example, three frontal variables, dim = 2 each, two parents (one removed)
|
||||
// Vars: 0, 1, 2, 3, 4; frontal: 0, 1, 2. parents: 3, 4;
|
||||
// Remove 1, 3
|
||||
Matrix Rinit = Matrix_(6, 11,
|
||||
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.1,
|
||||
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 1.0, 0.0, 1.0, 0.2,
|
||||
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,-1.0, 1.0, 0.0, 0.3,
|
||||
0.0, 0.0, 0.0, 4.0, 0.0, 4.0, 3.0, 2.0, 0.0, 9.0, 0.4,
|
||||
0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 3.0, 0.0, 0.5,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 4.0, 0.0, 8.0, 0.0, 6.0, 0.6);
|
||||
|
||||
vector<size_t> init_dims; init_dims += 2, 2, 2, 2, 2, 1;
|
||||
GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end());
|
||||
Vector sigmas = ones(6);
|
||||
vector<size_t> init_keys; init_keys += 0, 1, 2, 3, 4;
|
||||
GaussianConditional::shared_ptr initConditional(new
|
||||
GaussianConditional(init_keys.begin(), init_keys.end(), 3, init_matrices, sigmas));
|
||||
|
||||
// Construct a solution vector
|
||||
VectorValues solution;
|
||||
solution.insert(0, zero(2));
|
||||
solution.insert(1, zero(2));
|
||||
solution.insert(2, zero(2));
|
||||
solution.insert(3, Vector_(2, 1.0, 2.0));
|
||||
solution.insert(4, Vector_(2, 3.0, 4.0));
|
||||
|
||||
initConditional->solveInPlace(solution);
|
||||
|
||||
std::set<Index> saved_indices;
|
||||
saved_indices += 0, 2, 4;
|
||||
|
||||
GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
|
||||
CHECK(actSummarized);
|
||||
|
||||
Matrix Rexp = Matrix_(4, 7,
|
||||
1.0, 0.0, 3.0, 0.0, -1.0, 0.0, 0.1,
|
||||
0.0, 1.0, 0.0, 3.0, 0.0, 1.0, 0.2,
|
||||
0.0, 0.0, 5.0, 0.0, 3.0, 0.0, 0.5,
|
||||
0.0, 0.0, 0.0, 4.0, 0.0, 6.0, 0.6);
|
||||
|
||||
// Update rhs
|
||||
Rexp.block(0, 6, 2, 1) -= Rinit.block(0, 2, 2, 2) * solution.at(1) + Rinit.block(0, 6, 2, 2) * solution.at(3);
|
||||
Rexp.block(2, 6, 2, 1) -= Rinit.block(4, 6, 2, 2) * solution.at(3);
|
||||
|
||||
vector<size_t> exp_dims; exp_dims += 2, 2, 2, 1;
|
||||
GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end());
|
||||
Vector exp_sigmas = ones(4);
|
||||
vector<size_t> exp_keys; exp_keys += 0, 2, 4;
|
||||
GaussianConditional expSummarized(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, exp_sigmas);
|
||||
|
||||
EXPECT(assert_equal(expSummarized, *actSummarized, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testConditioning, directed_elimination_multifrontal_multidim2 ) {
|
||||
// Example from LinearAugmentedSystem
|
||||
// 4 variables, last two in ordering kept - should be able to do this with no computation.
|
||||
|
||||
vector<size_t> init_dims; init_dims += 3, 3, 2, 2, 1;
|
||||
|
||||
//Full initial conditional: density on [3] [4] [5] [6]
|
||||
Matrix Rinit = Matrix_(10, 11,
|
||||
8.78422312, -0.0375455118, -0.0387376278, -5.059576, 0.0, 0.0, -0.0887200041, 0.00429643583, -0.130078263, 0.0193260727, 0.0,
|
||||
0.0, 8.46951839, 9.51456887, -0.0224291821, -5.24757636, 0.0, 0.0586258904, -0.173455825, 0.11090295, -0.330696013, 0.0,
|
||||
0.0, 0.0, 16.5539485, 0.00105159359, -2.35354497, -6.04085484, -0.0212095105, 0.0978729072, 0.00471054272, 0.0694956367, 0.0,
|
||||
0.0, 0.0, 0.0, 10.9015885, -0.0105694572, 0.000582715469, -0.0410535006, 0.00162772139, -0.0601433772, 0.0082824087,0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 10.5531086, -1.34722553, 0.02438072, -0.0644224578, 0.0561372492, -0.148932792, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 21.4870439, -0.00443305851, 0.0234766354, 0.00484572411, 0.0101997356, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.61246954, 0.02287419, -0.102870789, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.04823446, -0.302033014, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.24068986, 0.0);
|
||||
Vector dinit = Vector_(10,
|
||||
-0.00186915, 0.00318554, 0.000592421, -0.000861, 0.00171528, 0.000274123, -0.0284011, 0.0275465, 0.0439795, -0.0222134);
|
||||
Rinit.rightCols(1) = dinit;
|
||||
Vector sigmas = ones(10);
|
||||
|
||||
GaussianConditional::rsd_type init_matrices(Rinit, init_dims.begin(), init_dims.end());
|
||||
vector<size_t> init_keys; init_keys += 3, 4, 5, 6;
|
||||
GaussianConditional::shared_ptr initConditional(new
|
||||
GaussianConditional(init_keys.begin(), init_keys.end(), 4, init_matrices, sigmas));
|
||||
|
||||
// Calculate a solution
|
||||
VectorValues solution;
|
||||
solution.insert(0, zero(3));
|
||||
solution.insert(1, zero(3));
|
||||
solution.insert(2, zero(3));
|
||||
solution.insert(3, zero(3));
|
||||
solution.insert(4, zero(3));
|
||||
solution.insert(5, zero(2));
|
||||
solution.insert(6, zero(2));
|
||||
|
||||
initConditional->solveInPlace(solution);
|
||||
|
||||
// Perform summarization
|
||||
std::set<Index> saved_indices;
|
||||
saved_indices += 5, 6;
|
||||
|
||||
GaussianConditional::shared_ptr actSummarized = conditionDensity(initConditional, saved_indices, solution);
|
||||
CHECK(actSummarized);
|
||||
|
||||
// Create expected value on [5], [6]
|
||||
Matrix Rexp = Matrix_(4, 5,
|
||||
2.73892865, 0.0242046766, -0.0459727048, 0.0445071938, -0.0284011,
|
||||
0.0, 2.61246954, 0.02287419, -0.102870789, 0.0275465,
|
||||
0.0, 0.0, 2.04823446, -0.302033014, 0.0439795,
|
||||
0.0, 0.0, 0.0, 2.24068986, -0.0222134);
|
||||
Vector expsigmas = ones(4);
|
||||
|
||||
vector<size_t> exp_dims; exp_dims += 2, 2, 1;
|
||||
GaussianConditional::rsd_type exp_matrices(Rexp, exp_dims.begin(), exp_dims.end());
|
||||
vector<size_t> exp_keys; exp_keys += 5, 6;
|
||||
GaussianConditional expConditional(exp_keys.begin(), exp_keys.end(), 2, exp_matrices, expsigmas);
|
||||
|
||||
EXPECT(assert_equal(expConditional, *actSummarized, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -7,6 +7,7 @@ set(nonlinear_local_libs
|
|||
nonlinear_unstable
|
||||
nonlinear
|
||||
linear
|
||||
linear_unstable
|
||||
inference
|
||||
geometry
|
||||
base
|
||||
|
|
|
|||
|
|
@ -0,0 +1,60 @@
|
|||
/**
|
||||
* @file summarization.cpp
|
||||
*
|
||||
* @date Jun 22, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam_unstable/nonlinear/summarization.h>
|
||||
#include <gtsam_unstable/linear/bayesTreeOperations.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::shared_ptr summarizeGraphSequential(
|
||||
const GaussianFactorGraph& full_graph, const std::vector<Index>& indices, bool useQR) {
|
||||
GaussianSequentialSolver solver(full_graph, useQR);
|
||||
return solver.jointFactorGraph(indices);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::shared_ptr summarizeGraphSequential(
|
||||
const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR) {
|
||||
std::vector<Index> indices;
|
||||
BOOST_FOREACH(const Key& k, saved_keys)
|
||||
indices.push_back(ordering[k]);
|
||||
return summarizeGraphSequential(full_graph, indices, useQR);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<GaussianFactorGraph,Ordering>
|
||||
partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values,
|
||||
const KeySet& overlap_keys) {
|
||||
// compute a new ordering with non-saved keys first
|
||||
Ordering ordering;
|
||||
KeySet eliminatedKeys;
|
||||
BOOST_FOREACH(const Key& key, values.keys()) {
|
||||
if (!overlap_keys.count(key)) {
|
||||
eliminatedKeys.insert(key);
|
||||
ordering += key;
|
||||
}
|
||||
}
|
||||
|
||||
BOOST_FOREACH(const Key& key, overlap_keys)
|
||||
ordering += key;
|
||||
|
||||
// reorder the system
|
||||
GaussianFactorGraph linearSystem = *graph.linearize(values, ordering);
|
||||
|
||||
// Eliminate frontals
|
||||
GaussianFactorGraph summarized_system;
|
||||
summarized_system.push_back(EliminateCholesky(linearSystem, eliminatedKeys.size()).second);
|
||||
return make_pair(summarized_system, ordering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
/**
|
||||
* @file summarization.h
|
||||
*
|
||||
* @brief Types and utility functions for summarization
|
||||
*
|
||||
* @date Jun 22, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Sequential graph summarization
|
||||
typedef FactorGraph<JacobianFactor> JacobianGraph;
|
||||
typedef boost::shared_ptr<JacobianGraph> shared_jacobianGraph;
|
||||
|
||||
/**
|
||||
* Summarization function that eliminates a set of variables (does not convert to Jacobians)
|
||||
* NOTE: uses sequential solver - requires fully constrained system
|
||||
*/
|
||||
GaussianFactorGraph::shared_ptr summarizeGraphSequential(
|
||||
const GaussianFactorGraph& full_graph, const std::vector<Index>& indices, bool useQR = false);
|
||||
|
||||
/** Summarization that also converts keys to indices */
|
||||
GaussianFactorGraph::shared_ptr summarizeGraphSequential(
|
||||
const GaussianFactorGraph& full_graph, const Ordering& ordering,
|
||||
const KeySet& saved_keys, bool useQR = false);
|
||||
|
||||
/**
|
||||
* Summarization function to remove a subset of variables from a system using
|
||||
* a partial cholesky approach. This does not require that the system be fully constrained.
|
||||
*
|
||||
* Performs linearization to apply an ordering
|
||||
*/
|
||||
std::pair<GaussianFactorGraph,Ordering>
|
||||
partialCholeskySummarization(const NonlinearFactorGraph& graph, const Values& values,
|
||||
const KeySet& overlap_keys);
|
||||
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
Loading…
Reference in New Issue