From f8ef1d96044e3c29666a0d43aee46f84eb5580b9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 12 Jul 2013 22:27:40 +0000 Subject: [PATCH] Cleanups in JacobianFactorUnordered --- gtsam/linear/JacobianFactorUnordered-inl.h | 2 +- gtsam/linear/JacobianFactorUnordered.cpp | 38 ++++------------------ gtsam/linear/JacobianFactorUnordered.h | 31 +++++++----------- 3 files changed, 18 insertions(+), 53 deletions(-) diff --git a/gtsam/linear/JacobianFactorUnordered-inl.h b/gtsam/linear/JacobianFactorUnordered-inl.h index c552f19ad..6c3cee8d3 100644 --- a/gtsam/linear/JacobianFactorUnordered-inl.h +++ b/gtsam/linear/JacobianFactorUnordered-inl.h @@ -92,7 +92,7 @@ namespace gtsam { terms | transformed(&_getPairSecond) | transformed(boost::mem_fn(&Matrix::cols)), - boost::assign::cref_list_of<1>((DenseIndex)1)), b.size()); + boost::assign::cref_list_of<1,DenseIndex>(1)), b.size()); // Check and add terms typedef std::pair Term; diff --git a/gtsam/linear/JacobianFactorUnordered.cpp b/gtsam/linear/JacobianFactorUnordered.cpp index 7095f86b4..2476411f2 100644 --- a/gtsam/linear/JacobianFactorUnordered.cpp +++ b/gtsam/linear/JacobianFactorUnordered.cpp @@ -218,7 +218,9 @@ namespace gtsam { // Cast or convert to Jacobians std::vector jacobians = _convertOrCastToJacobians(graph); - // Order variable slots + // Order variable slots - we maintain the vector of ordered slots, as well as keep a list + // 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then + // be added after all of the ordered variables. vector orderedSlots; orderedSlots.reserve(variableSlots->size()); if(ordering) { @@ -258,9 +260,10 @@ namespace gtsam { // Allocate matrix and copy keys in order gttic(allocate); - Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1>((DenseIndex)1)), m); // Allocate augmented matrix + Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix Base::keys_.resize(orderedSlots.size()); - boost::range::copy(orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); // Get variable keys + boost::range::copy( // Get variable keys + orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); gttoc(allocate); // Loop over slots in combined factor and copy blocks from source factors @@ -354,7 +357,6 @@ namespace gtsam { /* ************************************************************************* */ Vector JacobianFactorUnordered::unweighted_error(const VectorValuesUnordered& c) const { Vector e = -getb(); - if (empty()) return e; for(size_t pos=0; pos > - JacobianFactorUnordered::sparse(const std::vector& columnIndices) const { - - std::vector > entries; - - // iterate over all variables in the factor - for(const_iterator var=begin(); varWhiten(getA(var))); - // find first column index for this key - size_t column_start = columnIndices[*var]; - 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(i, column_start + j, s)); - } - } - - Vector whitenedb(model_->whiten(getb())); - size_t bcolumn = columnIndices.back(); - for (size_t i = 0; i < (size_t) whitenedb.size(); i++) - entries.push_back(boost::make_tuple(i, bcolumn, whitenedb(i))); - - // return the result - return entries; - } - /* ************************************************************************* */ JacobianFactorUnordered JacobianFactorUnordered::whiten() const { JacobianFactorUnordered result(*this); diff --git a/gtsam/linear/JacobianFactorUnordered.h b/gtsam/linear/JacobianFactorUnordered.h index 04e741ebc..01245e52b 100644 --- a/gtsam/linear/JacobianFactorUnordered.h +++ b/gtsam/linear/JacobianFactorUnordered.h @@ -253,15 +253,6 @@ namespace gtsam { /** x += A'*e */ void transposeMultiplyAdd(double alpha, const Vector& e, VectorValuesUnordered& x) const; - /** - * Return vector of i, j, and s to generate an m-by-n sparse matrix - * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. - * As above, the standard deviations are baked into A and b - * @param columnIndices First column index for each variable. - */ - std::vector > - sparse(const std::vector& columnIndices) const; - /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactorUnordered whiten() const; @@ -272,17 +263,17 @@ namespace gtsam { /** set noiseModel correctly */ void setModel(bool anyConstrained, const Vector& sigmas); - /** - * Densely partially eliminate with QR factorization, this is usually provided as an argument to - * one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors - * are first factored with Cholesky decomposition to produce JacobianFactors, by calling the - * conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the - * order specified in \c keys. - * @param factors Factors to combine and eliminate - * @param keys The variables to eliminate in the order as specified here in \c keys - * @return The conditional and remaining factor - * - * \addtogroup LinearSolving */ + /** + * Densely partially eliminate with QR factorization, this is usually provided as an argument to + * one of the factor graph elimination functions (see EliminateableFactorGraph). HessianFactors + * are first factored with Cholesky decomposition to produce JacobianFactors, by calling the + * conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the + * order specified in \c keys. + * @param factors Factors to combine and eliminate + * @param keys The variables to eliminate in the order as specified here in \c keys + * @return The conditional and remaining factor + * + * \addtogroup LinearSolving */ friend GTSAM_EXPORT std::pair, boost::shared_ptr > EliminateQRUnordered(const GaussianFactorGraphUnordered& factors, const OrderingUnordered& keys);