diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index bc14cc670..7f9c5ea3c 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -126,7 +126,7 @@ namespace gtsam { * @param scatter A mapping from variable index to slot index in this HessianFactor * @param info The information matrix to be updated */ - virtual void updateHessian(const Scatter& scatter, + virtual void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const = 0; /// y += alpha * A'*A*x @@ -141,6 +141,12 @@ namespace gtsam { /// Gradient wrt a key at any values virtual Vector gradient(Key key, const VectorValues& x) const = 0; + // Determine position of a given key + template + static DenseIndex Slot(const CONTAINER& keys, Key key) { + return std::find(keys.begin(), keys.end(), key) - keys.begin(); + } + private: /** Serialization function */ friend class boost::serialization::access; @@ -150,7 +156,7 @@ namespace gtsam { } }; // GaussianFactor - + /// traits template<> struct traits : public Testable { diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index c74d53476..c071f8daa 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, gttic(update); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) if(factor) - factor->updateHessian(*scatter, &info_); + factor->updateHessian(keys_, &info_); gttoc(update); } @@ -346,15 +346,15 @@ double HessianFactor::error(const VectorValues& c) const { } /* ************************************************************************* */ -void HessianFactor::updateHessian(const Scatter& scatter, +void HessianFactor::updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); // Apply updates to the upper triangle DenseIndex n = size(), N = info->nBlocks()-1; for (DenseIndex j = 0; j <= n; ++j) { - const DenseIndex J = j==n ? N : scatter.slot(keys_[j]); + const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]); for (DenseIndex i = 0; i <= j; ++i) { - const DenseIndex I = i==n ? N : scatter.slot(keys_[i]); + const DenseIndex I = (i==n) ? N : Slot(infoKeys, keys_[i]); (*info)(I, J) += info_(i, j); } } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 160d05b15..b74d557ea 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -344,7 +344,7 @@ namespace gtsam { * @param scatter A mapping from variable index to slot index in this HessianFactor * @param info The information matrix to be updated */ - void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const; + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 1e3433268..5b90d913d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -498,7 +498,7 @@ map JacobianFactor::hessianBlockDiagonal() const { } /* ************************************************************************* */ -void JacobianFactor::updateHessian(const Scatter& scatter, +void JacobianFactor::updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_JacobianFactor); @@ -512,7 +512,7 @@ void JacobianFactor::updateHessian(const Scatter& scatter, "JacobianFactor::updateHessian: cannot update information with " "constrained noise model"); JacobianFactor whitenedFactor = whiten(); - whitenedFactor.updateHessian(scatter, info); + whitenedFactor.updateHessian(infoKeys, info); } else { // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1; @@ -520,10 +520,10 @@ void JacobianFactor::updateHessian(const Scatter& scatter, // Apply updates to the upper triangle // Loop over blocks of A, including RHS with j==n for (DenseIndex j = 0; j <= n; ++j) { - const DenseIndex J = j==n ? N : scatter.slot(keys_[j]); + const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]); // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { - const DenseIndex I = scatter.slot(keys_[i]); + const DenseIndex I = Slot(infoKeys, keys_[i]); (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j); } // Fill diagonal block with Aj'*Aj diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 00a9b5488..ff7310d9c 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -278,7 +278,7 @@ namespace gtsam { * @param scatter A mapping from variable index to slot index in this HessianFactor * @param info The information matrix to be updated */ - void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const; + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; /** Return A*x */ Vector operator*(const VectorValues& x) const; diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 557ba3f36..96e61aa33 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -38,6 +38,16 @@ using namespace gtsam; const double tol = 1e-5; +/* ************************************************************************* */ +TEST(HessianFactor, Slot) +{ + FastVector keys = list_of(2)(4)(1); + EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2)); + EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4)); + EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1)); + EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist +} + /* ************************************************************************* */ TEST(HessianFactor, emptyConstructor) { @@ -302,15 +312,17 @@ TEST(HessianFactor, CombineAndEliminate) gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); + Matrix93 A0; A0 << A10, Z_3x3, Z_3x3; + Matrix93 A1; A1 << A11, A01, A21; Vector9 b; b << b1, b0, b2; Vector9 sigmas; sigmas << s1, s0, s2; // create a full, uneliminated version of the factor JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + // Make sure combining works + EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6)); + // perform elimination on jacobian GaussianConditional::shared_ptr expectedConditional; JacobianFactor::shared_ptr expectedRemainingFactor; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 4425d63d0..d969f08a2 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -149,7 +149,7 @@ namespace gtsam { : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {} // Fixed-size matrix update - void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { + void updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_LinearizedFactor); // Whiten the factor if it has a noise model @@ -160,15 +160,12 @@ namespace gtsam { "JacobianFactor::updateHessian: cannot update information with " "constrained noise model"); JacobianFactor whitenedFactor = whiten(); - whitenedFactor.updateHessian(scatter, info); + whitenedFactor.updateHessian(infoKeys, info); } else { - // N is number of variables in information matrix - DenseIndex N = info->nBlocks() - 1; - // First build an array of slots - DenseIndex slotC = scatter.slot(keys_.front()); - DenseIndex slotL = scatter.slot(keys_.back()); - DenseIndex slotB = N; + DenseIndex slotC = Slot(infoKeys, keys_.front()); + DenseIndex slotL = Slot(infoKeys, keys_.back()); + DenseIndex slotB = info->nBlocks() - 1; // We perform I += A'*A to the upper triangle (*info)(slotC, slotC).knownOffDiagonal() += AC_.transpose() * AC_; diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 87d78911d..71944c670 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -115,7 +115,7 @@ public: return D; } - virtual void updateHessian(const Scatter& scatter, + virtual void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const { throw std::runtime_error( "RegularImplicitSchurFactor::updateHessian non implemented");