12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Removed unnecessary summarization files in gtsam_unstable, restricted compilation of gtsam_unstable for components needing iSAM2 - core gtsam_unstable library builds

release/4.3a0
Alex Cunningham 2013-08-08 20:08:52 +00:00
parent 2adfe5ffd3
commit 883c870dff
11 changed files with 105 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -2436,6 +2436,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="gtsam_unstable-shared" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>gtsam_unstable-shared</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="gtsam_unstable-static" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>gtsam_unstable-static</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>

18
gtsam_unstable/CMakeLists.txt
View File

@ -7,7 +7,7 @@ set (gtsam_unstable_subdirs
linear
dynamics
nonlinear
slam
#slam
)
set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES})
@ -19,10 +19,26 @@ add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-fail
# Sources to remove from builds
set (excluded_sources # "")
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/BatchFixedLagSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchFilter.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentFilteringAndSmoothing.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/FixedLagSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/IncrementalFixedLagSmoother.cpp"
# "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/LinearizedFactor.cpp"
)
set (excluded_headers # "")
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/BatchFixedLagSmoother.h"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchFilter.h"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentBatchSmoother.h"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/ConcurrentFilteringAndSmoothing.h"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/FixedLagSmoother.h"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/IncrementalFixedLagSmoother.h"
# "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/LinearizedFactor.h"
)
# assemble core libaries

18
gtsam_unstable/linear/bayesTreeOperations.cpp
View File

