diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index b987ba24e..c8446e6a7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -174,40 +174,20 @@ namespace gtsam { } /* ************************************************************************* */ - using BoostTriplets = GaussianFactorGraph::SparseMatrixBoostTriplets; - BoostTriplets GaussianFactorGraph::sparseJacobian( - const Ordering& ordering, size_t& nrows, size_t& ncols) const { + using BoostTriplets = std::vector>; + BoostTriplets GaussianFactorGraph::sparseJacobian() const { BoostTriplets entries; entries.reserve(60 * size()); - sparseJacobianInPlace(entries, ordering, nrows, ncols); + size_t nrows, ncols; + sparseJacobianInPlace(entries, Ordering(this->keys()), nrows, ncols); return entries; } /* ************************************************************************* */ - BoostTriplets GaussianFactorGraph::sparseJacobian( - const Ordering& ordering) const { - size_t dummy1, dummy2; - return sparseJacobian(ordering, dummy1, dummy2); - } - - /* ************************************************************************* */ - BoostTriplets GaussianFactorGraph::sparseJacobian( - size_t& nrows, size_t& ncols) const { - return sparseJacobian(Ordering(this->keys()), nrows, ncols); - } - - /* ************************************************************************* */ - BoostTriplets GaussianFactorGraph::sparseJacobian() const { - size_t dummy1, dummy2; - return sparseJacobian(dummy1, dummy2); - } - - /* ************************************************************************* */ - Matrix GaussianFactorGraph::sparseJacobian_( - const Ordering& ordering, size_t& nrows, size_t& ncols) const { + Matrix GaussianFactorGraph::sparseJacobian_() const { gttic_(GaussianFactorGraph_sparseJacobian_matrix); // call sparseJacobian - auto result = sparseJacobian(ordering, nrows, ncols); + auto result = sparseJacobian(); // translate to base 1 matrix size_t nzmax = result.size(); @@ -222,26 +202,7 @@ namespace gtsam { } /* ************************************************************************* */ - 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 { - size_t dummy1, dummy2; - return sparseJacobian_(dummy1, dummy2); - } - - /* ************************************************************************* */ - using GtsamTriplets = GaussianFactorGraph::SparseMatrixGtsamTriplets; + using GtsamTriplets = std::vector>; GtsamTriplets GaussianFactorGraph::sparseJacobianFast( const Ordering& ordering, size_t& nrows, size_t& ncols) const { GtsamTriplets entries; @@ -250,25 +211,6 @@ namespace gtsam { return entries; } - /* ************************************************************************* */ - GtsamTriplets GaussianFactorGraph::sparseJacobianFast( - const Ordering& ordering) const { - size_t dummy1, dummy2; - return sparseJacobianFast(ordering, dummy1, dummy2); - } - - /* ************************************************************************* */ - GtsamTriplets GaussianFactorGraph::sparseJacobianFast( - size_t& nrows, size_t& ncols) const { - return sparseJacobianFast(Ordering(this->keys()), nrows, ncols); - } - - /* ************************************************************************* */ - GtsamTriplets GaussianFactorGraph::sparseJacobianFast() const { - size_t dummy1, dummy2; - return sparseJacobianFast(dummy1, dummy2); - } - /* ************************************************************************* */ Matrix GaussianFactorGraph::augmentedJacobian( const Ordering& ordering) const { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 584eb02ce..70327d9f4 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -180,38 +180,14 @@ namespace gtsam { ///@name Linear Algebra ///@{ - /// Sparse matrix representation as vector of tuples. - typedef std::vector> - SparseMatrixBoostTriplets; - /// Sparse matrix representation as vector of slightly more efficient - /// tuples. - typedef std::vector> SparseMatrixGtsamTriplets; - /** * 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 */ - 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; + std::vector> sparseJacobian() const; /** * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] @@ -219,44 +195,16 @@ namespace gtsam { * 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 - * @param[out] nrows The number of rows in the Jacobian - * @param[out] ncols The number of columns in the Jacobian - */ - 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; /** Returns a sparse matrix with `int` indices instead of `size_t` for - * slightly faster performance */ - SparseMatrixGtsamTriplets sparseJacobianFast(const Ordering& ordering, - size_t& nrows, - size_t& ncols) const; - - /** Returns an int-indexed sparse matrix without outputting its dimensions - */ - SparseMatrixGtsamTriplets sparseJacobianFast(const Ordering& ordering) const; - - /** Returns an int-indexed sparse matrix with default ordering + * 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 */ - SparseMatrixGtsamTriplets sparseJacobianFast(size_t& nrows, - size_t& ncols) const; - - /** Returns an int-indexed sparse matrix with default ordering and without - * outputting its dimensions */ - SparseMatrixGtsamTriplets sparseJacobianFast() const; + 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$