diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e5e39d1a1..0b0d76fda 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -63,7 +63,7 @@ public: QF.push_back( KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second)); - JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); + JacobianFactor::fillTerms(QF, - Enull.transpose() * b, model); } }; diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 88fb4b6e1..fd1a5764d 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -128,7 +128,7 @@ public: * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix * Fast version - works on with sparsity */ - static void sparseSchurComplement(const std::vector& Fblocks, + static void SparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, /*output ->*/SymmetricBlockMatrix& augmentedHessian) { // Schur complement trick @@ -167,7 +167,7 @@ public: * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. */ - static void updateSparseSchurComplement( + static void UpdateSparseSchurComplement( const std::vector& Fblocks, const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, const FastVector& allKeys, const FastVector& keys, @@ -248,7 +248,7 @@ public: SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); // Do the Schur complement - sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian); + SparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian); return augmentedHessian.matrix(); } @@ -257,7 +257,7 @@ public: Matrix augmented = augmentedInformation(); int m = this->keys_.size(); size_t M = D * m; - return augmented.block(0,0,M,M); + return augmented.block(0, 0, M, M); } /// Return the diagonal of the Hessian for this factor diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index c9421e173..60e939a89 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -272,7 +272,7 @@ public: EtE.diagonal() += lambda * EtE.diagonal(); } else { DenseIndex n = E.cols(); - EtE += lambda * Eigen::MatrixXd::Identity(n,n); + EtE += lambda * Eigen::MatrixXd::Identity(n, n); } return (EtE).inverse(); @@ -342,7 +342,8 @@ public: SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); // build augmented hessian - sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); + RegularImplicitSchurFactor::SparseSchurComplement(Fblocks, E, P, b, + augmentedHessian); augmentedHessian(m, m)(0, 0) = f; return boost::make_shared >(keys_, @@ -363,7 +364,7 @@ public: std::vector Fblocks; double f = computeJacobians(Fblocks, E, b, cameras, point); Matrix3 P = PointCov(E, lambda, diagonalDamping); - RegularImplicitSchurFactor::updateSparseSchurComplement(Fblocks, + RegularImplicitSchurFactor::UpdateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, keys_, augmentedHessian); } @@ -420,7 +421,8 @@ public: const size_t M = ZDim * m; Matrix E0(M, M - 3); computeJacobiansSVD(F, E0, b, cameras, point); - SharedIsotropic n = noiseModel::Isotropic::Sigma(M-3, noiseModel_->sigma()); + SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3, + noiseModel_->sigma()); return boost::make_shared >(F, E0, b, n); }