From 7091c2bd2ebc1f216c9b31e6fb14afe2a29ba553 Mon Sep 17 00:00:00 2001 From: Sungtae An Date: Thu, 1 Jan 2015 17:35:58 -0500 Subject: [PATCH] To resolve conflicts first, revert the commitment of merging JacobianSchurFactor into RegularJacobianFactor. JacobianSchurFactor is merged into RegularJacobianFactor. Derived classes from JacobianSchurFactor are changed to be derived from RegularJacobianFactor. (reverted from commit 7e0033208c2d3b635dbd14246e869ec7fcd254f8) --- gtsam/slam/JacobianFactorQ.h | 18 +++--- gtsam/slam/JacobianFactorQR.h | 16 ++--- gtsam/slam/JacobianFactorSVD.h | 12 ++-- gtsam/slam/JacobianSchurFactor.h | 93 ++++++++++++++++++++++++++++++ gtsam/slam/RegularJacobianFactor.h | 57 +++--------------- 5 files changed, 119 insertions(+), 77 deletions(-) create mode 100644 gtsam/slam/JacobianSchurFactor.h diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 957041abc..f4dfb9422 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -6,20 +6,16 @@ #pragma once -#include +#include "JacobianSchurFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public RegularJacobianFactor { +class JacobianFactorQ: public JacobianSchurFactor { -private: - - typedef RegularJacobianFactor Base; - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; + typedef JacobianSchurFactor Base; public: @@ -29,7 +25,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -41,10 +37,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()) : - RegularJacobianFactor() { + JacobianSchurFactor() { size_t j = 0, m2 = E.rows(), m = m2 / 2; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); @@ -54,7 +50,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 KeyMatrix2D& it, Fblocks) + BOOST_FOREACH(const typename Base::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 30faf9ec0..a928106a8 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -6,35 +6,31 @@ */ #pragma once -#include +#include namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public RegularJacobianFactor { +class JacobianFactorQR: public JacobianSchurFactor { -private: - - typedef RegularJacobianFactor Base; - typedef Eigen::Matrix Matrix2D; - typedef std::pair KeyMatrix2D; + typedef JacobianSchurFactor Base; 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()) : - RegularJacobianFactor() { + JacobianSchurFactor() { // Create a number of Jacobian factors in a factor graph GaussianFactorGraph gfg; Symbol pointKey('p', 0); size_t i = 0; - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) { + BOOST_FOREACH(const typename Base::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 0f4c52038..e28185038 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -5,29 +5,27 @@ */ #pragma once -#include +#include "gtsam/slam/JacobianSchurFactor.h" namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public RegularJacobianFactor { +class JacobianFactorSVD: public JacobianSchurFactor { -private: +public: 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()) : RegularJacobianFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -39,7 +37,7 @@ public: /// Constructor JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { 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 new file mode 100644 index 000000000..2beecc264 --- /dev/null +++ b/gtsam/slam/JacobianSchurFactor.h @@ -0,0 +1,93 @@ +/* + * @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 @@ -32,9 +30,6 @@ 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; @@ -42,10 +37,6 @@ 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. */ @@ -66,33 +57,6 @@ 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()); @@ -112,7 +81,7 @@ public: for (size_t pos = 0; pos < size(); ++pos) { Ax += Ab_(pos) - * ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[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_) { @@ -125,15 +94,12 @@ public: /// Again iterate over all A matrices and insert Ai^T into y for (size_t pos = 0; pos < size(); ++pos){ - DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( + MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; } } - /** - * @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 - */ + /** Raw memory access version of multiplyHessianAdd */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { if (empty()) return; @@ -202,13 +168,6 @@ 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; - }; }