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

Made GaussianFactorGraph::sparse and sparseJacobian functions take no arguments, and instead compute column indices internally

release/4.3a0
Richard Roberts 2011-10-29 23:12:58 +00:00
parent eb8fb31b2a
commit 5408ab0a2d
4 changed files with 87 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/VariableSlots.h>
@ -80,18 +81,32 @@ namespace gtsam {
}
/* ************************************************************************* */
std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian(
const std::vector<size_t>& columnIndices) const {
std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
// First find dimensions of each variable
FastVector<size_t> dims;
BOOST_FOREACH(const sharedFactor& factor, *this) {
for(GaussianFactor::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) {
if(dims.size() <= *pos)
dims.resize(*pos + 1, 0);
dims[*pos] = factor->getDim(pos);
}
}
// Compute first scalar column of each variable
vector<size_t> columnIndices(dims.size()+1, 0);
for(size_t j=1; j<dims.size()+1; ++j)
columnIndices[j] = columnIndices[j-1] + dims[j-1];
// Iterate over all factors, adding sparse scalar entries
typedef boost::tuple<size_t, size_t, double> triplet;
std::vector<triplet> entries;
size_t i = 0;
FastVector<triplet> entries;
size_t row = 0;
BOOST_FOREACH(const sharedFactor& factor, *this) {
// Convert to JacobianFactor if necessary
JacobianFactor::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (!jacobianFactor) {
HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<
HessianFactor>(factor));
HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian)
jacobianFactor.reset(new JacobianFactor(*hessian));
else
@ -99,33 +114,38 @@ namespace gtsam {
"GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
// Add entries, adjusting the row index i
std::vector<triplet> factorEntries(
jacobianFactor->sparse(columnIndices));
entries.reserve(entries.size() + factorEntries.size());
for (size_t k = 0; k < factorEntries.size(); ++k)
entries.push_back(boost::make_tuple(
factorEntries[k].get<0> () + i, factorEntries[k].get<
1> (), factorEntries[k].get<2> ()));
// Whiten the factor and add entries for it
// iterate over all variables in the factor
const JacobianFactor whitened(jacobianFactor->whiten());
for(JacobianFactor::const_iterator pos=whitened.begin(); pos<whitened.end(); ++pos) {
JacobianFactor::constABlock whitenedA = whitened.getA(pos);
// find first column index for this key
size_t column_start = columnIndices[*pos];
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
double s = whitenedA(i,j);
if (std::abs(s) > 1e-12) entries.push_back(
boost::make_tuple(row+i, column_start+j, s));
}
}
JacobianFactor::constBVector whitenedb(whitened.getb());
size_t bcolumn = columnIndices.back();
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i)));
// Increment row index
i += jacobianFactor->rows();
row += jacobianFactor->rows();
}
return entries;
return vector<triplet>(entries.begin(), entries.end());
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparse(const Vector& columnIndices) const {
// translate from base 1 Vector to vector of base 0 indices
std::vector<size_t> indices;
for (int i = 0; i < columnIndices.size(); i++)
indices.push_back(columnIndices[i] - 1);
Matrix GaussianFactorGraph::sparse() const {
// call sparseJacobian
typedef boost::tuple<size_t, size_t, double> triplet;
std::vector < boost::tuple<size_t, size_t, double> > result =
sparseJacobian(indices);
std::vector<triplet> result = sparseJacobian();
// translate to base 1 matrix
size_t nzmax = result.size();

7
gtsam/linear/GaussianFactorGraph.h
View File

@ -152,18 +152,15 @@ namespace gtsam {
* Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix,
* where i(k) and j(k) are the base 0 row and column indices, s(k) a double.
* The standard deviations are baked into A and b
* @param columnIndices First column index for each variable.
*/
std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian(
const std::vector<size_t>& columnIndices) const;
std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
/**
* Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
* The standard deviations are baked into A and b
* @param columnIndices First column index for each variable, base 1, in vector format.
*/
Matrix sparse(const Vector& columnIndices) const;
Matrix sparse() const;
/**
* Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b

43
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -62,8 +62,7 @@ TEST(GaussianFactorGraph, initialization) {
1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., 7., 7.,
10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., -5., 5., 5., -1., 1.5
);
Vector columnIndices = Vector_(4,1.0,3.0,5.0,7.0);
Matrix actualIJS = fg.sparse(columnIndices);
Matrix actualIJS = fg.sparse();
EQUALITY(expectedIJS, actualIJS);
}
@ -171,7 +170,7 @@ TEST(GaussianFactorGraph, Combine2)
}
/* ************************************************************************* */
TEST_UNSAFE(GaussianFactor, CombineAndEliminate)
TEST(GaussianFactor, CombineAndEliminate)
{
Matrix A01 = Matrix_(3,3,
1.0, 0.0, 0.0,
@ -573,6 +572,44 @@ TEST(GaussianFactor, permuteWithInverse)
#endif
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, sparseJacobian) {
// Create factor graph:
// x1 x2 x3 x4 x5 b
// 1 2 3 0 0 4
// 5 6 7 0 0 8
// 9 10 0 11 12 13
// 0 0 0 14 15 16
// Expected - NOTE that we transpose this!
Matrix expected = Matrix_(16,3,
1., 1., 2.,
1., 2., 4.,
1., 3., 6.,
2., 1.,10.,
2., 2.,12.,
2., 3.,14.,
1., 6., 8.,
2., 6.,16.,
3., 1.,18.,
3., 2.,20.,
3., 4.,22.,
3., 5.,24.,
4., 4.,28.,
4., 5.,30.,
3., 6.,26.,
4., 6.,32.).transpose();
GaussianFactorGraph gfg;
SharedDiagonal model = sharedSigma(2, 0.5);
gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model);
gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model);
Matrix actual = gfg.sparse();
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

3
tests/testGaussianFactorGraph.cpp
View File

@ -957,7 +957,7 @@ TEST(GaussianFactorGraph, replace)
//}
/* ************************************************************************* */
/**
/*
* x2 x1 x3 b
* 1 1 1 1 1 0 1
* 1 1 1 -> 1 1 1
@ -1011,7 +1011,6 @@ TEST(GaussianFactorGraph, createSmoother2)
CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */