diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 6a359e9d1..947d4745c 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -100,24 +100,15 @@ namespace gtsam { } /* ************************************************************************* */ - /** Performs in-place population of a sparse jacobian. Contains the - * common functionality amongst different sparseJacobian functions. - * @param graph the factor graph to get the Jacobian from - * @param entries a container of triplets that supports - * `emplace_back(size_t, size_t, double) - * @param ordering the variable ordering - * @param[out] nrows the nurmber of rows in the Jacobian - * @param[out] ncols the number of columns in the Jacobian - */ - template - void sparseJacobianInPlace(const GaussianFactorGraph& graph, T& entries, - const Ordering& ordering, size_t& nrows, - size_t& ncols) { - gttic_(GaussianFactorGraph_sparseJacobianInPlace); + 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 auto& factor : graph) { + for (const auto& factor : *this) { if (!static_cast(factor)) continue; for (auto it = factor->begin(); it != factor->end(); ++it) { @@ -134,8 +125,10 @@ namespace gtsam { } // Iterate over all factors, adding sparse scalar entries + SparseTriplets entries; + entries.reserve(60 * size()); nrows = 0; - for (const auto& factor : graph) { + for (const auto& factor : *this) { if (!static_cast(factor)) continue; // Convert to JacobianFactor if necessary @@ -179,16 +172,13 @@ namespace gtsam { } ncols++; // +1 for b-column + return entries; } /* ************************************************************************* */ - using BoostTriplets = std::vector >; - BoostTriplets GaussianFactorGraph::sparseJacobian() const { - BoostTriplets entries; - entries.reserve(60 * size()); + SparseTriplets GaussianFactorGraph::sparseJacobian() const { size_t nrows, ncols; - sparseJacobianInPlace(*this, entries, Ordering(this->keys()), nrows, ncols); - return entries; + return sparseJacobian(Ordering(this->keys()), nrows, ncols); } /* ************************************************************************* */ @@ -202,23 +192,13 @@ namespace gtsam { 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>(); + 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; } - /* ************************************************************************* */ - using GtsamTriplets = std::vector >; - GtsamTriplets GaussianFactorGraph::sparseJacobianFast( - const Ordering& ordering, size_t& nrows, size_t& ncols) const { - GtsamTriplets entries; - entries.reserve(60 * size()); - sparseJacobianInPlace(*this, entries, ordering, nrows, ncols); - return entries; - } - /* ************************************************************************* */ Matrix GaussianFactorGraph::augmentedJacobian( const Ordering& ordering) const { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 661d0a4a8..e3304d5e8 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -181,13 +181,20 @@ namespace gtsam { ///@{ /** - * 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. + * 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 boost tuples + * @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] @@ -197,15 +204,6 @@ namespace gtsam { */ Matrix sparseJacobian_() const; - /** Returns a sparse matrix with `int` indices instead of `size_t` for - * slightly faster performance - * @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 - */ - std::vector > sparseJacobianFast( - const Ordering& ordering, size_t& nrows, size_t& ncols) const; - /** * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ * Jacobian matrix, augmented with b with the noise models baked diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h index bc99b5c89..483ae7f8d 100644 --- a/gtsam/linear/SparseEigen.h +++ b/gtsam/linear/SparseEigen.h @@ -41,7 +41,7 @@ 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.sparseJacobianFast(ordering, 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. diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 493e7667c..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; } @@ -119,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))); } }