diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index f4dfb9422..957041abc 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -6,16 +6,20 @@ #pragma once -#include "JacobianSchurFactor.h" +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public JacobianSchurFactor { +class JacobianFactorQ: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; +private: + + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; public: @@ -25,7 +29,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -37,10 +41,10 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, + JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + RegularJacobianFactor() { size_t j = 0, m2 = E.rows(), m = m2 / 2; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); @@ -50,7 +54,7 @@ public: std::vector < KeyMatrix > QF; QF.reserve(m); // Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D) - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // Which is then passed to the normal JacobianFactor constructor JacobianFactor::fillTerms(QF, Q * b, model); diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index a928106a8..30faf9ec0 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,31 +6,35 @@ */ #pragma once -#include +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public JacobianSchurFactor { +class JacobianFactorQR: public RegularJacobianFactor { - typedef JacobianSchurFactor Base; +private: + + typedef RegularJacobianFactor Base; + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, + JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + RegularJacobianFactor() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) { + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) { gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, b.segment<2>(2 * i), model); i += 1; diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e28185038..0f4c52038 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,27 +5,29 @@ */ #pragma once -#include "gtsam/slam/JacobianSchurFactor.h" +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public JacobianSchurFactor { +class JacobianFactorSVD: public RegularJacobianFactor { -public: +private: typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; typedef std::pair KeyMatrix; +public: + /// Default constructor JacobianFactorSVD() {} /// Empty constructor with keys JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -37,7 +39,7 @@ public: /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { size_t numKeys = Enull.rows() / 2; size_t j = 0, m2 = 2*numKeys-3; // PLAIN NULL SPACE TRICK diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h deleted file mode 100644 index 2beecc264..000000000 --- a/gtsam/slam/JacobianSchurFactor.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * @file JacobianSchurFactor.h - * @brief Jacobianfactor that combines and eliminates points - * @date Oct 27, 2013 - * @uthor Frank Dellaert - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam { -/** - * JacobianFactor for Schur complement that uses Q noise model - */ -template -class JacobianSchurFactor: public JacobianFactor { - -public: - - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - /** - * @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; poswhiten(Ax) : Ax; - } - - /** - * @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; postransposeMultiplyAdd(alpha,Ax,y); - if (empty()) return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } - - // multiply with alpha - Ax *= alpha; - - // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; pos +#include #include #include #include @@ -30,6 +32,9 @@ class RegularJacobianFactor: public JacobianFactor { private: + typedef JacobianFactor Base; + typedef RegularJacobianFactor This; + /** Use eigen magic to access raw memory */ typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; @@ -37,6 +42,10 @@ private: 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. */ @@ -57,6 +66,33 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } + /** + * @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; poswhiten(Ax) : Ax; + } + + /** + * @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& 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()); @@ -81,7 +112,7 @@ public: for (size_t pos = 0; pos < size(); ++pos) { Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); + * ConstDMap(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_) { @@ -94,12 +125,15 @@ public: /// 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_( + DMap(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; @@ -168,6 +202,13 @@ public: } } + /** Non-RAW memory access versions for testRegularImplicitSchurFactor + * TODO: They should be removed from Regular Factors + */ + using Base::multiplyHessianAdd; + using Base::hessianDiagonal; + using Base::gradientAtZero; + }; }