From dde32f7fe2800b33794e52c598a3f7891715e9b4 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 17 Nov 2014 19:10:27 -0500 Subject: [PATCH] templated SmartFactor classes on measurement dimension, ZDim --- gtsam/slam/JacobianFactorQ.h | 16 ++++---- gtsam/slam/JacobianFactorQR.h | 12 +++--- gtsam/slam/JacobianFactorSVD.h | 18 ++++----- gtsam/slam/JacobianSchurFactor.h | 4 +- gtsam/slam/SmartFactorBase.h | 38 ++++++++++--------- gtsam/slam/SmartProjectionFactor.h | 12 +++--- gtsam/slam/SmartStereoProjectionFactor.h | 4 +- .../tests/testRegularImplicitSchurFactor.cpp | 4 +- 8 files changed, 57 insertions(+), 51 deletions(-) diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index de13f7ef0..9b9383d7b 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -12,10 +12,10 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template -class JacobianFactorQ: public JacobianSchurFactor { +template +class JacobianFactorQ: public JacobianSchurFactor { - typedef JacobianSchurFactor Base; + typedef JacobianSchurFactor Base; public: @@ -25,7 +25,7 @@ public: /// Empty constructor with keys JacobianFactorQ(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); typedef std::pair KeyMatrix; @@ -37,11 +37,11 @@ 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() { - size_t j = 0, m2 = E.rows(), m = m2 / 2; + JacobianSchurFactor() { + size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q Matrix Q = eye(m2) - E * P * E.transpose(); // Calculate pre-computed Jacobian matrices @@ -51,7 +51,7 @@ public: 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) - QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); + QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * 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..ccd6e8756 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -12,10 +12,10 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template -class JacobianFactorQR: public JacobianSchurFactor { +template +class JacobianFactorQR: public JacobianSchurFactor { - typedef JacobianSchurFactor Base; + typedef JacobianSchurFactor Base; public: @@ -25,14 +25,14 @@ public: JacobianFactorQR(const std::vector& Fblocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : - JacobianSchurFactor() { + JacobianSchurFactor() { // 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) { - gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second, - b.segment<2>(2 * i), model); + gfg.add(pointKey, E.block(ZDim * i, 0), it.first, it.second, + b.segment(ZDim * i), model); i += 1; } //gfg.print("gfg"); diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 8bed77d0f..e0a5f4566 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -11,12 +11,12 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template -class JacobianFactorSVD: public JacobianSchurFactor { +template +class JacobianFactorSVD: public JacobianSchurFactor { public: - typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 + typedef Eigen::Matrix Matrix2D; // e.g 2 x 6 with Z=Point2 typedef std::pair KeyMatrix2D; typedef std::pair KeyMatrix; @@ -25,7 +25,7 @@ public: /// Empty constructor with keys JacobianFactorSVD(const FastVector& keys, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { Matrix zeroMatrix = Matrix::Zero(0,D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -36,10 +36,10 @@ public: } /// Constructor - JacobianFactorSVD(const std::vector >& Fblocks, const Matrix& Enull, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { - size_t numKeys = Enull.rows() / Z::Dim(); - size_t j = 0, m2 = Z::Dim()*numKeys-3; + JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + size_t numKeys = Enull.rows() / ZDim; + size_t j = 0, m2 = ZDim*numKeys-3; // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) @@ -48,7 +48,7 @@ public: std::vector QF; QF.reserve(numKeys); BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) - QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, Z::Dim() * j++, m2, Z::Dim()) * it.second)); + 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 2beecc264..5d28bbada 100644 --- a/gtsam/slam/JacobianSchurFactor.h +++ b/gtsam/slam/JacobianSchurFactor.h @@ -18,12 +18,12 @@ namespace gtsam { /** * JacobianFactor for Schur complement that uses Q noise model */ -template +template class JacobianSchurFactor: public JacobianFactor { public: - typedef Eigen::Matrix Matrix2D; + typedef Eigen::Matrix Matrix2D; typedef std::pair KeyMatrix2D; // Use eigen magic to access raw memory diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index f7751422e..7c5a1175d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -34,7 +34,6 @@ #include #include #include -//#include namespace gtsam { /// Base class with no internal point, completely functional @@ -50,14 +49,16 @@ protected: boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + /// Definitions for blocks of F - typedef Eigen::Matrix Matrix2D; // F - typedef Eigen::Matrix MatrixD2; // F' + typedef Eigen::Matrix Matrix2D; // F + typedef Eigen::Matrix MatrixD2; // F' typedef std::pair KeyMatrix2D; // Fblocks typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix Matrix23; + typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix VectorD; - typedef Eigen::Matrix Matrix2; + typedef Eigen::Matrix Matrix2; /// shorthand for base class type typedef NonlinearFactor Base; @@ -65,6 +66,7 @@ protected: /// shorthand for this class typedef SmartFactorBase This; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -262,7 +264,7 @@ public: // **************************************************************************************************** /// Compute F, E only (called below in both vanilla and SVD versions) /// Given a Point3, assumes dimensionality is 3 - double computeJacobians(std::vector >& Fblocks, Matrix& E, + double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { size_t numKeys = this->keys_.size(); @@ -292,11 +294,11 @@ public: if (D == 6) { // optimize only camera pose Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); } else { - Hcam.block(0, 0) = Fi; // Z::Dim() x 6 block for the cameras - Hcam.block(0, 6) = Hcali; // Z::Dim() x nrCal block for the cameras + Hcam.block(0, 0) = Fi; // Z::Dim() x 6 block for the cameras + Hcam.block(0, 6) = Hcali; // Z::Dim() x nrCal block for the cameras Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); } - E.block(Z::dimension * i, 0) = Ei; + E.block(ZDim_t::value * i, 0) = Ei; subInsert(b, bi, Z::Dim() * i); } return f; @@ -304,7 +306,7 @@ public: // **************************************************************************************************** /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector >& Fblocks, Matrix& E, + double computeJacobians(std::vector& Fblocks, Matrix& E, Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -335,20 +337,20 @@ public: const double lambda = 0.0) const { size_t numKeys = this->keys_.size(); - std::vector > Fblocks; + std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); F = zeros(Z::Dim() * numKeys, D * numKeys); for (size_t i = 0; i < this->keys_.size(); ++i) { - F.block(Z::Dim() * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras + F.block(ZDim_t::value * i, D * i) = Fblocks.at(i).second; // Z::Dim() x 6 block for the cameras } return f; } // **************************************************************************************************** /// SVD version - double computeJacobiansSVD(std::vector >& Fblocks, Matrix& Enull, + double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -645,27 +647,27 @@ public: } // **************************************************************************************************** - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector > Fblocks; + std::vector Fblocks; Matrix E; Matrix3 PointCov; Vector b; computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, b); + return boost::make_shared >(Fblocks, E, PointCov, b); } // **************************************************************************************************** boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { size_t numKeys = this->keys_.size(); - std::vector < KeyMatrix2D,Eigen::aligned_allocator > Fblocks; + std::vector Fblocks; Vector b; Matrix Enull(Z::Dim()*numKeys, Z::Dim()*numKeys-3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6, Z> >(Fblocks, Enull, b); + return boost::make_shared< JacobianFactorSVD<6, ZDim_t::value> >(Fblocks, Enull, b); } private: diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index ee0636142..c4d0867dc 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -104,6 +104,8 @@ protected: /// shorthand for this class typedef SmartProjectionFactor This; + typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + public: /// shorthand for a smart pointer to a factor @@ -418,16 +420,16 @@ public: } /// create factor - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { Cameras myCameras; // TODO triangulate twice ?? @@ -435,7 +437,7 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// different (faster) way to compute Jacobian factor @@ -444,7 +446,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate diff --git a/gtsam/slam/SmartStereoProjectionFactor.h b/gtsam/slam/SmartStereoProjectionFactor.h index 9a73b84fe..4d2a69ddf 100644 --- a/gtsam/slam/SmartStereoProjectionFactor.h +++ b/gtsam/slam/SmartStereoProjectionFactor.h @@ -107,6 +107,8 @@ protected: /// shorthand for this class typedef SmartStereoProjectionFactor This; + typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + public: /// shorthand for a smart pointer to a factor @@ -480,7 +482,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 0161d4fb6..d94304ed5 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -115,7 +115,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { // Create JacobianFactor with same error const SharedDiagonal model; - JacobianFactorQ<6> jf(Fblocks, E, P, b, model); + JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model); { // error double expectedError = jf.error(xvalues); @@ -165,7 +165,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { } // Create JacobianFactorQR - JacobianFactorQR<6> jfq(Fblocks, E, P, b, model); + JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model); { const SharedDiagonal model; VectorValues yActual = zero;