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

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.

release/4.3a0
Frank Dellaert 2009-11-06 05:43:03 +00:00
parent fb74ef03b2
commit e1716a39cd
11 changed files with 248 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cpp/BayesTree-inl.h
View File

@ -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<class Conditional>
template<class Factor>
@ -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<Conditional> > bayesNet;
typename boost::shared_ptr<BayesNet<Conditional> > chordalBayesNet =
graph.eliminate(bayesNet,ordering);
//chordalBayesNet->print("chordalBayesNet");
boost::shared_ptr<Conditional> marginal = chordalBayesNet->back();
return marginal;
return chordalBayesNet->back(); // the root is the marginal
}
/* ************************************************************************* */

19
cpp/Factor.h
View File

@ -11,28 +11,15 @@
#pragma once
#include <set>
#include <map>
#include <list>
#include <boost/utility.hpp> // 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_<other.key_; }
const std::string& key() const { return key_;}
std::size_t dim() const { return dim_;}
bool equals(const Variable& var) const { return key_ == var.key_ && dim_ == var.dim_;}
};
/** A set of variables, used to eliminate linear factor factor graphs. TODO FD: move */
class VariableSet : public std::set<Variable> {
};
/** A map from key to dimension, useful in various contexts */
typedef std::map<std::string,int> Dimensions;
/**
* A simple factor class to use in a factor graph.

48
cpp/LinearFactor.cpp
View File

@ -6,7 +6,6 @@
*/
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/list_inserter.hpp> // for 'insert()'
#include <boost/assign/std/list.hpp> // for operator += in Ordering
@ -148,13 +147,11 @@ list<string> 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<int>, list<int>, list<double> >
LinearFactor::sparse(const Ordering& ordering, const Dimensions& variables) const {
// declare return values
list<int> I,J;
list<double> 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<int>, list<int>, list<double> >(I,J,S);
}
/* ************************************************************************* */
void LinearFactor::append_factor(LinearFactor::shared_ptr f, const size_t m,
const size_t pos) {

15
cpp/LinearFactor.h
View File

@ -10,6 +10,7 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/serialization/map.hpp>
#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, Vector> 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<int>, std::list<int>, std::list<double> >
sparse(const Ordering& ordering, const Dimensions& variables) const;
/* ************************************************************************* */
// MUTABLE functions. FD:on the path to being eradicated
/* ************************************************************************* */

70
cpp/LinearFactorGraph.cpp
View File

@ -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<LinearFactor>;
@ -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<Matrix,Vector> LinearFactorGraph::matrix(const Ordering& ordering) const {
}
/* ************************************************************************* */
Matrix LinearFactorGraph::sparse(const Ordering& ordering) const {
// return values
list<int> I,J;
list<double> 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<int> i1,j1;
list<double> 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<int>(), 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;
}
/* ************************************************************************* */

10
cpp/LinearFactorGraph.h
View File

@ -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,Vector> 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;
};
}

2
cpp/NonlinearOptimizer-inl.h
View File

@ -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");

2
cpp/VSLAMFactor.cpp
View File

@ -66,7 +66,7 @@ LinearFactor::shared_ptr VSLAMFactor<Config>::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;
}

1
cpp/gtsam.h
View File

@ -96,6 +96,7 @@ class LinearFactorGraph {
VectorConfig optimize(const Ordering& ordering);
GaussianBayesNet* eliminate(const Ordering& ordering);
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
Matrix sparse(const Ordering& ordering) const;
};
class Point2 {

87
cpp/testLinearFactor.cpp
View File

@ -10,6 +10,7 @@
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp>
#include <boost/assign/std/map.hpp> // for insert
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
@ -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<Variable>::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<class T>
void print(const list<T>& i) {
copy(i.begin(), i.end(), ostream_iterator<T> (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<int> i,j;
list<double> s;
boost::tie(i,j,s) = lf->sparse(ord, fg.dimensions());
list<int> i1,j1;
i1 += 1,2,1,2;
j1 += 1,2,3,4;
list<double> 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<int> i,j;
list<double> s;
boost::tie(i,j,s) = lf->sparse(ord, fg.dimensions());
list<int> i1,j1;
i1 += 1,2,1,2;
j1 += 1,2,5,6;
list<double> s1;
s1 += 10,10,-10,-10;
CHECK(i==i1);
CHECK(j==j1);
CHECK(s==s1);
}
/* ************************************************************************* */
TEST( LinearFactor, size )
{

51
cpp/testLinearFactorGraph.cpp
View File

@ -8,6 +8,7 @@
#include <iostream>
using namespace std;
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // 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);}
/* ************************************************************************* */