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

Merged in feature/betterOrdering (pull request #100) 

Fix Ordering
release/4.3a0
Frank Dellaert 2015-06-28 17:04:53 -07:00
parent 8d5657cd5e 464a82cc51
commit f9d139b2db
18 changed files with 785 additions and 470 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
examples/SFMExample_bal_COLAMD_METIS.cpp Normal file
View File

@ -0,0 +1,144 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file SFMExample.cpp
* @brief This file is to compare the ordering performance for COLAMD vs METIS.
* Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
* @author Frank Dellaert, Zhaoyang Lv
*/
// For an explanation of headers, see SFMExample.cpp
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/base/timing.h>
#include <vector>
using namespace std;
using namespace gtsam;
using symbol_shorthand::C;
using symbol_shorthand::P;
// We will be using a projection factor that ties a SFM_Camera to a 3D point.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
/* ************************************************************************* */
int main (int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfM_data mydata;
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/
LevenbergMarquardtParams params_using_COLAMD, params_using_METIS;
try {
params_using_METIS.setVerbosity("ERROR");
gttic_(METIS_ORDERING);
params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph);
gttoc_(METIS_ORDERING);
params_using_COLAMD.setVerbosity("ERROR");
gttic_(COLAMD_ORDERING);
params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph);
gttoc_(COLAMD_ORDERING);
} catch (exception& e) {
cout << e.what();
}
// expect they have different ordering results
if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
cout << "COLAMD and METIS produce the same ordering. "
<< "Problem here!!!" << endl;
}
/* Optimize the graph with METIS and COLAMD and time the results */
Values result_METIS, result_COLAMD;
try {
gttic_(OPTIMIZE_WITH_METIS);
LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS);
result_METIS = lm_METIS.optimize();
gttoc_(OPTIMIZE_WITH_METIS);
gttic_(OPTIMIZE_WITH_COLAMD);
LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD);
result_COLAMD = lm_COLAMD.optimize();
gttoc_(OPTIMIZE_WITH_COLAMD);
} catch (exception& e) {
cout << e.what();
}
{ // printing the result
cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
cout << "METIS final error: " << graph.error(result_METIS) << endl;
cout << endl << endl;
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") \
% mydata.number_tracks() % mydata.number_cameras() \
<< endl;
tictoc_print_();
}
return 0;
}
/* ************************************************************************* */

2
examples/SolverComparer.cpp
View File

@ -576,7 +576,7 @@ void runStats()
{
cout << "Gathering statistics..." << endl;
GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear)));
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear)));
treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
ofstream file;

