diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index bc906d24e..f6bc1dd8c 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -9,20 +9,21 @@ namespace gtsam { /** - * JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis + * JacobianFactor for Schur complement that uses the "Nullspace Trick" by + * Mourikis et al. * * This trick is equivalent to the Schur complement, but can be faster. - * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses, - * is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., - * The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) - * where Enull is an m x (m-3) matrix - * Then Enull'*E*dp = 0, and + * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are + * poses, is multiplied by Enull, a matrix that spans the left nullspace of E, + * i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * + * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and * |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| * Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix. * - * The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks. - * Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) - * Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult + * The code below assumes that F is block diagonal and is given as a vector of + * ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for + * D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as + * a 1x2 * 2x6 multiplication. */ template class JacobianFactorSVD: public RegularJacobianFactor { @@ -37,10 +38,10 @@ public: JacobianFactorSVD() { } - /// Empty constructor with keys - JacobianFactorSVD(const KeyVector& keys, // - const SharedDiagonal& model = SharedDiagonal()) : - Base() { + /// Empty constructor with keys. + JacobianFactorSVD(const KeyVector& keys, + const SharedDiagonal& model = SharedDiagonal()) + : Base() { Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -51,18 +52,21 @@ public: } /** - * @brief Constructor - * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) - * and a reduced point derivative, Enull - * and creates a reduced-rank Jacobian factor on the CameraSet + * @brief Construct a new JacobianFactorSVD object, createing a reduced-rank + * Jacobian factor on the CameraSet. * - * @Fblocks: + * @param keys keys associated with F blocks. + * @param Fblocks CameraSet derivatives, ZDim*D blocks of block-diagonal F + * @param Enull a reduced point derivative + * @param b right-hand side + * @param model noise model */ - JacobianFactorSVD(const KeyVector& keys, - const std::vector >& Fblocks, const Matrix& Enull, - const Vector& b, // - const SharedDiagonal& model = SharedDiagonal()) : - Base() { + JacobianFactorSVD( + const KeyVector& keys, + const std::vector >& Fblocks, + const Matrix& Enull, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) + : Base() { size_t numKeys = Enull.rows() / ZDim; size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? // PLAIN nullptr SPACE TRICK @@ -74,9 +78,8 @@ public: QF.reserve(numKeys); for (size_t k = 0; k < Fblocks.size(); ++k) { Key key = keys[k]; - QF.push_back( - KeyMatrix(key, - (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k])); + QF.emplace_back( + key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]); } JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 2ed6aa491..b4a341719 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -1,6 +1,6 @@ /** * @file RegularImplicitSchurFactor.h - * @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor + * @brief A subclass of GaussianFactor specialized to structureless SFM. * @author Frank Dellaert * @author Luca Carlone */ @@ -20,6 +20,20 @@ namespace gtsam { /** * RegularImplicitSchurFactor + * + * A specialization of a GaussianFactor to structure-less SFM, which is very + * fast in a conjugate gradient (CG) solver. Specifically, as measured in + * timeSchurFactors.cpp, it stays very fast for an increasing number of cameras. + * The magic is in multiplyHessianAdd, which does the Hessian-vector multiply at + * the core of CG, and implements + * y += F'*alpha*(I - E*P*E')*F*x + * where + * - F is the 2mx6m Jacobian of the m 2D measurements wrpt m 6DOF poses + * - E is the 2mx3 Jacabian of the m 2D measurements wrpt a 3D point + * - P is the covariance on the point + * The equation above implicitly executes the Schur complement by removing the + * information E*P*E' from the Hessian. It is also very fast as we do not use + * the full 6m*6m F matrix, but rather only it's m 6x6 diagonal blocks. */ template class RegularImplicitSchurFactor: public GaussianFactor { @@ -38,9 +52,10 @@ protected: static const int ZDim = traits::dimension; ///< Measurement dimension typedef Eigen::Matrix MatrixZD; ///< type of an F block - typedef Eigen::Matrix MatrixDD; ///< camera hessian + typedef Eigen::Matrix MatrixDD; ///< camera Hessian + typedef std::vector > FBlocks; - const std::vector > FBlocks_; ///< All ZDim*D F blocks (one for each camera) + FBlocks FBlocks_; ///< All ZDim*D F blocks (one for each camera) const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Vector b_; ///< 2m-dimensional RHS vector @@ -52,17 +67,25 @@ public: } /// Construct from blocks of F, E, inv(E'*E), and RHS vector b - RegularImplicitSchurFactor(const KeyVector& keys, - const std::vector >& FBlocks, const Matrix& E, const Matrix& P, - const Vector& b) : - GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { - } + + /** + * @brief Construct a new RegularImplicitSchurFactor object. + * + * @param keys keys corresponding to cameras + * @param Fs All ZDim*D F blocks (one for each camera) + * @param E Jacobian of measurements wrpt point. + * @param P point covariance matrix + * @param b RHS vector + */ + RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs, + const Matrix& E, const Matrix& P, const Vector& b) + : GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {} /// Destructor ~RegularImplicitSchurFactor() override { } - std::vector >& FBlocks() const { + const FBlocks& Fs() const { return FBlocks_; }