diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 69890c32d..13eaee7e3 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -100,33 +100,35 @@ namespace gtsam { } /* ************************************************************************* */ - vector > GaussianFactorGraph::sparseJacobian() const { + using SparseTriplets = std::vector >; + SparseTriplets 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; - for (const sharedFactor& factor : *this) { - if (!static_cast(factor)) - continue; + for (const auto& factor : *this) { + 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; - for (const sharedFactor& factor : *this) { + SparseTriplets entries; + entries.reserve(30 * size()); + nrows = 0; + for (const auto& factor : *this) { if (!static_cast(factor)) continue; // Convert to JacobianFactor if necessary @@ -138,52 +140,60 @@ namespace gtsam { if (hessian) jacobianFactor.reset(new JacobianFactor(*hessian)); else - throw invalid_argument( - "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); + throw std::invalid_argument( + "GaussianFactorGraph contains a factor that is neither a " + "JacobianFactor nor a HessianFactor."); } // Whiten the factor and add entries for it // iterate over all variables in the factor const JacobianFactor whitened(jacobianFactor->whiten()); - for (JacobianFactor::const_iterator key = whitened.begin(); - key < whitened.end(); ++key) { + for (auto key = whitened.begin(); key < whitened.end(); ++key) { JacobianFactor::constABlock whitenedA = whitened.getA(key); // find first column index for this key size_t column_start = columnIndices[*key]; - for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) - for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { + 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)); + 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; + } + + /* ************************************************************************* */ + SparseTriplets GaussianFactorGraph::sparseJacobian() const { + size_t nrows, ncols; + return sparseJacobian(Ordering(this->keys()), nrows, ncols); } /* ************************************************************************* */ Matrix GaussianFactorGraph::sparseJacobian_() const { - + gttic_(GaussianFactorGraph_sparseJacobian_matrix); // call sparseJacobian - typedef boost::tuple triplet; - vector result = sparseJacobian(); + auto result = sparseJacobian(); // translate to base 1 matrix size_t nzmax = result.size(); - Matrix IJS(3,nzmax); + 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>(); + const auto& entry = result[k]; + IJS(0, k) = double(std::get<0>(entry) + 1); + IJS(1, k) = double(std::get<1>(entry) + 1); + IJS(2, k) = std::get<2>(entry); } return IJS; } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 2b9e8e675..e3304d5e8 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -181,15 +181,25 @@ 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. + * Returns a sparse augmented Jacbian matrix as a vector of i, j, and s, + * where i(k) and j(k) are the base 0 row and column indices, and s(k) is + * the entry as a double. * The standard deviations are baked into A and b + * @return the sparse matrix as a std::vector of std::tuples + * @param ordering the column ordering + * @param[out] nrows The number of rows in the augmented Jacobian + * @param[out] ncols The number of columns in the augmented Jacobian */ - std::vector > sparseJacobian() const; + std::vector > sparseJacobian( + const Ordering& ordering, size_t& nrows, size_t& ncols) const; + + /** Returns a sparse augmented Jacobian matrix with default ordering */ + 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. + * 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; diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h index 7963d3ef5..483ae7f8d 100644 --- a/gtsam/linear/SparseEigen.h +++ b/gtsam/linear/SparseEigen.h @@ -30,89 +30,33 @@ namespace gtsam { -typedef Eigen::SparseMatrix SparseEigen; +/// Eigen-format sparse matrix. Note: ColMajor is ~20% faster since +/// InnerIndices must be sorted +typedef Eigen::SparseMatrix SparseEigen; /// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph SparseEigen sparseJacobianEigen( const GaussianFactorGraph &gfg, const Ordering &ordering) { - // TODO(gerry): eliminate copy/pasta by making GaussianFactorGraph version - // more general, or by creating an Eigen::Triplet compatible wrapper for - // boost::tuple return type - - // First find dimensions of each variable - std::map dims; - for (const boost::shared_ptr &factor : gfg) { - if (!static_cast(factor)) continue; - - 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; - std::map columnIndices; - for (const auto key : ordering) { - columnIndices[key] = currentColIndex; - currentColIndex += dims[key]; - } - - // Iterate over all factors, adding sparse scalar entries - std::vector> entries; - entries.reserve(60 * gfg.size()); - - size_t row = 0; - for (const boost::shared_ptr &factor : gfg) { - if (!static_cast(factor)) continue; - - // Convert to JacobianFactor if necessary - JacobianFactor::shared_ptr jacobianFactor( - boost::dynamic_pointer_cast(factor)); - if (!jacobianFactor) { - HessianFactor::shared_ptr hessian( - boost::dynamic_pointer_cast(factor)); - if (hessian) - jacobianFactor.reset(new JacobianFactor(*hessian)); - else - throw std::invalid_argument( - "GaussianFactorGraph contains a factor that is neither a " - "JacobianFactor nor a HessianFactor."); - } - - // Whiten the factor and add entries for it - // iterate over all variables in the factor - const JacobianFactor whitened(jacobianFactor->whiten()); - for (JacobianFactor::const_iterator key = whitened.begin(); - key < whitened.end(); ++key) { - JacobianFactor::constABlock whitenedA = whitened.getA(key); - // find first column index for this key - size_t column_start = columnIndices[*key]; - 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.emplace_back(row + 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++) { - double s = whitenedb(i); - if (std::abs(s) > 1e-12) entries.emplace_back(row + i, bcolumn, s); - } - - // Increment row index - row += jacobianFactor->rows(); - } - - // ...and make a sparse matrix with it. - SparseEigen Ab(row, currentColIndex + 1); - Ab.setFromTriplets(entries.begin(), entries.end()); + gttic_(SparseEigen_sparseJacobianEigen); + // intermediate `entries` vector is kind of unavoidable due to how expensive + // factor->rows() is, which prevents us from populating SparseEigen directly. + size_t nrows, ncols; + auto entries = gfg.sparseJacobian(ordering, nrows, ncols); + // declare sparse matrix + SparseEigen Ab(nrows, ncols); + // See Eigen::set_from_triplets. This is about 5% faster. + // pass 1: count the nnz per inner-vector + std::vector nnz(ncols, 0); + for (const auto &entry : entries) nnz[std::get<1>(entry)]++; + Ab.reserve(nnz); + // pass 2: insert the elements + for (const auto &entry : entries) + Ab.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry); return Ab; } SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) { + gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering); return sparseJacobianEigen(gfg, Ordering(gfg.keys())); } diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 8b9ce94a9..bb07a36aa 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -36,16 +36,16 @@ using namespace boost::assign; using namespace std; using namespace gtsam; -typedef boost::tuple BoostTriplet; -bool triplet_equal(BoostTriplet a, BoostTriplet b) { - if (a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>() && - a.get<2>() == b.get<2>()) return true; +typedef std::tuple SparseTriplet; +bool triplet_equal(SparseTriplet a, SparseTriplet b) { + if (get<0>(a) == get<0>(b) && get<1>(a) == get<1>(b) && + get<2>(a) == get<2>(b)) return true; cout << "not equal:" << endl; cout << "\texpected: " - "(" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl; + "(" << get<0>(a) << ", " << get<1>(a) << ") = " << get<2>(a) << endl; cout << "\tactual: " - "(" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl; + "(" << get<0>(b) << ", " << get<1>(b) << ") = " << get<2>(b) << endl; return false; } @@ -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); } @@ -118,14 +119,14 @@ TEST(GaussianFactorGraph, sparseJacobian) { EXPECT(assert_equal(expectedMatlab, actual)); - // BoostTriplets + // SparseTriplets auto boostActual = gfg.sparseJacobian(); // check the triplets size... EXPECT_LONGS_EQUAL(16, boostActual.size()); // check content for (int i = 0; i < 16; i++) { EXPECT(triplet_equal( - BoostTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)), + SparseTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)), boostActual.at(i))); } }