18
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -55,9 +55,9 @@ namespace gtsam {
// have a VariableIndex already here because we computed one if needed in the previous 'else'
// block.
if (orderingType == Ordering::METIS)
return eliminateSequential(Ordering::metis(asDerived()), function, variableIndex, orderingType);
return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
else
return eliminateSequential(Ordering::colamd(*variableIndex), function, variableIndex, orderingType);
return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
}
}
@ -93,9 +93,9 @@ namespace gtsam {
// have a VariableIndex already here because we computed one if needed in the previous 'else'
// block.
if (orderingType == Ordering::METIS)
return eliminateMultifrontal(Ordering::metis(asDerived()), function, variableIndex, orderingType);
return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType);
else
return eliminateMultifrontal(Ordering::colamd(*variableIndex), function, variableIndex, orderingType);
return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType);
}
}
@ -125,7 +125,7 @@ namespace gtsam {
if(variableIndex) {
gttic(eliminatePartialSequential);
// Compute full ordering
Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables);
Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
// Split off the part of the ordering for the variables being eliminated
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
@ -163,7 +163,7 @@ namespace gtsam {
if(variableIndex) {
gttic(eliminatePartialMultifrontal);
// Compute full ordering
Ordering fullOrdering = Ordering::colamdConstrainedFirst(*variableIndex, variables);
Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
// Split off the part of the ordering for the variables being eliminated
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
@ -216,7 +216,7 @@ namespace gtsam {
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
Ordering totalOrdering =
Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
// Split up ordering
const size_t nVars = variablesOrOrdering->size();
@ -275,7 +275,7 @@ namespace gtsam {
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables);
Ordering totalOrdering =
Ordering::colamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
// Split up ordering
const size_t nVars = variablesOrOrdering->size();
@ -301,7 +301,7 @@ namespace gtsam {
if(variableIndex)
{
// Compute a total ordering for all variables
Ordering totalOrdering = Ordering::colamdConstrainedLast(*variableIndex, variables);
Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables);
// Split out the part for the marginalized variables
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());

3
gtsam/inference/EliminateableFactorGraph.h
View File

@ -128,7 +128,8 @@ namespace gtsam {
OptionalOrderingType orderingType = boost::none) const;
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
* provided, the ordering provided by COLAMD will be used.
* provided, the ordering will be computed using either COLAMD or METIS, dependeing on
* the parameter orderingType (Ordering::COLAMD or Ordering::METIS)
*
* <b> Example - Full Cholesky elimination in COLAMD order: </b>
* \code

2
gtsam/inference/ISAM-inst.h
View File

@ -46,7 +46,7 @@ namespace gtsam {
const VariableIndex varIndex(factors);
const KeySet newFactorKeys = newFactors.keys();
const Ordering constrainedOrdering =
Ordering::colamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
Ordering::ColamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());

148
gtsam/inference/Ordering.cpp
View File

@ -29,33 +29,32 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
FastMap<Key, size_t> Ordering::invert() const
{
/* ************************************************************************* */
FastMap<Key, size_t> Ordering::invert() const {
FastMap<Key, size_t> inverted;
for(size_t pos = 0; pos < this->size(); ++pos)
for (size_t pos = 0; pos < this->size(); ++pos)
inverted.insert(make_pair((*this)[pos], pos));
return inverted;
}
}
/* ************************************************************************* */
Ordering Ordering::colamd(const VariableIndex& variableIndex)
{
/* ************************************************************************* */
Ordering Ordering::Colamd(const VariableIndex& variableIndex) {
// Call constrained version with all groups set to zero
vector<int> dummy_groups(variableIndex.size(), 0);
return Ordering::colamdConstrained(variableIndex, dummy_groups);
}
return Ordering::ColamdConstrained(variableIndex, dummy_groups);
}
/* ************************************************************************* */
Ordering Ordering::colamdConstrained(
const VariableIndex& variableIndex, std::vector<int>& cmember)
{
/* ************************************************************************* */
Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
std::vector<int>& cmember) {
gttic(Ordering_COLAMDConstrained);
gttic(Prepare);
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
size_t nEntries = variableIndex.nEntries(), nFactors =
variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */
size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors,
(int) nVars); /* colamd arg 3: size of the array A */
vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
@ -69,14 +68,14 @@ namespace gtsam {
const VariableIndex::Factors& column = key_factors.second;
size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(size_t factorIndex, column) {
if(lastFactorId != numeric_limits<size_t>::max())
if (lastFactorId != numeric_limits<size_t>::max())
assert(factorIndex > lastFactorId);
A[count++] = (int)factorIndex; // copy sparse column
A[count++] = (int) factorIndex; // copy sparse column
}
p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
// Store key in array and increment index
keys[index] = key_factors.first;
++ index;
++index;
}
assert((size_t)count == variableIndex.nEntries());
@ -84,8 +83,8 @@ namespace gtsam {
//double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
double knobs[CCOLAMD_KNOBS];
ccolamd_set_defaults(knobs);
knobs[CCOLAMD_DENSE_ROW]=-1;
knobs[CCOLAMD_DENSE_COL]=-1;
knobs[CCOLAMD_DENSE_ROW] = -1;
knobs[CCOLAMD_DENSE_COL] = -1;
int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
@ -93,11 +92,13 @@ namespace gtsam {
// call colamd, result will be in p
/* returns (1) if successful, (0) otherwise*/
if(nVars > 0) {
if (nVars > 0) {
gttic(ccolamd);
int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]);
if(rv != 1)
throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0],
knobs, stats, &cmember[0]);
if (rv != 1)
throw runtime_error(
(boost::format("ccolamd failed with return value %1%") % rv).str());
}
// ccolamd_report(stats);
@ -106,17 +107,16 @@ namespace gtsam {
// Convert elimination ordering in p to an ordering
Ordering result;
result.resize(nVars);
for(size_t j = 0; j < nVars; ++j)
for (size_t j = 0; j < nVars; ++j)
result[j] = keys[p[j]];
gttoc(Fill_Ordering);
return result;
}
}
/* ************************************************************************* */
Ordering Ordering::colamdConstrainedLast(
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, bool forceOrder)
{
/* ************************************************************************* */
Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
const std::vector<Key>& constrainLast, bool forceOrder) {
gttic(Ordering_COLAMDConstrainedLast);
size_t n = variableIndex.size();
@ -133,17 +133,16 @@ namespace gtsam {
int group = (constrainLast.size() != n ? 1 : 0);
BOOST_FOREACH(Key key, constrainLast) {
cmember[keyIndices.at(key)] = group;
if(forceOrder)
++ group;
if (forceOrder)
++group;
}
return Ordering::colamdConstrained(variableIndex, cmember);
}
return Ordering::ColamdConstrained(variableIndex, cmember);
}
/* ************************************************************************* */
Ordering Ordering::colamdConstrainedFirst(
const VariableIndex& variableIndex, const std::vector<Key>& constrainFirst, bool forceOrder)
{
/* ************************************************************************* */
Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
const std::vector<Key>& constrainFirst, bool forceOrder) {
gttic(Ordering_COLAMDConstrainedFirst);
const int none = -1;
@ -161,23 +160,22 @@ namespace gtsam {
int group = 0;
BOOST_FOREACH(Key key, constrainFirst) {
cmember[keyIndices.at(key)] = group;
if(forceOrder)
++ group;
if (forceOrder)
++group;
}
if(!forceOrder && !constrainFirst.empty())
++ group;
if (!forceOrder && !constrainFirst.empty())
++group;
BOOST_FOREACH(int& c, cmember)
if(c == none)
if (c == none)
c = group;
return Ordering::colamdConstrained(variableIndex, cmember);
}
return Ordering::ColamdConstrained(variableIndex, cmember);
}
/* ************************************************************************* */
Ordering Ordering::colamdConstrained(const VariableIndex& variableIndex,
const FastMap<Key, int>& groups)
{
/* ************************************************************************* */
Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
const FastMap<Key, int>& groups) {
gttic(Ordering_COLAMDConstrained);
size_t n = variableIndex.size();
std::vector<int> cmember(n, 0);
@ -195,13 +193,11 @@ namespace gtsam {
cmember[keyIndices.at(p.first)] = p.second;
}
return Ordering::colamdConstrained(variableIndex, cmember);
}
return Ordering::ColamdConstrained(variableIndex, cmember);
}
/* ************************************************************************* */
Ordering Ordering::metis(const MetisIndex& met)
{
/* ************************************************************************* */
Ordering Ordering::Metis(const MetisIndex& met) {
gttic(Ordering_METIS);
vector<idx_t> xadj = met.xadj();
@ -209,62 +205,60 @@ namespace gtsam {
vector<idx_t> perm, iperm;
idx_t size = met.nValues();
for (idx_t i = 0; i < size; i++)
{
for (idx_t i = 0; i < size; i++) {
perm.push_back(0);
iperm.push_back(0);
}
int outputError;
outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], &iperm[0]);
outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0],
&iperm[0]);
Ordering result;
if (outputError != METIS_OK)
{
if (outputError != METIS_OK) {
std::cout << "METIS failed during Nested Dissection ordering!\n";
return result;
}
result.resize(size);
for (size_t j = 0; j < (size_t)size; ++j){
for (size_t j = 0; j < (size_t) size; ++j) {
// We have to add the minKey value back to obtain the original key in the Values
result[j] = met.intToKey(perm[j]);
}
return result;
}
}
/* ************************************************************************* */
void Ordering::print(const std::string& str, const KeyFormatter& keyFormatter) const
{
/* ************************************************************************* */
void Ordering::print(const std::string& str,
const KeyFormatter& keyFormatter) const {
cout << str;
// Print ordering in index order
// Print the ordering with varsPerLine ordering entries printed on each line,
// for compactness.
static const size_t varsPerLine = 10;
bool endedOnNewline = false;
for(size_t i = 0; i < size(); ++i) {
if(i % varsPerLine == 0)
for (size_t i = 0; i < size(); ++i) {
if (i % varsPerLine == 0)
cout << "Position " << i << ": ";
if(i % varsPerLine != 0)
if (i % varsPerLine != 0)
cout << ", ";
cout << keyFormatter(at(i));
if(i % varsPerLine == varsPerLine - 1) {
if (i % varsPerLine == varsPerLine - 1) {
cout << "\n";
endedOnNewline = true;
} else {
endedOnNewline = false;
}
}
if(!endedOnNewline)
if (!endedOnNewline)
cout << "\n";
cout.flush();
}
}
/* ************************************************************************* */
bool Ordering::equals(const Ordering& other, double tol) const
{
/* ************************************************************************* */
bool Ordering::equals(const Ordering& other, double tol) const {
return (*this) == other;
}
}
}

120
gtsam/inference/Ordering.h
View File

@ -30,36 +30,43 @@
namespace gtsam {
class Ordering : public std::vector<Key> {
protected:
class Ordering: public std::vector<Key> {
protected:
typedef std::vector<Key> Base;
public:
public:
/// Type of ordering to use
enum OrderingType {
COLAMD, METIS, CUSTOM
COLAMD, METIS, NATURAL, CUSTOM
};
typedef Ordering This; ///< Typedef to this class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
/// Create an empty ordering
GTSAM_EXPORT Ordering() {}
GTSAM_EXPORT
Ordering() {
}
/// Create from a container
template<typename KEYS>
explicit Ordering(const KEYS& keys) : Base(keys.begin(), keys.end()) {}
explicit Ordering(const KEYS& keys) :
Base(keys.begin(), keys.end()) {
}
/// Create an ordering using iterators over keys
template<typename ITERATOR>
Ordering(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {}
Ordering(ITERATOR firstKey, ITERATOR lastKey) :
Base(firstKey, lastKey) {
}
/// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling
/// push_back.
boost::assign::list_inserter<boost::assign_detail::call_push_back<This> >
operator+=(Key key) {
return boost::assign::make_list_inserter(boost::assign_detail::call_push_back<This>(*this))(key);
boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=(
Key key) {
return boost::assign::make_list_inserter(
boost::assign_detail::call_push_back<This>(*this))(key);
}
/// Invert (not reverse) the ordering - returns a map from key to order position
@ -71,11 +78,12 @@ namespace gtsam {
/// performance). This internally builds a VariableIndex so if you already have a VariableIndex,
/// it is faster to use COLAMD(const VariableIndex&)
template<class FACTOR>
static Ordering colamd(const FactorGraph<FACTOR>& graph) {
return colamd(VariableIndex(graph)); }
static Ordering Colamd(const FactorGraph<FACTOR>& graph) {
return Colamd(VariableIndex(graph));
}
/// Compute a fill-reducing ordering using COLAMD from a VariableIndex.
static GTSAM_EXPORT Ordering colamd(const VariableIndex& variableIndex);
static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
/// for note on performance). This internally builds a VariableIndex so if you already have a
@ -86,9 +94,11 @@ namespace gtsam {
/// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
template<class FACTOR>
static Ordering colamdConstrainedLast(const FactorGraph<FACTOR>& graph,
static Ordering ColamdConstrainedLast(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainLast, bool forceOrder = false) {
return colamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder); }
return ColamdConstrainedLast(VariableIndex(graph), constrainLast,
forceOrder);
}
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
/// function constrains the variables in \c constrainLast to the end of the ordering, and orders
@ -96,8 +106,9 @@ namespace gtsam {
/// variables in \c constrainLast will be ordered in the same order specified in the vector<Key>
/// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
static GTSAM_EXPORT Ordering colamdConstrainedLast(const VariableIndex& variableIndex,
const std::vector<Key>& constrainLast, bool forceOrder = false);
static GTSAM_EXPORT Ordering ColamdConstrainedLast(
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast,
bool forceOrder = false);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
/// for note on performance). This internally builds a VariableIndex so if you already have a
@ -108,9 +119,11 @@ namespace gtsam {
/// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be
/// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
template<class FACTOR>
static Ordering colamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
static Ordering ColamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainFirst, bool forceOrder = false) {
return colamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder); }
return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst,
forceOrder);
}
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
/// function constrains the variables in \c constrainFirst to the front of the ordering, and
@ -119,7 +132,8 @@ namespace gtsam {
/// vector<Key> \c constrainFirst. If \c forceOrder is false, the variables in \c
/// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
/// reduce fill-in as well.
static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex,
static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
const VariableIndex& variableIndex,
const std::vector<Key>& constrainFirst, bool forceOrder = false);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
@ -132,9 +146,10 @@ namespace gtsam {
/// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the
/// CCOLAMD documentation for more information.
template<class FACTOR>
static Ordering colamdConstrained(const FactorGraph<FACTOR>& graph,
static Ordering ColamdConstrained(const FactorGraph<FACTOR>& graph,
const FastMap<Key, int>& groups) {
return colamdConstrained(VariableIndex(graph), groups); }
return ColamdConstrained(VariableIndex(graph), groups);
}
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. In this
/// function, a group for each variable should be specified in \c groups, and each group of
@ -143,11 +158,11 @@ namespace gtsam {
/// appear in \c groups in arbitrary order. Any variables not present in \c groups will be
/// assigned to group 0. This function simply fills the \c cmember argument to CCOLAMD with the
/// supplied indices, see the CCOLAMD documentation for more information.
static GTSAM_EXPORT Ordering colamdConstrained(const VariableIndex& variableIndex,
const FastMap<Key, int>& groups);
static GTSAM_EXPORT Ordering ColamdConstrained(
const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
/// Return a natural Ordering. Typically used by iterative solvers
template <class FACTOR>
template<class FACTOR>
static Ordering Natural(const FactorGraph<FACTOR> &fg) {
KeySet src = fg.keys();
std::vector<Key> keys(src.begin(), src.end());
@ -157,43 +172,70 @@ namespace gtsam {
/// METIS Formatting function
template<class FACTOR>
static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj, std::vector<int>& adj, const FactorGraph<FACTOR>& graph);
static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
std::vector<int>& adj, const FactorGraph<FACTOR>& graph);
/// Compute an ordering determined by METIS from a VariableIndex
static GTSAM_EXPORT Ordering metis(const MetisIndex& met);
static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
template<class FACTOR>
static Ordering metis(const FactorGraph<FACTOR>& graph)
{
return metis(MetisIndex(graph));
static Ordering Metis(const FactorGraph<FACTOR>& graph) {
return Metis(MetisIndex(graph));
}
/// @}
/// @name Named Constructors @{
template<class FACTOR>
static Ordering Create(OrderingType orderingType,
const FactorGraph<FACTOR>& graph) {
switch (orderingType) {
case COLAMD:
return Colamd(graph);
case METIS:
return Metis(graph);
case NATURAL:
return Natural(graph);
case CUSTOM:
throw std::runtime_error(
"Ordering::Create error: called with CUSTOM ordering type.");
default:
throw std::runtime_error(
"Ordering::Create error: called with unknown ordering type.");
}
}
/// @}
/// @name Testable @{
GTSAM_EXPORT void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
GTSAM_EXPORT
void print(const std::string& str = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
GTSAM_EXPORT bool equals(const Ordering& other, double tol = 1e-9) const;
GTSAM_EXPORT
bool equals(const Ordering& other, double tol = 1e-9) const;
/// @}
private:
private:
/// Internal COLAMD function
static GTSAM_EXPORT Ordering colamdConstrained(
static GTSAM_EXPORT Ordering ColamdConstrained(
const VariableIndex& variableIndex, std::vector<int>& cmember);
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
};
/// traits
template<> struct traits<Ordering> : public Testable<Ordering> {};
/// traits
template<> struct traits<Ordering> : public Testable<Ordering> {
};
}

120
gtsam/inference/tests/testOrdering.cpp
View File

@ -28,45 +28,47 @@ using namespace std;
using namespace gtsam;
using namespace boost::assign;
namespace example {
SymbolicFactorGraph symbolicChain() {
SymbolicFactorGraph sfg;
sfg.push_factor(0, 1);
sfg.push_factor(1, 2);
sfg.push_factor(2, 3);
sfg.push_factor(3, 4);
sfg.push_factor(4, 5);
return sfg;
}
}
/* ************************************************************************* */
TEST(Ordering, constrained_ordering) {
SymbolicFactorGraph sfg;
// create graph with wanted variable set = 2, 4
sfg.push_factor(0,1);
sfg.push_factor(1,2);
sfg.push_factor(2,3);
sfg.push_factor(3,4);
sfg.push_factor(4,5);
SymbolicFactorGraph sfg = example::symbolicChain();
// unconstrained version
Ordering actUnconstrained = Ordering::colamd(sfg);
Ordering actUnconstrained = Ordering::Colamd(sfg);
Ordering expUnconstrained = Ordering(list_of(0)(1)(2)(3)(4)(5));
EXPECT(assert_equal(expUnconstrained, actUnconstrained));
// constrained version - push one set to the end
Ordering actConstrained = Ordering::colamdConstrainedLast(sfg, list_of(2)(4));
Ordering actConstrained = Ordering::ColamdConstrainedLast(sfg, list_of(2)(4));
Ordering expConstrained = Ordering(list_of(0)(1)(5)(3)(4)(2));
EXPECT(assert_equal(expConstrained, actConstrained));
// constrained version - push one set to the start
Ordering actConstrained2 = Ordering::colamdConstrainedFirst(sfg, list_of(2)(4));
Ordering actConstrained2 = Ordering::ColamdConstrainedFirst(sfg,
list_of(2)(4));
Ordering expConstrained2 = Ordering(list_of(2)(4)(0)(1)(3)(5));
EXPECT(assert_equal(expConstrained2, actConstrained2));
}
/* ************************************************************************* */
TEST(Ordering, grouped_constrained_ordering) {
SymbolicFactorGraph sfg;
// create graph with constrained groups:
// 1: 2, 4
// 2: 5
sfg.push_factor(0,1);
sfg.push_factor(1,2);
sfg.push_factor(2,3);
sfg.push_factor(3,4);
sfg.push_factor(4,5);
SymbolicFactorGraph sfg = example::symbolicChain();
// constrained version - push one set to the end
FastMap<size_t, int> constraints;
@ -74,7 +76,7 @@ TEST(Ordering, grouped_constrained_ordering) {
constraints[4] = 1;
constraints[5] = 2;
Ordering actConstrained = Ordering::colamdConstrained(sfg, constraints);
Ordering actConstrained = Ordering::ColamdConstrained(sfg, constraints);
Ordering expConstrained = list_of(0)(1)(3)(2)(4)(5);
EXPECT(assert_equal(expConstrained, actConstrained));
}
@ -111,9 +113,7 @@ TEST(Ordering, csr_format) {
vector<int> xadjExpected, adjExpected;
xadjExpected += 0, 2, 5, 8, 11, 13, 16, 20, 24, 28, 31, 33, 36, 39, 42, 44;
adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11,
2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11,
13, 8, 12, 14, 9, 13 ;
adjExpected += 1, 5, 0, 2, 6, 1, 3, 7, 2, 4, 8, 3, 9, 0, 6, 10, 1, 5, 7, 11, 2, 6, 8, 12, 3, 7, 9, 13, 4, 8, 14, 5, 11, 6, 10, 12, 7, 11, 13, 8, 12, 14, 9, 13;
EXPECT(xadjExpected == mi.xadj());
EXPECT(adjExpected.size() == mi.adj().size());
@ -140,7 +140,6 @@ TEST(Ordering, csr_format_2) {
EXPECT(xadjExpected == mi.xadj());
EXPECT(adjExpected.size() == mi.adj().size());
EXPECT(adjExpected == mi.adj());
}
/* ************************************************************************* */
@ -170,7 +169,6 @@ TEST(Ordering, csr_format_3) {
EXPECT(xadjExpected == mi.xadj());
EXPECT(adjExpected.size() == mi.adj().size());
EXPECT(adjExpected == adjAcutal);
}
/* ************************************************************************* */
@ -197,7 +195,7 @@ TEST(Ordering, csr_format_4) {
EXPECT(adjExpected.size() == mi.adj().size());
EXPECT(adjExpected == adjAcutal);
Ordering metOrder = Ordering::metis(sfg);
Ordering metOrder = Ordering::Metis(sfg);
// Test different symbol types
sfg.push_factor(Symbol('l', 1));
@ -206,8 +204,7 @@ TEST(Ordering, csr_format_4) {
sfg.push_factor(Symbol('x', 3), Symbol('l', 1));
sfg.push_factor(Symbol('x', 4), Symbol('l', 1));
Ordering metOrder2 = Ordering::metis(sfg);
Ordering metOrder2 = Ordering::Metis(sfg);
}
/* ************************************************************************* */
@ -229,8 +226,77 @@ TEST(Ordering, metis) {
EXPECT(adjExpected.size() == mi.adj().size());
EXPECT(adjExpected == mi.adj());
Ordering metis = Ordering::metis(sfg);
Ordering metis = Ordering::Metis(sfg);
}
/* ************************************************************************* */
TEST(Ordering, MetisLoop) {
// create linear graph
SymbolicFactorGraph sfg = example::symbolicChain();
// add loop closure
sfg.push_factor(0, 5);
// METIS
#if !defined(__APPLE__)
{
Ordering actual = Ordering::Create(Ordering::METIS, sfg);
// - P( 0 4 1)
// | - P( 2 | 4 1)
// | | - P( 3 | 4 2)
// | - P( 5 | 0 1)
Ordering expected = Ordering(list_of(3)(2)(5)(0)(4)(1));
EXPECT(assert_equal(expected, actual));
}
#else
{
Ordering actual = Ordering::Create(Ordering::METIS, sfg);
// - P( 1 0 3)
// | - P( 4 | 0 3)
// | | - P( 5 | 0 4)
// | - P( 2 | 1 3)
Ordering expected = Ordering(list_of(5)(4)(2)(1)(0)(3));
EXPECT(assert_equal(expected, actual));
}
#endif
}
/* ************************************************************************* */
TEST(Ordering, Create) {
// create chain graph
SymbolicFactorGraph sfg = example::symbolicChain();
// COLAMD
{
//- P( 4 5)
//| - P( 3 | 4)
//| | - P( 2 | 3)
//| | | - P( 1 | 2)
//| | | | - P( 0 | 1)
Ordering actual = Ordering::Create(Ordering::COLAMD, sfg);
Ordering expected = Ordering(list_of(0)(1)(2)(3)(4)(5));
EXPECT(assert_equal(expected, actual));
}
// METIS
{
Ordering actual = Ordering::Create(Ordering::METIS, sfg);
//- P( 1 0 2)
//| - P( 3 4 | 2)
//| | - P( 5 | 4)
Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2));
EXPECT(assert_equal(expected, actual));
}
// CUSTOM
CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM, sfg), runtime_error);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

4
gtsam/nonlinear/DoglegOptimizer.cpp
View File

@ -95,8 +95,8 @@ void DoglegOptimizer::iterate(void) {
/* ************************************************************************* */
DoglegParams DoglegOptimizer::ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const {
if(!params.ordering)
params.ordering = Ordering::colamd(graph);
if (!params.ordering)
params.ordering = Ordering::Create(params.orderingType, graph);
return params;
}

7
gtsam/nonlinear/GaussNewtonOptimizer.cpp
View File

@ -46,10 +46,9 @@ void GaussNewtonOptimizer::iterate() {
/* ************************************************************************* */
GaussNewtonParams GaussNewtonOptimizer::ensureHasOrdering(
GaussNewtonParams params, const NonlinearFactorGraph& graph) const
{
if(!params.ordering)
params.ordering = Ordering::colamd(graph);
GaussNewtonParams params, const NonlinearFactorGraph& graph) const {
if (!params.ordering)
params.ordering = Ordering::Create(params.orderingType, graph);
return params;
}

8
gtsam/nonlinear/ISAM2.cpp
View File

@ -341,7 +341,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
Ordering order;
if(constrainKeys)
{
order = Ordering::colamdConstrained(variableIndex_, *constrainKeys);
order = Ordering::ColamdConstrained(variableIndex_, *constrainKeys);
}
else
{
@ -351,11 +351,11 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
FastMap<Key, int> constraintGroups;
BOOST_FOREACH(Key var, observedKeys)
constraintGroups[var] = 1;
order = Ordering::colamdConstrained(variableIndex_, constraintGroups);
order = Ordering::ColamdConstrained(variableIndex_, constraintGroups);
}
else
{
order = Ordering::colamd(variableIndex_);
order = Ordering::Colamd(variableIndex_);
}
}
gttoc(ordering);
@ -481,7 +481,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
// Generate ordering
gttic(Ordering);
Ordering ordering = Ordering::colamdConstrained(affectedFactorsVarIndex, constraintGroups);
Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups);
gttoc(Ordering);
ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(

11
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -254,6 +254,7 @@ void LevenbergMarquardtOptimizer::iterate() {
bool systemSolvedSuccessfully;
try {
// ============ Solve is where most computation happens !! =================
delta = solve(dampedSystem, state_.values, params_);
systemSolvedSuccessfully = true;
} catch (const IndeterminantLinearSystemException& e) {
@ -281,7 +282,9 @@ void LevenbergMarquardtOptimizer::iterate() {
if (linearizedCostChange >= 0) { // step is valid
// update values
gttic(retract);
// ============ This is where the solution is updated ====================
newValues = state_.values.retract(delta);
// =======================================================================
gttoc(retract);
// compute new error
@ -361,12 +364,8 @@ void LevenbergMarquardtOptimizer::iterate() {
/* ************************************************************************* */
LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering(
LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const {
if (!params.ordering){
if (params.orderingType == Ordering::METIS)
params.ordering = Ordering::metis(graph);
else
params.ordering = Ordering::colamd(graph);
}
if (!params.ordering)
params.ordering = Ordering::Create(params.orderingType, graph);
return params;
}

4
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -248,13 +248,13 @@ KeySet NonlinearFactorGraph::keys() const {
/* ************************************************************************* */
Ordering NonlinearFactorGraph::orderingCOLAMD() const
{
return Ordering::colamd(*this);
return Ordering::Colamd(*this);
}
/* ************************************************************************* */
Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const
{
return Ordering::colamdConstrained(*this, constraints);
return Ordering::ColamdConstrained(*this, constraints);
}
/* ************************************************************************* */

70
gtsam/symbolic/tests/testSymbolicBayesTree.cpp
View File

@ -691,6 +691,76 @@ TEST(SymbolicBayesTree, complicatedMarginal)
}
}
/* ************************************************************************* */
TEST(SymbolicBayesTree, COLAMDvsMETIS) {
// create circular graph
SymbolicFactorGraph sfg;
sfg.push_factor(0, 1);
sfg.push_factor(1, 2);
sfg.push_factor(2, 3);
sfg.push_factor(3, 4);
sfg.push_factor(4, 5);
sfg.push_factor(0, 5);
// COLAMD
{
Ordering ordering = Ordering::Create(Ordering::COLAMD, sfg);
EXPECT(assert_equal(Ordering(list_of(0)(5)(1)(4)(2)(3)), ordering));
// - P( 4 2 3)
// | - P( 1 | 2 4)
// | | - P( 5 | 1 4)
// | | | - P( 0 | 1 5)
SymbolicBayesTree expected;
expected.insertRoot(
MakeClique(list_of(4)(2)(3), 3,
list_of(
MakeClique(list_of(1)(2)(4), 1,
list_of(
MakeClique(list_of(5)(1)(4), 1,
list_of(MakeClique(list_of(0)(1)(5), 1))))))));
SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering);
EXPECT(assert_equal(expected, actual));
}
// METIS
{
Ordering ordering = Ordering::Create(Ordering::METIS, sfg);
// Linux and Mac split differently when using mettis
#if !defined(__APPLE__)
EXPECT(assert_equal(Ordering(list_of(3)(2)(5)(0)(4)(1)), ordering));
#else
EXPECT(assert_equal(Ordering(list_of(5)(4)(2)(1)(0)(3)), ordering));
#endif
// - P( 1 0 3)
// | - P( 4 | 0 3)
// | | - P( 5 | 0 4)
// | - P( 2 | 1 3)
SymbolicBayesTree expected;
#if !defined(__APPLE__)
expected.insertRoot(
MakeClique(list_of(2)(4)(1), 3,
list_of(
MakeClique(list_of(0)(1)(4), 1,
list_of(MakeClique(list_of(5)(0)(4), 1))))(
MakeClique(list_of(3)(2)(4), 1))));
#else
expected.insertRoot(
MakeClique(list_of(1)(0)(3), 3,
list_of(
MakeClique(list_of(4)(0)(3), 1,
list_of(MakeClique(list_of(5)(0)(4), 1))))(
MakeClique(list_of(2)(1)(3), 1))));
#endif
SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering);
EXPECT(assert_equal(expected, actual));
}
}
/* ************************************************************************* */
int main() {
TestResult tr;

2
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -205,7 +205,7 @@ void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
}
// COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
ordering_ = Ordering::colamdConstrainedFirst(factors_,
ordering_ = Ordering::ColamdConstrainedFirst(factors_,
std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()));
if (debug) {

4
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -362,9 +362,9 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
// COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
if(keysToMove && keysToMove->size() > 0) {
ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end()));
ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end()));
}else{
ordering_ = Ordering::colamd(factors_);
ordering_ = Ordering::Colamd(factors_);
}
}

2
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() {
variableIndex_ = VariableIndex(factors_);
KeyVector separatorKeys = separatorValues_.keys();
ordering_ = Ordering::colamdConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end()));
ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end()));
}

4
tests/testNonlinearFactorGraph.cpp
View File

@ -79,14 +79,14 @@ TEST( NonlinearFactorGraph, GET_ORDERING)
{
Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2
NonlinearFactorGraph nlfg = createNonlinearFactorGraph();
Ordering actual = Ordering::colamd(nlfg);
Ordering actual = Ordering::Colamd(nlfg);
EXPECT(assert_equal(expected,actual));
// Constrained ordering - put x2 at the end
Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2);
FastMap<Key, int> constraints;
constraints[X(2)] = 1;
Ordering actualConstrained = Ordering::colamdConstrained(nlfg, constraints);
Ordering actualConstrained = Ordering::ColamdConstrained(nlfg, constraints);
EXPECT(assert_equal(expectedConstrained, actualConstrained));
}