From 0bf95ae7f63e1bf26845a843bb0d59c0df917513 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 26 Feb 2015 11:44:51 +0100 Subject: [PATCH] Removed obsolete code, including slow Schur-complement versions --- gtsam/slam/SmartFactorBase.h | 133 +----------------- .../tests/testSmartProjectionCameraFactor.cpp | 5 +- .../slam/SmartStereoProjectionFactor.h | 10 +- 3 files changed, 12 insertions(+), 136 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index a5d2cfabe..10de39049 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -342,18 +342,6 @@ public: F.block(This::ZDim * i, Dim * i) = Fblocks.at(i).second; } - /** - * Compute F, E, and b, where F and E are the stacked derivatives - * with respect to the point. The value of cameras/point are passed as parameters. - */ - double computeJacobians(Matrix& F, Matrix& E, Vector& b, - const Cameras& cameras, const Point3& point) const { - std::vector Fblocks; - double f = computeJacobians(Fblocks, E, b, cameras, point); - FillDiagonalF(Fblocks, F); - return f; - } - /// SVD version double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point) const { @@ -370,16 +358,6 @@ public: return f; } - /// Matrix version of SVD - // TODO, there should not be a Matrix version, really - double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, - const Cameras& cameras, const Point3& point) const { - std::vector Fblocks; - double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - FillDiagonalF(Fblocks, F); - return f; - } - /** * Linearize returns a Hessianfactor that is an approximation of error(p) */ @@ -394,69 +372,19 @@ public: double f = computeJacobians(Fblocks, E, b, cameras, point); Matrix3 P = PointCov(E, lambda, diagonalDamping); -//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix -#ifdef HESSIAN_BLOCKS - // Create structures for Hessian Factors - std::vector < Matrix > Gs(m * (m + 1) / 2); - std::vector < Vector > gs(m); - - sparseSchurComplement(Fblocks, E, P, b, Gs, gs); - // schurComplement(Fblocks, E, P, b, Gs, gs); - - //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); - //std::vector < Vector > gs2(gs.begin(), gs.end()); - - return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, f); -#else // we create directly a SymmetricBlockMatrix - size_t n1 = Dim * m + 1; + // we create directly a SymmetricBlockMatrix + size_t M1 = Dim * m + 1; std::vector dims(m + 1); // this also includes the b term std::fill(dims.begin(), dims.end() - 1, Dim); dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*Dim+1 x 10*Dim+1) - sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + // build augmented hessian + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); augmentedHessian(m, m)(0, 0) = f; + return boost::make_shared >(keys_, augmentedHessian); -#endif - } - - /** - * Do Schur complement, given Jacobian as F,E,P. - * Slow version - works on full matrices - */ - void schurComplement(const std::vector& Fblocks, const Matrix& E, - const Matrix3& P, const Vector& b, - /*output ->*/std::vector& Gs, std::vector& gs) const { - // Schur complement trick - // Gs = F' * F - F' * E * inv(E'*E) * E' * F - // gs = F' * (b - E * inv(E'*E) * E' * b) - // This version uses full matrices - - int numKeys = this->keys_.size(); - - /// Compute Full F ???? - Matrix F; - FillDiagonalF(Fblocks, F); - - Matrix H(Dim * numKeys, Dim * numKeys); - Vector gs_vector; - - H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); - - // Populate Gs and gs - int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera - DenseIndex i1D = i1 * Dim; - gs.at(i1) = gs_vector.segment(i1D); - for (DenseIndex i2 = 0; i2 < numKeys; i2++) { - if (i2 >= i1) { - Gs.at(GsCount2) = H.block(i1D, i2 * Dim); - GsCount2++; - } - } - } } /** @@ -500,55 +428,6 @@ public: } // end of for over cameras } - /** - * Do Schur complement, given Jacobian as F,E,P, return Gs/gs - * Fast version - works on with sparsity - */ - void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix3& 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(ZDim * i1, 0) * P; - - { // for i1 = i2 - // Dim = (Dx2) * (2) - gs.at(i1) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b - - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // Dim = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - Gs.at(GsIndex) = Fi1.transpose() - * (Fi1 - Ei1_P * E.block(ZDim * 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(ZDim * i2, 0).transpose() * Fi2); - GsIndex++; - } - } // end of for over cameras - } - /** * 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. diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 4ff53664d..a106bf382 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -746,7 +746,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler()); factor1->add(level_uv, c1, unit2); factor1->add(level_uv_right, c2, unit2); - Matrix expectedF, expectedE; + Matrix expectedE; Vector expectedb; CameraSet cameras; @@ -757,7 +757,8 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { Point3 point; if (factor1->point()) point = *(factor1->point()); - factor1->computeJacobians(expectedF, expectedE, expectedb, cameras, point); + vector Fblocks; + factor1->computeJacobians(Fblocks, expectedE, expectedb, cameras, point); NonlinearFactorGraph generalGraph; SFMFactor sfm1(level_uv, unit2, c1, l1); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 551ad078c..1cb026040 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -406,9 +406,11 @@ public: } // ================================================================== + std::vector Fblocks; Matrix F, E; Vector b; - double f = computeJacobians(F, E, b, cameras); + double f = computeJacobians(Fblocks, E, b, cameras); + Base::FillDiagonalF(Fblocks,F); // expensive !!! // Schur complement trick // Frank says: should be possible to do this more efficiently? @@ -591,12 +593,6 @@ public: return true; } - /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Vector& b, - const Cameras& cameras) const { - return Base::computeJacobians(F, E, b, cameras, point_); - } - /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionErrorAfterTriangulation(const Values& values) const { Cameras cameras;