diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index a3a14c6c3..11058ef40 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -33,6 +33,8 @@ #include #include +#include + /** * Matrix is a typedef in the gtsam namespace * TODO: make a version to work with matlab wrapping @@ -73,6 +75,12 @@ GTSAM_MAKE_MATRIX_DEFS(9); typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; +/// Sparse matrix representation as vector of tuples. +/// See SparseMatrix.h for additional representations SparseMatrixEigenTriplets +/// and SparseMatrixEigen +typedef std::vector> + SparseMatrixBoostTriplets; + // Matrix formatting arguments when printing. // Akin to Matlab style. const Eigen::IOFormat& matlabFormat(); diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 9a22d6111..7790b5328 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -100,7 +100,9 @@ namespace gtsam { } /* ************************************************************************* */ - vector > GaussianFactorGraph::sparseJacobian() const { + SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian( + const Ordering& ordering, size_t& nrows, size_t& ncols) const { + gttic_(GaussianFactorGraph_sparseJacobian); // First find dimensions of each variable typedef std::map KeySizeMap; KeySizeMap dims; @@ -108,24 +110,24 @@ namespace gtsam { if (!static_cast(factor)) continue; - for (GaussianFactor::const_iterator key = factor->begin(); - key != factor->end(); ++key) { - dims[*key] = factor->getDim(key); + for (auto it = factor->begin(); it != factor->end(); ++it) { + dims[*it] = factor->getDim(it); } } // Compute first scalar column of each variable - size_t currentColIndex = 0; + ncols = 0; KeySizeMap columnIndices = dims; - for (const KeySizeMap::value_type& col : dims) { - columnIndices[col.first] = currentColIndex; - currentColIndex += dims[col.first]; + for (const auto key : ordering) { + columnIndices[key] = ncols; + ncols += dims[key]; } // Iterate over all factors, adding sparse scalar entries - typedef boost::tuple triplet; - vector entries; - size_t row = 0; + SparseMatrixBoostTriplets entries; + entries.reserve(60 * size()); + + nrows = 0; for (const sharedFactor& factor : *this) { if (!static_cast(factor)) continue; @@ -154,38 +156,79 @@ namespace gtsam { for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { double s = whitenedA(i, j); if (std::abs(s) > 1e-12) - entries.emplace_back(row + i, column_start + j, s); + entries.emplace_back(nrows + i, column_start + j, s); } } JacobianFactor::constBVector whitenedb(whitened.getb()); - size_t bcolumn = currentColIndex; - for (size_t i = 0; i < (size_t) whitenedb.size(); i++) - entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i))); + for (size_t i = 0; i < (size_t) whitenedb.size(); i++) { + double s = whitenedb(i); + if (std::abs(s) > 1e-12) entries.emplace_back(nrows + i, ncols, s); + } // Increment row index - row += jacobianFactor->rows(); + nrows += jacobianFactor->rows(); } - return vector(entries.begin(), entries.end()); + + ncols++; // +1 for b-column + return entries; + } + + /* ************************************************************************* */ + SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian( + const Ordering& ordering) const { + size_t dummy1, dummy2; + return sparseJacobian(ordering, dummy1, dummy2); + } + + /* ************************************************************************* */ + SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian( + size_t& nrows, size_t& ncols) const { + return sparseJacobian(Ordering(this->keys()), nrows, ncols); + } + + /* ************************************************************************* */ + SparseMatrixBoostTriplets GaussianFactorGraph::sparseJacobian() const { + size_t dummy1, dummy2; + return sparseJacobian(dummy1, dummy2); + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::sparseJacobian_( + const Ordering& ordering, size_t& nrows, size_t& ncols) const { + gttic_(GaussianFactorGraph_sparseJacobian_matrix); + // call sparseJacobian + auto result = sparseJacobian(ordering, nrows, ncols); + + // translate to base 1 matrix + size_t nzmax = result.size(); + Matrix IJS(3, nzmax); + for (size_t k = 0; k < result.size(); k++) { + const auto& entry = result[k]; + IJS(0, k) = double(entry.get<0>() + 1); + IJS(1, k) = double(entry.get<1>() + 1); + IJS(2, k) = entry.get<2>(); + } + return IJS; + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::sparseJacobian_( + const Ordering& ordering) const { + size_t dummy1, dummy2; + return sparseJacobian_(ordering, dummy1, dummy2); + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::sparseJacobian_( + size_t& nrows, size_t& ncols) const { + return sparseJacobian_(Ordering(this->keys()), nrows, ncols); } /* ************************************************************************* */ Matrix GaussianFactorGraph::sparseJacobian_() const { - - // call sparseJacobian - typedef boost::tuple triplet; - vector result = sparseJacobian(); - - // translate to base 1 matrix - size_t nzmax = result.size(); - Matrix IJS(3,nzmax); - for (size_t k = 0; k < result.size(); k++) { - const triplet& entry = result[k]; - IJS(0,k) = double(entry.get<0>() + 1); - IJS(1,k) = double(entry.get<1>() + 1); - IJS(2,k) = entry.get<2>(); - } - return IJS; + size_t dummy1, dummy2; + return sparseJacobian_(dummy1, dummy2); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 2b9e8e675..5eb47ca48 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -181,17 +181,51 @@ 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 + * Return vector of i, j, and s to generate an m-by-n sparse augmented + * 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 ordering the column ordering + * @param[out] nrows The number of rows in the Jacobian + * @param[out] ncols The number of columns in the Jacobian + * @return the sparse matrix in one of the 4 forms above */ - std::vector > sparseJacobian() const; + SparseMatrixBoostTriplets sparseJacobian(const Ordering& ordering, + size_t& nrows, + size_t& ncols) const; + + /// Returns a sparse augmented Jacobian without outputting its dimensions + SparseMatrixBoostTriplets sparseJacobian( + const Ordering& ordering) const; + + /// Returns a sparse augmented Jacobian with default Ordering + SparseMatrixBoostTriplets sparseJacobian(size_t& nrows, + size_t& ncols) const; + + /// Returns a sparse augmented Jacobian without with default ordering and + /// outputting its dimensions + SparseMatrixBoostTriplets 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. + * 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. Note: i, j are 1-indexed. * The standard deviations are baked into A and b */ + Matrix sparseJacobian_(const Ordering& ordering, + size_t& nrows, + size_t& ncols) const; + + /// Returns a matrix-form sparse augmented Jacobian without outputting its + /// dimensions + Matrix sparseJacobian_( + const Ordering& ordering) const; + + /// Returns a matrix-form sparse augmented Jacobian with default Ordering + Matrix sparseJacobian_(size_t& nrows, + size_t& ncols) const; + + /// Returns a matrix-form sparse augmented Jacobian with default ordering + /// and without outputting its dimensions Matrix sparseJacobian_() const; /** diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 8b9ce94a9..493e7667c 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -66,10 +66,11 @@ TEST(GaussianFactorGraph, initialization) { // Test sparse, which takes a vector and returns a matrix, used in MATLAB // Note that this the augmented vector and the RHS is in column 7 Matrix expectedIJS = - (Matrix(3, 22) << 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., - 8., 7., 8., 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).finished(); + (Matrix(3, 21) << + 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 6., 7., 8., 7., 8., 7., 8., + 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 3., 4., 5., 6., 7., 7., + 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., + 1., -5., -5., 5., 5., -1., 1.5).finished(); Matrix actualIJS = fg.sparseJacobian_(); EQUALITY(expectedIJS, actualIJS); }