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

Sped up and fixed (?) sparse

release/4.3a0
Frank Dellaert 2009-12-12 06:18:29 +00:00
parent 99533f286f
commit c38683cd64
6 changed files with 57 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
cpp/GaussianFactor.cpp
View File

@ -220,17 +220,19 @@ Matrix GaussianFactor::matrix_augmented(const Ordering& ordering) const {
/* ************************************************************************* */
boost::tuple<list<int>, list<int>, list<double> >
GaussianFactor::sparse(const Ordering& ordering, const Dimensions& variables) const {
GaussianFactor::sparse(const Dimensions& columnIndices) 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);
// iterate over all matrices in the factor
string key; Matrix Aj;
FOREACH_PAIR( key, Aj, As_) {
// find first column index for this key
// TODO: check if end() and throw exception if not found
Dimensions::const_iterator it = columnIndices.find(key);
int column_start = it->second;
for (size_t i = 0; i < Aj.size1(); i++) {
double sigma_i = sigmas_(i);
for (size_t j = 0; j < Aj.size2(); j++)
@ -240,15 +242,6 @@ GaussianFactor::sparse(const Ordering& ordering, const Dimensions& variables) co
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

4
cpp/GaussianFactor.h
View File

@ -211,10 +211,10 @@ public:
* 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
* @param first column index for each variable
*/
boost::tuple<std::list<int>, std::list<int>, std::list<double> >
sparse(const Ordering& ordering, const Dimensions& variables) const;
sparse(const Dimensions& columnIndices) const;
/**
* Add gradient contribution to gradient config g

27
cpp/GaussianFactorGraph.cpp
View File

@ -156,6 +156,27 @@ pair<Matrix,Vector> GaussianFactorGraph::matrix(const Ordering& ordering) const
return lf.matrix(ordering);
}
/* ************************************************************************* */
Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
// get the dimensions for all variables
Dimensions variableSet = dimensions();
// Find the starting index and dimensions for all variables given the order
size_t j = 1;
Dimensions result;
BOOST_FOREACH(string key, ordering) {
// associate key with first column index
result.insert(make_pair(key,j));
// find dimension for this key
Dimensions::const_iterator it = variableSet.find(key);
// advance column index to next block by adding dim(key)
j += it->second;
}
return result;
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
@ -163,8 +184,8 @@ Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
list<int> I,J;
list<double> S;
// get the dimensions for all variables
Dimensions variableSet = dimensions();
// get the starting column indices for all variables
Dimensions indices = columnIndices(ordering);
// Collect the I,J,S lists for all factors
int row_index = 0;
@ -173,7 +194,7 @@ Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
// get sparse lists for the factor
list<int> i1,j1;
list<double> s1;
boost::tie(i1,j1,s1) = factor->sparse(ordering,variableSet);
boost::tie(i1,j1,s1) = factor->sparse(indices);
// add row_start to every row index
transform(i1.begin(), i1.end(), i1.begin(), bind2nd(plus<int>(), row_index));

7
cpp/GaussianFactorGraph.h
View File

@ -152,6 +152,13 @@ namespace gtsam {
*/
std::pair<Matrix,Vector> matrix (const Ordering& ordering) const;
/**
* get the starting column indices for all variables
* @param ordering of variables needed for matrix column order
* @return The set of all variable/index pairs
*/
Dimensions columnIndices(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.

8
cpp/testGaussianFactor.cpp
View File

@ -552,7 +552,7 @@ TEST( GaussianFactor, sparse )
list<int> i,j;
list<double> s;
boost::tie(i,j,s) = lf->sparse(ord, fg.dimensions());
boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord));
list<int> i1,j1;
i1 += 1,2,1,2;
@ -581,14 +581,14 @@ TEST( GaussianFactor, sparse2 )
list<int> i,j;
list<double> s;
boost::tie(i,j,s) = lf->sparse(ord, fg.dimensions());
boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord));
list<int> i1,j1;
i1 += 1,2,1,2;
j1 += 1,2,5,6;
j1 += 5,6,1,2;
list<double> s1;
s1 += 10,10,-10,-10;
s1 += -10,-10,10,10;
CHECK(i==i1);
CHECK(j==j1);

6
cpp/testGaussianFactorGraph.cpp
View File

@ -330,11 +330,11 @@ TEST( GaussianFactorGraph, sparse )
Matrix ijs = fg.sparse(ord);
EQUALITY(ijs, Matrix_(3, 14,
EQUALITY(Matrix_(3, 14,
// f(x1) f(x2,x1) f(l1,x1) f(x2,l1)
+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.));
+5., 6., 5., 6., 1., 2., 3.,4., 5., 6., 3., 4., 1., 2.,
10.,10., -10.,-10., 10., 10., 5.,5.,-5.,-5., 5., 5.,-5.,-5.), ijs);
}
/* ************************************************************************* */