diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index bb11cc608..8fc1711db 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -80,18 +81,32 @@ namespace gtsam { } /* ************************************************************************* */ - std::vector > GaussianFactorGraph::sparseJacobian( - const std::vector& columnIndices) const { + std::vector > GaussianFactorGraph::sparseJacobian() const { + // First find dimensions of each variable + FastVector 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 columnIndices(dims.size()+1, 0); + for(size_t j=1; j triplet; - std::vector entries; - size_t i = 0; + FastVector entries; + size_t row = 0; BOOST_FOREACH(const sharedFactor& factor, *this) { // Convert to JacobianFactor if necessary JacobianFactor::shared_ptr jacobianFactor( boost::dynamic_pointer_cast(factor)); if (!jacobianFactor) { - HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast< - HessianFactor>(factor)); + HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast(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 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 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(entries.begin(), entries.end()); } /* ************************************************************************* */ - Matrix GaussianFactorGraph::sparse(const Vector& columnIndices) const { - - // translate from base 1 Vector to vector of base 0 indices - std::vector indices; - for (int i = 0; i < columnIndices.size(); i++) - indices.push_back(columnIndices[i] - 1); + Matrix GaussianFactorGraph::sparse() const { // call sparseJacobian typedef boost::tuple triplet; - std::vector < boost::tuple > result = - sparseJacobian(indices); + std::vector result = sparseJacobian(); // translate to base 1 matrix size_t nzmax = result.size(); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index e0c3d1c18..7f86fbf50 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -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 > sparseJacobian( - const std::vector& columnIndices) const; + std::vector > 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 diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index da02bb106..3e99ec627 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -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);} /* ************************************************************************* */ diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index 7724043f6..2db9ed9b5 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -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);} /* ************************************************************************* */