@ -13,14 +13,6 @@ 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;
@ -55,9 +47,9 @@ GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) {
size_t nrRows = std::min(rowsRemaining, varDim);
// Extract submatrices
std::vector<std::pair<Index, Matrix> > matrices;
std::vector<std::pair<Key, Matrix> > matrices;
for (colIt = rowIt; colIt != jf->end(); ++colIt) {
Index idx = *colIt;
Key idx = *colIt;
const Matrix subA = jf->getA(colIt).middleRows(startRow, nrRows);
matrices.push_back(make_pair(idx, subA));
}
@ -103,11 +95,11 @@ void findCliques(const GaussianBayesTree::sharedClique& current_clique,
/* ************************************************************************* */
std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTree& bayesTree, const std::set<Index>& savedIndices) {
const GaussianBayesTree& bayesTree, const std::set<Key>& 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];
BOOST_FOREACH(const Key& index, savedIndices) {
GaussianBayesTree::sharedClique clique = bayesTree[index];
// add path back to root to affected set
findCliques(clique, affected_cliques);

12
gtsam_unstable/linear/bayesTreeOperations.h
View File

@ -12,15 +12,9 @@
#include <gtsam_unstable/base/dllexport.h>
#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 */
GTSAM_UNSTABLE_EXPORT std::set<Index> keysToIndices(const KeySet& keys, const Ordering& ordering);
// Linear Graph Operations
/**
@ -47,7 +41,7 @@ GTSAM_UNSTABLE_EXPORT GaussianFactorGraph removePriors(const GaussianFactorGraph
* @return the set of conditionals extracted from cliques.
*/
GTSAM_UNSTABLE_EXPORT std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTree& bayesTree, const std::set<Index>& savedIndices);
const GaussianBayesTree& bayesTree, const std::set<Key>& savedIndices);
/**
* Recursively traverses from a given clique in a Bayes Tree and collects all of the conditionals
@ -78,9 +72,9 @@ GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool s
if (root && root->conditional()) {
GaussianConditional::shared_ptr conditional = root->conditional();
if (!splitConditionals)
result.push_back(conditional->toFactor());
result.push_back(conditional);
else
result.push_back(splitFactor(conditional->toFactor()));
result.push_back(splitFactor(conditional));
}
BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, root->children())
result.push_back(liquefy<BAYESTREE>(child, splitConditionals));

40
gtsam_unstable/linear/conditioning.cpp
View File

@ -8,13 +8,16 @@
#include <gtsam_unstable/linear/conditioning.h>
#include <gtsam_unstable/linear/bayesTreeOperations.h>
#include <boost/assign/std/vector.hpp>
using namespace std;
using namespace boost::assign;
namespace gtsam {
/* ************************************************************************* */
GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shared_ptr& initConditional,
const std::set<Index>& saved_indices, const VectorValues& solution) {
const std::set<Key>& saved_indices, const VectorValues& solution) {
const bool verbose = false;
if (!initConditional)
@ -26,12 +29,12 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
}
// Check for presence of variables to remove
std::set<Index> frontalsToRemove, parentsToRemove;
BOOST_FOREACH(const Index& frontal, initConditional->frontals())
std::set<Key> frontalsToRemove, parentsToRemove;
BOOST_FOREACH(const Key& frontal, initConditional->frontals())
if (!saved_indices.count(frontal))
frontalsToRemove.insert(frontal);
BOOST_FOREACH(const Index& parent, initConditional->parents())
BOOST_FOREACH(const Key& parent, initConditional->parents())
if (!saved_indices.count(parent))
parentsToRemove.insert(parent);
@ -50,14 +53,14 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
size_t oldOffset = 0;
vector<size_t> newDims, oldDims;
vector<size_t> oldColOffsets;
vector<Index> newIndices;
vector<Key> newIndices;
vector<size_t> newIdxToOldIdx; // Access to arrays, maps from new var to old var
const vector<Index>& oldIndices = initConditional->keys();
const vector<Key>& 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);
size_t varDim = initConditional->getDim(varIt);
oldDims += varDim;
if (!frontalsToRemove.count(*varIt)) {
newTotalCols += varDim;
@ -73,7 +76,7 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
}
varIt = initConditional->beginParents();
for (; varIt != initConditional->endParents(); ++varIt) {
size_t varDim = initConditional->dim(varIt);
size_t varDim = initConditional->getDim(varIt);
oldDims += varDim;
if (!parentsToRemove.count(*varIt)) {
newTotalCols += varDim;
@ -99,7 +102,7 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
size_t oldColOffset = oldColOffsets.at(newfrontalIdx);
// get R block, sliced by row
Eigen::Block<GaussianConditional::rsd_type::constBlock> rblock =
Eigen::Block<GaussianConditional::constABlock> rblock =
initConditional->get_R().block(oldColOffset, oldColOffset, dim, oldRNrCols-oldColOffset);
if (verbose) cout << " rblock\n" << rblock << endl;
@ -113,14 +116,14 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
if (verbose) cout << " full_matrix: set rhs\n" << full_matrix << endl;
// set sigmas
sigmas.segment(newColOffset, dim) = initConditional->get_sigmas().segment(oldColOffset, dim);
sigmas.segment(newColOffset, dim) = initConditional->get_model()->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];
Key parentKey = oldIndices[oldIdx];
size_t parentDim = oldDims[oldIdx];
if (verbose) cout << " Adding parents from R: parentKey: " << parentKey << " parentDim: " << parentDim << endl;
if (!frontalsToRemove.count(parentKey)) {
@ -144,8 +147,8 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
// 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);
Key parentKey = *oldParent;
size_t parentDim = initConditional->getDim(oldParent);
if (verbose) cout << " Adding parents from S: parentKey: " << parentKey << " parentDim: " << parentDim << endl;
if (parentsToRemove.count(parentKey)) {
// Solve out the variable
@ -169,17 +172,18 @@ GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shar
// Construct a new conditional
if (verbose) cout << "backsubSummarize() Complete!" << endl;
GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end());
// GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end());
VerticalBlockMatrix matrices(newDims, full_matrix);
return GaussianConditional::shared_ptr(new
GaussianConditional(newIndices.begin(), newIndices.end(), newNrFrontals, matrices, sigmas));
GaussianConditional(newIndices, newNrFrontals, matrices, noiseModel::Diagonal::Sigmas(sigmas)));
}
/* ************************************************************************* */
GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree,
const std::set<Index>& saved_indices) {
const std::set<Key>& saved_indices) {
const bool verbose = false;
VectorValues solution = optimize(bayesTree);
VectorValues solution = bayesTree.optimize();
// FIXME: set of conditionals does not manage possibility of solving out whole separators
std::set<GaussianConditional::shared_ptr> affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices);
@ -191,7 +195,7 @@ GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree,
GaussianConditional::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution);
if (reducedConditional) {
if (verbose) reducedConditional->print("Final conditional");
summarized_graph.push_back(reducedConditional->toFactor());
summarized_graph.push_back(reducedConditional);
} else if (verbose) {
cout << "Conditional removed after summarization!" << endl;
}

11
gtsam_unstable/linear/conditioning.h
View File

@ -12,7 +12,6 @@
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
@ -27,16 +26,18 @@ namespace gtsam {
* @param saved_indices is the set of indices that should appear in the result
* @param solution is a full solution for the system
*/
GTSAM_UNSTABLE_EXPORT gtsam::GaussianConditional::shared_ptr conditionDensity(const gtsam::GaussianConditional::shared_ptr& initConditional,
const std::set<gtsam::Index>& saved_indices, const gtsam::VectorValues& solution);
GTSAM_UNSTABLE_EXPORT GaussianConditional::shared_ptr conditionDensity(
const GaussianConditional::shared_ptr& initConditional,
const std::set<Key>& saved_indices, const 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_UNSTABLE_EXPORT gtsam::GaussianFactorGraph conditionDensity(const gtsam::GaussianBayesTree& bayesTree,
const std::set<gtsam::Index>& saved_indices);
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph conditionDensity(
const GaussianBayesTree& bayesTree,
const std::set<Key>& saved_indices);
} // \namespace gtsam

9
gtsam_unstable/nonlinear/CMakeLists.txt
View File

@ -19,7 +19,14 @@ set (nonlinear_full_libs
${gtsam_unstable-default})
# Exclude tests that don't work
set (nonlinear_excluded_tests "")
set (nonlinear_excluded_tests #"")
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testBatchFixedLagSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testConcurrentBatchFilter.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testConcurrentBatchSmoother.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testIncrementalFixedLagSmoother.cpp"
#"${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/test/testLinearizedFactor.cpp"
)
# Add all tests
gtsam_add_subdir_tests(nonlinear_unstable "${nonlinear_local_libs}" "${nonlinear_full_libs}" "${nonlinear_excluded_tests}")

62
gtsam_unstable/nonlinear/LinearizedFactor.cpp
View File

@ -22,38 +22,31 @@
namespace gtsam {
/* ************************************************************************* */
LinearizedGaussianFactor::LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points) {
LinearizedGaussianFactor::LinearizedGaussianFactor(
const GaussianFactor::shared_ptr& gaussian, const Values& lin_points)
: NonlinearFactor(gaussian->keys())
{
// Extract the keys and linearization points
BOOST_FOREACH(const Index& idx, gaussian->keys()) {
// find full symbol
if (idx < ordering.size()) {
Key key = ordering.key(idx);
// extract linearization point
assert(lin_points.exists(key));
this->lin_points_.insert(key, lin_points.at(key));
// store keys
this->keys_.push_back(key);
} else {
throw std::runtime_error("LinearizedGaussianFactor: could not find index in decoder!");
}
BOOST_FOREACH(const Key& key, gaussian->keys()) {
// extract linearization point
assert(lin_points.exists(key));
this->lin_points_.insert(key, lin_points.at(key));
}
}
/* ************************************************************************* */
// LinearizedJacobianFactor
/* ************************************************************************* */
LinearizedJacobianFactor::LinearizedJacobianFactor() : Ab_(matrix_) {
}
/* ************************************************************************* */
LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian,
const Ordering& ordering, const Values& lin_points)
: Base(jacobian, ordering, lin_points), Ab_(matrix_) {
LinearizedJacobianFactor::LinearizedJacobianFactor(
const JacobianFactor::shared_ptr& jacobian, const Values& lin_points)
: Base(jacobian, lin_points), Ab_(matrix_) {
// Get the Ab matrix from the Jacobian factor, with any covariance baked in
AbMatrix fullMatrix = jacobian->matrix_augmented(true);
AbMatrix fullMatrix = jacobian->augmentedJacobian();
// Create the dims array
size_t *dims = (size_t *)alloca(sizeof(size_t) * (jacobian->size() + 1));
@ -110,12 +103,12 @@ double LinearizedJacobianFactor::error(const Values& c) const {
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
LinearizedJacobianFactor::linearize(const Values& c, const Ordering& ordering) const {
LinearizedJacobianFactor::linearize(const Values& c) const {
// Create the 'terms' data structure for the Jacobian constructor
std::vector<std::pair<Index, Matrix> > terms;
BOOST_FOREACH(Key key, keys()) {
terms.push_back(std::make_pair(ordering[key], this->A(key)));
terms.push_back(std::make_pair(key, this->A(key)));
}
// compute rhs
@ -140,19 +133,16 @@ Vector LinearizedJacobianFactor::error_vector(const Values& c) const {
return errorVector;
}
/* ************************************************************************* */
// LinearizedHessianFactor
/* ************************************************************************* */
LinearizedHessianFactor::LinearizedHessianFactor() : info_(matrix_) {
}
/* ************************************************************************* */
LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian,
const Ordering& ordering, const Values& lin_points)
: Base(hessian, ordering, lin_points), info_(matrix_) {
LinearizedHessianFactor::LinearizedHessianFactor(
const HessianFactor::shared_ptr& hessian, const Values& lin_points)
: Base(hessian, lin_points), info_(matrix_) {
// Copy the augmented matrix holding G, g, and f
Matrix fullMatrix = hessian->info();
@ -228,13 +218,7 @@ double LinearizedHessianFactor::error(const Values& c) const {
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) const {
// Use the ordering to convert the keys into indices;
std::vector<Index> js;
BOOST_FOREACH(Key key, this->keys()){
js.push_back(ordering.at(key));
}
LinearizedHessianFactor::linearize(const Values& c) const {
// Make a copy of the info matrix
Matrix newMatrix;
@ -275,7 +259,7 @@ LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) co
// Create a Hessian Factor from the modified info matrix
//return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo));
return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, Gs, gs, f));
return boost::shared_ptr<GaussianFactor>(new HessianFactor(keys(), Gs, gs, f));
}
} // \namespace aspn

18
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -19,7 +19,6 @@
#include <vector>
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
@ -51,10 +50,9 @@ public:
/**
* @param gaussian: A jacobian or hessian factor
* @param ordering: The full ordering used to linearize this factor
* @param lin_points: The linearization points for, at least, the variables used by this factor
*/
LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points);
LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points);
virtual ~LinearizedGaussianFactor() {};
@ -111,11 +109,9 @@ public:
/**
* @param jacobian: A jacobian factor
* @param ordering: The ordering used to linearize this factor
* @param lin_points: The linearization points for, at least, the variables used by this factor
*/
LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian,
const Ordering& ordering, const Values& lin_points);
LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, const Values& lin_points);
virtual ~LinearizedJacobianFactor() {}
@ -148,8 +144,7 @@ public:
* Reimplemented from NoiseModelFactor to directly copy out the
* matrices and only update the RHS b with an updated residual
*/
boost::shared_ptr<GaussianFactor> linearize(
const Values& c, const Ordering& ordering) const;
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
/** (A*x-b) */
Vector error_vector(const Values& c) const;
@ -204,11 +199,9 @@ public:
/**
* Use this constructor with the ordering used to linearize the graph
* @param hessian: A hessian factor
* @param ordering: The ordering used to linearize this factor
* @param lin_points: The linearization points for, at least, the variables used by this factor
*/
LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian,
const Ordering& ordering, const Values& lin_points);
LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, const Values& lin_points);
virtual ~LinearizedHessianFactor() {}
@ -270,8 +263,7 @@ public:
* Reimplemented from NoiseModelFactor to directly copy out the
* matrices and only update the RHS b with an updated residual
*/
boost::shared_ptr<GaussianFactor> linearize(
const Values& c, const Ordering& ordering) const;
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const;
private:
/** Serialization function */

34
gtsam_unstable/nonlinear/sequentialSummarization.cpp
View File

@ -1,34 +0,0 @@
/**
* @file summarization.cpp
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#include <gtsam_unstable/nonlinear/sequentialSummarization.h>
#include <boost/foreach.hpp>
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);
}
/* ************************************************************************* */
} // \namespace gtsam

34
gtsam_unstable/nonlinear/sequentialSummarization.h
View File

@ -1,34 +0,0 @@
/**
* @file summarization.h
*
* @brief Types and utility functions for summarization
*
* @date Jun 22, 2012
* @author Alex Cunningham
*/
#pragma once
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
/**
* Summarization function that eliminates a set of variables (does not convert to Jacobians)
* NOTE: uses sequential solver - requires fully constrained system
*/
GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const std::vector<Index>& indices, bool useQR = false);
/** Summarization that also converts keys to indices */
GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const Ordering& ordering,
const KeySet& saved_keys, bool useQR = false);
} // \namespace gtsam