From e15cfb3d339dfdd5f46ccc944ca9746e6eae855f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 22 Feb 2015 08:10:34 +0100 Subject: [PATCH] Grabbed some methods from JacobianSchurFactor, added VectorValues versions --- gtsam/slam/RegularJacobianFactor.h | 165 ++++++++++++++++++----------- 1 file changed, 106 insertions(+), 59 deletions(-) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index dcf709854..38e1407f0 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -21,25 +21,34 @@ #include #include #include -#include namespace gtsam { +/** + * JacobianFactor with constant sized blocks + * Provides raw memory access versions of linear operator. + * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD + */ template class RegularJacobianFactor: public JacobianFactor { private: - /** Use eigen magic to access raw memory */ + // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; typedef Eigen::Map ConstDMap; public: + /// Default constructor + RegularJacobianFactor() {} + /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the - * collection of keys and matrices making up the factor. */ + * collection of keys and matrices making up the factor. + * TODO Verify terms are regular + */ template RegularJacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : @@ -49,7 +58,9 @@ public: /** Constructor with arbitrary number keys, and where the augmented matrix is given all together * instead of in block terms. Note that only the active view of the provided augmented matrix * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed - * factor. */ + * factor. + * TODO Verify complies to regular + */ template RegularJacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = @@ -63,52 +74,11 @@ public: JacobianFactor::multiplyHessianAdd(alpha, x, y); } - /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x - * Note: this is not assuming a fixed dimension for the variables, - * but requires the vector accumulatedDims to tell the dimension of - * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, - * then accumulatedDims is [0 3 9 11 13] - * NOTE: size of accumulatedDims is size of keys + 1!! */ - void multiplyHessianAdd(double alpha, const double* x, double* y, - const std::vector& accumulatedDims) const { - - /// Use eigen magic to access raw memory - typedef Eigen::Matrix VectorD; - typedef Eigen::Map MapD; - typedef Eigen::Map ConstMapD; - - if (empty()) - return; - Vector Ax = zero(Ab_.rows()); - - /// Just iterate over all A matrices and multiply in correct config part (looping over keys) - /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' - /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) - for (size_t pos = 0; pos < size(); ++pos) { - Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], - accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); - } - /// Deal with noise properly, need to Double* whiten as we are dividing by variance - if (model_) { - model_->whitenInPlace(Ax); - model_->whitenInPlace(Ax); - } - - /// multiply with alpha - Ax *= alpha; - - /// Again iterate over all A matrices and insert Ai^T into y - for (size_t pos = 0; pos < size(); ++pos) { - MapD(y + accumulatedDims[keys_[pos]], - accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( - pos).transpose() * Ax; - } - } - - /** Raw memory access version of multiplyHessianAdd */ + /** + * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x) + * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way + */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { - if (empty()) return; Vector Ax = zero(Ab_.rows()); @@ -131,10 +101,13 @@ public: DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax; } - /** Raw memory access version of hessianDiagonal - * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n - */ - virtual void hessianDiagonal(double* d) const { + /// Expose base class hessianDiagonal + virtual VectorValues hessianDiagonal() const { + return JacobianFactor::hessianDiagonal(); + } + + /// Raw memory access version of hessianDiagonal + void hessianDiagonal(double* d) const { // Loop over all variables in the factor for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal @@ -152,10 +125,13 @@ public: } } - /** Raw memory access version of gradientAtZero - * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n - */ - virtual void gradientAtZero(double* d) const { + /// Expose base class gradientAtZero + virtual VectorValues gradientAtZero() const { + return JacobianFactor::gradientAtZero(); + } + + /// Raw memory access version of gradientAtZero + void gradientAtZero(double* d) const { // Get vector b not weighted Vector b = getb(); @@ -179,7 +155,78 @@ public: } } + /** + * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e + * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way + */ + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const { + Vector E = alpha * (model_ ? model_->whiten(e) : e); + // Just iterate over all A matrices and insert Ai^e into y + for (size_t pos = 0; pos < size(); ++pos) + DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E; + } + + /** + * @brief double* Matrix-vector multiply, i.e. y = A*x + * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way + */ + Vector operator*(const double* x) const { + Vector Ax = zero(Ab_.rows()); + if (empty()) + return Ax; + + // Just iterate over all A matrices and multiply in correct config part + for (size_t pos = 0; pos < size(); ++pos) + Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]); + + return model_ ? model_->whiten(Ax) : Ax; + } + + /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * Note: this is not assuming a fixed dimension for the variables, + * but requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! + * TODO Frank asks: why is this here if not regular ???? + */ + void multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const { + + /// Use Eigen magic to access raw memory + typedef Eigen::Map VectorMap; + typedef Eigen::Map ConstVectorMap; + + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + /// Just iterate over all A matrices and multiply in correct config part (looping over keys) + /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + Ax += Ab_(pos) * ConstVectorMap(x + offset, dim); + } + /// Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + /// multiply with alpha + Ax *= alpha; + + /// Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax; + } + } + }; +// end class RegularJacobianFactor -} - +}// end namespace gtsam