diff --git a/gtsam_unstable/slam/ImplicitSchurFactor.h b/gtsam_unstable/slam/ImplicitSchurFactor.h index 139e194b4..234d13f1f 100644 --- a/gtsam_unstable/slam/ImplicitSchurFactor.h +++ b/gtsam_unstable/slam/ImplicitSchurFactor.h @@ -1,7 +1,8 @@ /** * @file ImplicitSchurFactor.h - * @brief A new type of linear factor (GaussianFactor), which is subclass of JacobiaFactor + * @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor * @author Frank Dellaert + * @author Luca Carlone */ #pragma once diff --git a/gtsam_unstable/slam/JacobianFactorQR.h b/gtsam_unstable/slam/JacobianFactorQR.h new file mode 100644 index 000000000..2d2d5b7a4 --- /dev/null +++ b/gtsam_unstable/slam/JacobianFactorQR.h @@ -0,0 +1,53 @@ +/* + * @file JacobianFactorQR.h + * @brief Jacobianfactor that combines and eliminates points + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once +#include + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianFactorQR: public JacobianSchurFactor { + + typedef JacobianSchurFactor Base; + +public: + + /** + * Constructor + */ + JacobianFactorQR(const std::vector& Fblocks, + const Matrix& E, const Matrix3& P, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : + 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); + i += 1; + } + //gfg.print("gfg"); + + // eliminate the point + GaussianBayesNet::shared_ptr bn; + GaussianFactorGraph::shared_ptr fg; + std::vector < Key > variables; + variables.push_back(pointKey); + boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); + //fg->print("fg"); + + JacobianFactor::operator=(JacobianFactor(*fg)); + } +}; +// class + +}// gtsam diff --git a/gtsam_unstable/slam/JacobianFactorSVD.h b/gtsam_unstable/slam/JacobianFactorSVD.h new file mode 100644 index 000000000..752a9f48e --- /dev/null +++ b/gtsam_unstable/slam/JacobianFactorSVD.h @@ -0,0 +1,44 @@ +/* + * @file JacobianFactorSVD.h + * @date Oct 27, 2013 + * @uthor Frank Dellaert + */ + +#pragma once +#include "gtsam_unstable/slam/JacobianSchurFactor.h" + +namespace gtsam { +/** + * JacobianFactor for Schur complement that uses Q noise model + */ +template +class JacobianFactorSVD: public JacobianSchurFactor { + +public: + + typedef Eigen::Matrix Matrix2D; + typedef std::pair KeyMatrix2D; + + /// Default constructor + JacobianFactorSVD() {} + + /// Constructor + JacobianFactorSVD(const std::vector& Fblocks, const Matrix& Enull, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor() { + size_t numKeys = Enull.rows() / 2; + size_t j = 0, m2 = 2*numKeys-3; + // PLAIN NULL SPACE TRICK + // Matrix Q = Enull * Enull.transpose(); + // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); + // JacobianFactor factor(QF, Q * b); + typedef std::pair KeyMatrix; + std::vector QF; + QF.reserve(numKeys); + BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) + QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, 2 * j++, m2, 2) * it.second)); + JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); + } +}; + +} diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index 2c81fb5f5..ebef221bb 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/gtsam_unstable/slam/SmartFactorBase.h @@ -51,6 +51,7 @@ protected: typedef Eigen::Matrix MatrixD2; // F' typedef std::pair KeyMatrix2D; // Fblocks typedef Eigen::Matrix MatrixDD; // camera hessian block + typedef Eigen::Matrix Matrix23; typedef Eigen::Matrix VectorD; typedef Eigen::Matrix Matrix2; @@ -321,7 +322,7 @@ public: const double lambda = 0.0) const { int numKeys = this->keys_.size(); - std::vector Fblocks; + std::vector < KeyMatrix2D > Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); F = zeros(2 * numKeys, D * numKeys); @@ -335,12 +336,13 @@ public: // **************************************************************************************************** /// SVD version double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, - Vector& b, const Cameras& cameras, const Point3& point, - double lambda = 0.0, bool diagonalDamping = false) const { + Vector& b, const Cameras& cameras, const Point3& point, double lambda = + 0.0, bool diagonalDamping = false) const { Matrix E; Matrix3 PointCov; // useless - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); // diagonalDamping should have no effect (only on PointCov) + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); // diagonalDamping should have no effect (only on PointCov) // Do SVD on A Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU); @@ -359,7 +361,7 @@ public: const Cameras& cameras, const Point3& point) const { int numKeys = this->keys_.size(); - std::vector Fblocks; + std::vector < KeyMatrix2D > Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); F.resize(2 * numKeys, D * numKeys); F.setZero(); @@ -378,13 +380,15 @@ public: int numKeys = this->keys_.size(); - std::vector Fblocks; + std::vector < KeyMatrix2D > Fblocks; Matrix E; Matrix3 PointCov; Vector b; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); +//#define HESSIAN_BLOCKS +#ifdef HESSIAN_BLOCKS // Create structures for Hessian Factors std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); std::vector < Vector > gs(numKeys); @@ -397,8 +401,44 @@ public: return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, f); +#else + size_t n1 = D * numKeys + 1; + std::vector dims(numKeys + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, D); + dims.back() = 1; + + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) + sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + augmentedHessian(numKeys,numKeys)(0,0) = f; + return boost::make_shared >( + this->keys_, augmentedHessian); + +#endif } + // **************************************************************************************************** + boost::shared_ptr > updateAugmentedHessian( + const Cameras& cameras, const Point3& point, const double lambda, + bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const { + + int numKeys = this->keys_.size(); + + std::vector < KeyMatrix2D > Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + + std::vector dims(numKeys + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, D); + dims.back() = 1; + + updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + std::cout << "f "<< f <& Fblocks, const Matrix& E, const Matrix& PointCov, const Vector& b, @@ -437,47 +477,133 @@ public: } // **************************************************************************************************** - void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& PointCov, const Vector& b, - /*output ->*/std::vector& Gs, std::vector& gs) const { + void updateSparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { // Schur complement trick - // Gs = F' * F - F' * E * inv(E'*E) * E' * F - // gs = F' * (b - E * inv(E'*E) * E' * b) + // Gs = F' * F - F' * E * P * E' * F + // gs = F' * (b - E * P * E' * b) + + // a single point is observed in numKeys cameras + size_t numKeys = this->keys_.size(); // cameras observing current point + size_t aug_numKeys = augmentedHessian.rows() - 1; // all cameras in the group + + // Blockwise Schur complement + for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera + + const Matrix2D& Fi1 = Fblocks.at(i1).second; + const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + + // D = (Dx2) * (2) + // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b + size_t aug_i1 = this->keys_[i1]; + std::cout << "i1 "<< i1 < (2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + + // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + std::cout << "filled 1 " <(2 * i1, 0).transpose() * Fi1); + + // upper triangular part of the hessian + for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + const Matrix2D& Fi2 = Fblocks.at(i2).second; + size_t aug_i2 = this->keys_[i2]; + std::cout << "i2 "<< i2 <(2 * i2, 0).transpose() * Fi2); + } + } // end of for over cameras + } + + // **************************************************************************************************** + void sparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { + // Schur complement trick + // Gs = F' * F - F' * E * P * E' * F + // gs = F' * (b - E * P * E' * b) // a single point is observed in numKeys cameras size_t numKeys = this->keys_.size(); // Blockwise Schur complement - int GsCount = 0; for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera - const Matrix2D& Fi1 = Fblocks.at(i1).second; - // D = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - for (size_t i2 = 0; i2 < numKeys; i2++) { + const Matrix2D& Fi1 = Fblocks.at(i1).second; + const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + + // D = (Dx2) * (2) + // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b + augmentedHessian(i1,numKeys) = Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + + // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + augmentedHessian(i1,i1) = + Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + + // upper triangular part of the hessian + for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera const Matrix2D& Fi2 = Fblocks.at(i2).second; - // Compute (Ei1 * PointCov * Ei2') - // (2x2) = (2x3) * (3x3) * (3x2) - Matrix2 E_invEtE_Et = E.block<2, 3>(2 * i1, 0) * PointCov * (E.block<2, 3>(2 * i2, 0)).transpose(); - - // D = (Dx2) * (2x2) * (2) - gs.at(i1) -= Fi1.transpose() * ( E_invEtE_Et * b.segment < 2 > (2 * i2) ); - - if (i2 == i1) { // diagonal entries - // (DxD) = (Dx2) * ( (2xD) - (2x2) * (2xD) ) - Gs.at(GsCount) = Fi1.transpose() * (Fi1 - E_invEtE_Et * Fi2); - GsCount++; - } - if (i2 > i1) { // off diagonal - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - Gs.at(GsCount) = - Fi1.transpose() * ( E_invEtE_Et * Fi2 ); - GsCount++; - } + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian(i1,i2) = + -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); } - } + } // end of for over cameras } + // **************************************************************************************************** + void sparseSchurComplement(const std::vector& Fblocks, + const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + /*output ->*/std::vector& Gs, std::vector& gs) const { + // Schur complement trick + // Gs = F' * F - F' * E * P * E' * F + // gs = F' * (b - E * P * E' * b) + + // a single point is observed in numKeys cameras + size_t numKeys = this->keys_.size(); + + int GsIndex = 0; + // Blockwise Schur complement + for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera + // GsIndex points to the upper triangular blocks + // 0 1 2 3 4 + // X 5 6 7 8 + // X X 9 10 11 + // X X X 12 13 + // X X X X 14 + const Matrix2D& Fi1 = Fblocks.at(i1).second; + + const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P; + + { // for i1 = i2 + // D = (Dx2) * (2) + gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b + // D = (Dx2) * (2x3) * (3*2m) * (2m x 1) + gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b)); + + // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) + Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); + GsIndex++; + } + // upper triangular part of the hessian + for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera + const Matrix2D& Fi2 = Fblocks.at(i2).second; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); + GsIndex++; + } + } // end of for over cameras + } // **************************************************************************************************** boost::shared_ptr > createImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, @@ -494,7 +620,7 @@ public: boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { - std::vector Fblocks; + std::vector < KeyMatrix2D > Fblocks; Matrix E; Matrix3 PointCov; Vector b;