From e1716a39cd0b68b118a24275e1f21ab3f266764a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 6 Nov 2009 05:43:03 +0000 Subject: [PATCH] Two changes: LinearFactor::sparse and LinearFactorGraph:sparse, and renamed VariableSet -> Dimensions, which is now a map from keys to integer variable dimensions. Merged in from the "sparse" branch created with Viorela. --- cpp/BayesTree-inl.h | 15 +++--- cpp/Factor.h | 19 ++------ cpp/LinearFactor.cpp | 48 ++++++++++++++++--- cpp/LinearFactor.h | 15 +++++- cpp/LinearFactorGraph.cpp | 70 ++++++++++++++++++++++------ cpp/LinearFactorGraph.h | 10 +++- cpp/NonlinearOptimizer-inl.h | 2 +- cpp/VSLAMFactor.cpp | 2 +- cpp/gtsam.h | 1 + cpp/testLinearFactor.cpp | 87 +++++++++++++++++++++++++++++------ cpp/testLinearFactorGraph.cpp | 51 +++++++++++++++----- 11 files changed, 248 insertions(+), 72 deletions(-) diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 25784a480..8ba80c07c 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -118,6 +118,13 @@ namespace gtsam { addClique(conditional,parent_clique); } + /* ************************************************************************* */ + // Desired: recursive, memoizing version + // Once we know the clique, can we do all with Nodes ? + // Sure, as P(x) = \int P(C|root) + // The natural cache is P(C|root), memoized, of course, in the clique C + // When any marginal is asked for, we calculate P(C|root) = P(C|Pi)P(Pi|root) + // Super-naturally recursive !!!!! /* ************************************************************************* */ template template @@ -153,19 +160,15 @@ namespace gtsam { graph.push_back(*factor); } - //graph.print(); + // TODO: can we prove reverse ordering is efficient? ordering.reverse(); - //ordering.print(); // eliminate to get marginal boost::shared_ptr > bayesNet; typename boost::shared_ptr > chordalBayesNet = graph.eliminate(bayesNet,ordering); - //chordalBayesNet->print("chordalBayesNet"); - - boost::shared_ptr marginal = chordalBayesNet->back(); - return marginal; + return chordalBayesNet->back(); // the root is the marginal } /* ************************************************************************* */ diff --git a/cpp/Factor.h b/cpp/Factor.h index 2240f9137..4d4fa512b 100644 --- a/cpp/Factor.h +++ b/cpp/Factor.h @@ -11,28 +11,15 @@ #pragma once #include +#include #include #include // for noncopyable #include "Testable.h" namespace gtsam { - /** A combination of a key with a dimension - TODO FD: move, vector specific */ - struct Variable { - private: - std::string key_; - std::size_t dim_; - public: - Variable(const std::string& key, std::size_t dim) : key_(key), dim_(dim) {} - bool operator< (const Variable& other) const {return key_ { - }; + /** A map from key to dimension, useful in various contexts */ + typedef std::map Dimensions; /** * A simple factor class to use in a factor graph. diff --git a/cpp/LinearFactor.cpp b/cpp/LinearFactor.cpp index 6237ac022..c5daecd9d 100644 --- a/cpp/LinearFactor.cpp +++ b/cpp/LinearFactor.cpp @@ -6,7 +6,6 @@ */ #include -#include #include // for 'insert()' #include // for operator += in Ordering @@ -148,13 +147,11 @@ list LinearFactor::keys() const { } /* ************************************************************************* */ -VariableSet LinearFactor::variables() const { - VariableSet result; +Dimensions LinearFactor::dimensions() const { + Dimensions result; string j; Matrix A; - FOREACH_PAIR(j,A,As_) { - Variable v(j,A.size2()); - result.insert(v); - } + FOREACH_PAIR(j,A,As_) + result.insert(make_pair(j,A.size2())); return result; } @@ -204,6 +201,43 @@ Matrix LinearFactor::matrix_augmented(const Ordering& ordering) const { return A; } +/* ************************************************************************* */ +boost::tuple, list, list > +LinearFactor::sparse(const Ordering& ordering, const Dimensions& variables) const { + + // declare return values + list I,J; + list S; + + // loop over all variables in correct order + size_t column_start = 1; + BOOST_FOREACH(string key, ordering) { + try { + const Matrix& Aj = get_A(key); + for (size_t i = 0; i < Aj.size1(); i++) { + double sigma_i = sigmas_(i); + for (size_t j = 0; j < Aj.size2(); j++) + if (Aj(i, j) != 0.0) { + I.push_back(i + 1); + J.push_back(j + column_start); + S.push_back(Aj(i, j) / sigma_i); + } + } + } catch (std::invalid_argument& exception) { + // it's ok to not have a key in the ordering + } + // find dimension for this key + Dimensions::const_iterator it = variables.find(key); + // TODO: check if end() and throw exception if not found + int dim = it->second; + // advance column index to next block by adding dim(key) + column_start += dim; + } + + // return the result + return boost::tuple, list, list >(I,J,S); +} + /* ************************************************************************* */ void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m, const size_t pos) { diff --git a/cpp/LinearFactor.h b/cpp/LinearFactor.h index 54d1db1cf..faaa3e31f 100644 --- a/cpp/LinearFactor.h +++ b/cpp/LinearFactor.h @@ -10,6 +10,7 @@ #pragma once #include +#include #include #include "Factor.h" @@ -165,7 +166,7 @@ public: * Find all variables and their dimensions * @return The set of all variable/dimension pairs */ - VariableSet variables() const; + Dimensions dimensions() const; /** * Get the dimension of a particular variable @@ -184,7 +185,7 @@ public: /** * Return (dense) matrix associated with factor - * NOTE: in this case, the precisions are baked into A and b + * NOTE: in this case, the standard deviations are baked into A and b * @param ordering of variables needed for matrix column order */ std::pair matrix(const Ordering& ordering) const; @@ -192,10 +193,20 @@ public: /** * Return (dense) matrix associated with factor * The returned system is an augmented matrix: [A b] + * As above, the standard deviations are baked into A and b * @param ordering of variables needed for matrix column order */ Matrix matrix_augmented(const Ordering& ordering) const; + /** + * Return vectors i, j, and s to generate an m-by-n sparse matrix + * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. + * As above, the standard deviations are baked into A and b + * @param ordering of variables needed for matrix column order + */ + boost::tuple, std::list, std::list > + sparse(const Ordering& ordering, const Dimensions& variables) const; + /* ************************************************************************* */ // MUTABLE functions. FD:on the path to being eradicated /* ************************************************************************* */ diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index 922bcf801..ace8f4cda 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -19,6 +19,9 @@ using namespace std; using namespace gtsam; +// trick from some reading group +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) + // Explicitly instantiate so we don't have to include everywhere template class FactorGraph; @@ -85,11 +88,12 @@ LinearFactorGraph LinearFactorGraph::combine2(const LinearFactorGraph& lfg1, /* ************************************************************************* */ -VariableSet LinearFactorGraph::variables() const { - VariableSet result; +Dimensions LinearFactorGraph::dimensions() const { + Dimensions result; BOOST_FOREACH(shared_factor factor,factors_) { - VariableSet vs = factor->variables(); - BOOST_FOREACH(Variable v,vs) result.insert(v); + Dimensions vs = factor->dimensions(); + string key; int dim; + FOREACH_PAIR(key,dim,vs) result.insert(make_pair(key,dim)); } return result; } @@ -101,17 +105,14 @@ LinearFactorGraph LinearFactorGraph::add_priors(double sigma) const { LinearFactorGraph result = *this; // find all variables and their dimensions - VariableSet vs = variables(); + Dimensions vs = dimensions(); // for each of the variables, add a prior - BOOST_FOREACH(Variable v,vs) { - size_t n = v.dim(); - const string& key = v.key(); - //NOTE: this was previously A=sigma*eye(n), when it should be A=eye(n)/sigma - Matrix A = eye(n); - Vector b = zero(n); - //To maintain interface with separate sigma, the inverse of the 'sigma' passed in used - shared_factor prior(new LinearFactor(key,A,b, 1/sigma)); + string key; int dim; + FOREACH_PAIR(key,dim,vs) { + Matrix A = eye(dim); + Vector b = zero(dim); + shared_factor prior(new LinearFactor(key,A,b, sigma)); result.push_back(prior); } return result; @@ -133,3 +134,46 @@ pair LinearFactorGraph::matrix(const Ordering& ordering) const { } /* ************************************************************************* */ +Matrix LinearFactorGraph::sparse(const Ordering& ordering) const { + + // return values + list I,J; + list S; + + // get the dimensions for all variables + Dimensions variableSet = dimensions(); + + // Collect the I,J,S lists for all factors + int row_index = 0; + BOOST_FOREACH(shared_factor factor,factors_) { + + // get sparse lists for the factor + list i1,j1; + list s1; + boost::tie(i1,j1,s1) = factor->sparse(ordering,variableSet); + + // add row_start to every row index + transform(i1.begin(), i1.end(), i1.begin(), bind2nd(plus(), row_index)); + + // splice lists from factor to the end of the global lists + I.splice(I.end(), i1); + J.splice(J.end(), j1); + S.splice(S.end(), s1); + + // advance row start + row_index += factor->numberOfRows(); + } + + // Convert them to vectors for MATLAB + // TODO: just create a sparse matrix class already + size_t nzmax = S.size(); + Matrix ijs(3,nzmax); + copy(I.begin(),I.end(),ijs.begin2()); + copy(J.begin(),J.end(),ijs.begin2()+nzmax); + copy(S.begin(),S.end(),ijs.begin2()+2*nzmax); + + // return the result + return ijs; +} + +/* ************************************************************************* */ diff --git a/cpp/LinearFactorGraph.h b/cpp/LinearFactorGraph.h index 8f7d3a08e..1aa88b4eb 100644 --- a/cpp/LinearFactorGraph.h +++ b/cpp/LinearFactorGraph.h @@ -93,7 +93,7 @@ namespace gtsam { * Find all variables and their dimensions * @return The set of all variable/dimension pairs */ - VariableSet variables() const; + Dimensions dimensions() const; /** * Add zero-mean i.i.d. Gaussian prior terms to each variable @@ -106,6 +106,14 @@ namespace gtsam { * @param ordering of variables needed for matrix column order */ std::pair matrix (const Ordering& ordering) const; + + /** + * Return 3*nzmax matrix where the rows correspond to the vectors i, j, and s + * to generate an m-by-n sparse matrix, which can be given to MATLAB's sparse function. + * The standard deviations are baked into A and b + * @param ordering of variables needed for matrix column order + */ + Matrix sparse(const Ordering& ordering) const; }; } diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index e12497b8f..0d0d37868 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -124,7 +124,7 @@ namespace gtsam { cout << "trying lambda = " << lambda_ << endl; // add prior-factors - LinearFactorGraph damped = linear.add_priors(sqrt(lambda_)); + LinearFactorGraph damped = linear.add_priors(1.0/sqrt(lambda_)); if (verbosity >= DAMPED) damped.print("damped"); diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp index 44a287610..60cd86491 100644 --- a/cpp/VSLAMFactor.cpp +++ b/cpp/VSLAMFactor.cpp @@ -66,7 +66,7 @@ LinearFactor::shared_ptr VSLAMFactor::linearize(const Config& c) const // Make new linearfactor, divide by sigma LinearFactor::shared_ptr - p(new LinearFactor(cameraFrameName_, Dh1/ConvenientFactor::sigma_, landmarkName_, Dh2/ConvenientFactor::sigma_, b/ConvenientFactor::sigma_)); + p(new LinearFactor(cameraFrameName_, Dh1, landmarkName_, Dh2, b, ConvenientFactor::sigma_)); return p; } diff --git a/cpp/gtsam.h b/cpp/gtsam.h index af302ea53..78274a8c2 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -96,6 +96,7 @@ class LinearFactorGraph { VectorConfig optimize(const Ordering& ordering); GaussianBayesNet* eliminate(const Ordering& ordering); pair matrix(const Ordering& ordering) const; + Matrix sparse(const Ordering& ordering) const; }; class Point2 { diff --git a/cpp/testLinearFactor.cpp b/cpp/testLinearFactor.cpp index 75b743c94..bedb46e42 100644 --- a/cpp/testLinearFactor.cpp +++ b/cpp/testLinearFactor.cpp @@ -10,6 +10,7 @@ #include #include // for operator += #include +#include // for insert using namespace boost::assign; #include @@ -63,24 +64,16 @@ TEST( LinearFactor, keys ) } /* ************************************************************************* */ -TEST( LinearFactor, variables ) +TEST( LinearFactor, dimensions ) { // get the factor "f2" from the small linear factor graph LinearFactorGraph fg = createLinearFactorGraph(); - LinearFactor::shared_ptr lf = fg[1]; - VariableSet actual = lf->variables(); - // create expected variable set - VariableSet expected; - Variable x1("x1", 2); - Variable x2("x2", 2); - expected += x1, x2; - - // verify - CHECK(expected.size() == actual.size()); - set::const_iterator exp, act; - for (exp=expected.begin(), act=actual.begin(); exp != expected.end(); act++, exp++) - CHECK((*exp).equals(*act)); + // Check a single factor + Dimensions expected; + insert(expected)("x1", 2)("x2", 2); + Dimensions actual = fg[1]->dimensions(); + CHECK(expected==actual); } /* ************************************************************************* */ @@ -536,6 +529,72 @@ TEST( LinearFactor, matrix_aug ) EQUALITY(Ab,Ab1); } +/* ************************************************************************* */ +// small aux. function to print out lists of anything +template +void print(const list& i) { + copy(i.begin(), i.end(), ostream_iterator (cout, ",")); + cout << endl; +} + +/* ************************************************************************* */ +TEST( LinearFactor, sparse ) +{ + // create a small linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // get the factor "f2" from the factor graph + LinearFactor::shared_ptr lf = fg[1]; + + // render with a given ordering + Ordering ord; + ord += "x1","x2"; + + list i,j; + list s; + boost::tie(i,j,s) = lf->sparse(ord, fg.dimensions()); + + list i1,j1; + i1 += 1,2,1,2; + j1 += 1,2,3,4; + + list s1; + s1 += -10,-10,10,10; + + CHECK(i==i1); + CHECK(j==j1); + CHECK(s==s1); +} + +/* ************************************************************************* */ +TEST( LinearFactor, sparse2 ) +{ + // create a small linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // get the factor "f2" from the factor graph + LinearFactor::shared_ptr lf = fg[1]; + + // render with a given ordering + Ordering ord; + ord += "x2","l1","x1"; + + list i,j; + list s; + boost::tie(i,j,s) = lf->sparse(ord, fg.dimensions()); + + list i1,j1; + i1 += 1,2,1,2; + j1 += 1,2,5,6; + + list s1; + s1 += 10,10,-10,-10; + + CHECK(i==i1); + CHECK(j==j1); + CHECK(s==s1); +} + /* ************************************************************************* */ TEST( LinearFactor, size ) { diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index 13c881e22..ee14ad79a 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -8,6 +8,7 @@ #include using namespace std; +#include #include #include // for operator += using namespace boost::assign; @@ -329,7 +330,7 @@ TEST( LinearFactorGraph, add_priors ) LinearFactorGraph expected = createLinearFactorGraph(); Matrix A = eye(2); Vector b = zero(2); - double sigma = 1.0/3.0; + double sigma = 3.0; expected.push_back(LinearFactor::shared_ptr(new LinearFactor("l1",A,b,sigma))); expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x1",A,b,sigma))); expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x2",A,b,sigma))); @@ -371,21 +372,39 @@ TEST( LinearFactorGraph, matrix ) boost::tie(A,b) = fg.matrix(ord); Matrix A1 = Matrix_(2*4,3*2, - 00.0, 0.0, 0.0, 0.0, 10.0, 0.0, - 00.0, 0.0, 0.0, 0.0, 0.0, 10.0, - 10.0, 0.0, 0.0, 0.0,-10.0, 0.0, - 00.0, 10.0, 0.0, 0.0, 0.0,-10.0, - 00.0, 0.0, 5.0, 0.0, -5.0, 0.0, - 00.0, 0.0, 0.0, 5.0, 0.0, -5.0, - -5.0, 0.0, 5.0, 0.0, 0.0, 0.0, - 00.0, -5.0, 0.0, 5.0, 0.0, 0.0 + +0., 0., 0., 0., 10., 0., // unary factor on x1 (prior) + +0., 0., 0., 0., 0., 10., + 10., 0., 0., 0.,-10., 0., // binary factor on x2,x1 (odometry) + +0., 10., 0., 0., 0.,-10., + +0., 0., 5., 0., -5., 0., // binary factor on l1,x1 (z1) + +0., 0., 0., 5., 0., -5., + -5., 0., 5., 0., 0., 0., // binary factor on x2,l1 (z2) + +0., -5., 0., 5., 0., 0. ); - Vector b1 = Vector_(8,-1.0, -1.0, 2.0, -1.0, 0.0, 1.0, -1.0, 1.5); + Vector b1 = Vector_(8,-1., -1., 2., -1., 0., 1., -1., 1.5); EQUALITY(A,A1); // currently fails CHECK(b==b1); // currently fails } +/* ************************************************************************* */ +TEST( LinearFactorGraph, sparse ) +{ + // create a small linear factor graph + LinearFactorGraph fg = createLinearFactorGraph(); + + // render with a given ordering + Ordering ord; + ord += "x2","l1","x1"; + + Matrix ijs = fg.sparse(ord); + + EQUALITY(ijs, Matrix_(3, 14, + +1., 2., 3., 4., 3., 4., 5.,6., 5., 6., 7., 8.,7.,8., + +5., 6., 1., 2., 5., 6., 3.,4., 5., 6., 1., 2.,3.,4., + 10.,10., 10.,10.,-10.,-10., 5.,5.,-5.,-5., -5.,-5.,5.,5.)); +} + /* ************************************************************************* */ TEST( LinearFactorGraph, CONSTRUCTOR_GaussianBayesNet ) { @@ -552,7 +571,7 @@ TEST( LinearFactorGraph, findAndRemoveFactors_twice ) } /* ************************************************************************* */ -TEST(timeLinearFactorGraph, createSmoother) +TEST(LinearFactorGraph, createSmoother) { LinearFactorGraph fg1 = createSmoother(2); LONGS_EQUAL(3,fg1.size()); @@ -560,6 +579,16 @@ TEST(timeLinearFactorGraph, createSmoother) LONGS_EQUAL(5,fg2.size()); } +/* ************************************************************************* */ +TEST( LinearFactorGraph, variables ) +{ + LinearFactorGraph fg = createLinearFactorGraph(); + Dimensions expected; + insert(expected)("l1", 2)("x1", 2)("x2", 2); + Dimensions actual = fg.dimensions(); + CHECK(expected==actual); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */