diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 7564e0347..5c217d6c1 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -127,7 +127,7 @@ Ordering FactorGraph::keys() const { * @param columns map from keys to a sparse column of non-zero row indices */ template -Ordering colamd(int n_col, int n_row, int nrNonZeros, const map >& columns) { +boost::shared_ptr colamd(int n_col, int n_row, int nrNonZeros, const map >& columns) { // Convert to compressed column major format colamd wants it in (== MATLAB format!) vector initialOrder; @@ -158,17 +158,16 @@ Ordering colamd(int n_col, int n_row, int nrNonZeros, const map delete [] A; // delete symbolic A // Convert elimination ordering in p to an ordering - Ordering result; + boost::shared_ptr result(new Ordering); for(int j = 0; j < n_col; j++) - result.push_back(initialOrder[j]); + result->push_back(initialOrder[j]); delete [] p; // delete colamd result vector return result; } - /* ************************************************************************* */ template -Ordering FactorGraph::getOrdering() const { +boost::shared_ptr FactorGraph::getOrdering_() const{ // A factor graph is really laid out in row-major format, each factor a row // Below, we compute a symbolic matrix stored in sparse columns. @@ -187,11 +186,18 @@ Ordering FactorGraph::getOrdering() const { int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */ if(n_col == 0) - return Ordering(); // empty ordering + return boost::shared_ptr(new Ordering); // empty ordering else return colamd(n_col, n_row, nrNonZeros, columns); } + +/* ************************************************************************* */ +template +Ordering FactorGraph::getOrdering() const { + return *getOrdering_(); // empty ordering +} + /* ************************************************************************* */ /** O(1) */ /* ************************************************************************* */ diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 058f6967e..21e6be96c 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -92,6 +92,11 @@ namespace gtsam { */ Ordering getOrdering() const; + /** + * shared pointer versions for MATLAB + */ + boost::shared_ptr getOrdering_() const; + /** * Return indices for all factors that involve the given node * @param key the key for the given node diff --git a/cpp/Pose2Graph.h b/cpp/Pose2Graph.h index 4b4409c42..195a37355 100644 --- a/cpp/Pose2Graph.h +++ b/cpp/Pose2Graph.h @@ -13,6 +13,7 @@ #include "Pose2Factor.h" #include "Pose2Config.h" #include "Testable.h" +#include "Ordering.h" namespace gtsam{ diff --git a/cpp/gtsam.h b/cpp/gtsam.h index e5e8f2aa4..d3aaf329e 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -196,6 +196,7 @@ class Pose2Graph{ bool equals(const Pose2Graph& p, double tol) const; GaussianFactorGraph* linearize_(const Pose2Config& config) const; void push_back(Pose2Factor* factor); + Ordering* getOrdering_() const; }; diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index d77154a10..64034b5f6 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -31,7 +31,6 @@ double tol=1e-4; TEST( GaussianFactorGraph, equals ){ GaussianFactorGraph fg = createGaussianFactorGraph(); - fg.print("fg"); GaussianFactorGraph fg2 = createGaussianFactorGraph(); CHECK(fg.equals(fg2)); }