diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 24033678d..88fb4b6e1 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -18,7 +18,7 @@ namespace gtsam { /** * RegularImplicitSchurFactor */ -template // +template // class RegularImplicitSchurFactor: public GaussianFactor { public: @@ -170,8 +170,12 @@ public: static void updateSparseSchurComplement( const std::vector& Fblocks, const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, - const FastVector& keys, const FastMap& KeySlotMap, + const FastVector& allKeys, const FastVector& keys, /*output ->*/SymmetricBlockMatrix& augmentedHessian) { + + FastMap KeySlotMap; + for (size_t slot = 0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); // Schur complement trick // G = F' * F - F' * E * P * E' * F // g = F' * (b - E * P * E' * b) @@ -232,15 +236,28 @@ public: augmentedHessian(aug_m, aug_m)(0, 0) += f; } + /// *Compute* full augmented information matrix virtual Matrix augmentedInformation() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::augmentedInformation non implemented"); - return Matrix(); + + // Create a SymmetricBlockMatrix + int m = this->keys_.size(); + size_t M1 = D * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, D); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + + // Do the Schur complement + sparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian); + return augmentedHessian.matrix(); } + + /// *Compute* full information matrix virtual Matrix information() const { - throw std::runtime_error( - "RegularImplicitSchurFactor::information non implemented"); - return Matrix(); + Matrix augmented = augmentedInformation(); + int m = this->keys_.size(); + size_t M = D * m; + return augmented.block(0,0,M,M); } /// Return the diagonal of the Hessian for this factor diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 8571a345d..3575a0286 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -255,6 +255,18 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0])); EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1])); EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); + + // augmentedInformation (test just checks diagonals) + Matrix actualInfo = factor.augmentedInformation(); + EXPECT(assert_equal(actualBD[0],actualInfo.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo.block<6,6>(12,12))); + + // information (test just checks diagonals) + Matrix actualInfo2 = factor.information(); + EXPECT(assert_equal(actualBD[0],actualInfo2.block<6,6>(0,0))); + EXPECT(assert_equal(actualBD[1],actualInfo2.block<6,6>(6,6))); + EXPECT(assert_equal(actualBD[3],actualInfo2.block<6,6>(12,12))); } /* ************************************************************************* */