diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index d33f3325b..c18ad07d1 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -24,9 +24,11 @@ namespace gtsam { * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQ: public JacobianSchurFactor { +class JacobianFactorQ: public JacobianSchurFactor { - typedef JacobianSchurFactor Base; + typedef JacobianSchurFactor Base; + typedef Eigen::Matrix MatrixZD; + typedef std::pair KeyMatrixZD; public: @@ -37,7 +39,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, // const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + Base() { Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -49,10 +51,10 @@ public: } /// Constructor - JacobianFactorQ(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + JacobianFactorQ(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b, const SharedDiagonal& model = + SharedDiagonal()) : + Base() { size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); @@ -62,7 +64,7 @@ public: std::vector QF; QF.reserve(m); // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) - BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) QF.push_back( KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); // Which is then passed to the normal JacobianFactor constructor diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index 112278766..cf72fd0b3 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -18,24 +18,26 @@ class GaussianBayesNet; * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorQR: public JacobianSchurFactor { +class JacobianFactorQR: public JacobianSchurFactor { - typedef JacobianSchurFactor Base; + typedef JacobianSchurFactor Base; + typedef Eigen::Matrix MatrixZD; + typedef std::pair KeyMatrixZD; public: /** * Constructor */ - JacobianFactorQR(const std::vector& Fblocks, - const Matrix& E, const Matrix3& P, const Vector& b, + JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, + const Matrix3& P, const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + Base() { // 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 KeyMatrixZD& it, Fblocks) { gfg.add(pointKey, E.block(ZDim * i, 0), it.first, it.second, b.segment(ZDim * i), model); i += 1; @@ -45,7 +47,7 @@ public: // eliminate the point boost::shared_ptr bn; GaussianFactorGraph::shared_ptr fg; - std::vector < Key > variables; + std::vector variables; variables.push_back(pointKey); boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); //fg->print("fg"); @@ -53,6 +55,6 @@ public: JacobianFactor::operator=(JacobianFactor(*fg)); } }; -// class +// end class JacobianFactorQR -}// gtsam +}// end namespace gtsam diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e0a5f4566..37043f448 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -12,43 +12,50 @@ namespace gtsam { * JacobianFactor for Schur complement that uses Q noise model */ template -class JacobianFactorSVD: public JacobianSchurFactor { +class JacobianFactorSVD: public JacobianSchurFactor { + + typedef JacobianSchurFactor Base; + typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 + typedef std::pair KeyMatrixZD; + typedef std::pair KeyMatrix; public: - typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 - typedef std::pair KeyMatrix2D; - typedef std::pair KeyMatrix; - /// Default constructor - JacobianFactorSVD() {} + JacobianFactorSVD() { + } /// Empty constructor with keys - JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - Matrix zeroMatrix = Matrix::Zero(0,D); + JacobianFactorSVD(const FastVector& keys, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { + Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); std::vector QF; QF.reserve(keys.size()); BOOST_FOREACH(const Key& key, keys) - QF.push_back(KeyMatrix(key, zeroMatrix)); + QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } /// Constructor - JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + JacobianFactorSVD(const std::vector& Fblocks, + const Matrix& Enull, const Vector& b, // + const SharedDiagonal& model = SharedDiagonal()) : + Base() { size_t numKeys = Enull.rows() / ZDim; - size_t j = 0, m2 = ZDim*numKeys-3; + size_t j = 0, m2 = ZDim * numKeys - 3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); - // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); std::vector QF; QF.reserve(numKeys); - BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); + BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + QF.push_back( + KeyMatrix(it.first, + (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h index e7f99a736..6a218a9a7 100644 --- a/gtsam/slam/JacobianSchurFactor.h +++ b/gtsam/slam/JacobianSchurFactor.h @@ -27,14 +27,11 @@ namespace gtsam { * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD * Provides raw memory access versions of linear operator. */ -template +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;