diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index c6ce340e6..cf6b779af 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -72,8 +72,8 @@ HessianFactor::HessianFactor() : info_(matrix_) { } /* ************************************************************************* */ -HessianFactor::HessianFactor(Index j1, const Matrix& G, const Vector& g, double f) : - GaussianFactor(j1), info_(matrix_) { +HessianFactor::HessianFactor(Index j, const Matrix& G, const Vector& g, double f) : + GaussianFactor(j), info_(matrix_) { if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); size_t dims[] = { G.rows(), 1 }; @@ -86,6 +86,26 @@ HessianFactor::HessianFactor(Index j1, const Matrix& G, const Vector& g, double assertInvariants(); } +/* ************************************************************************* */ +// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) +// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g +HessianFactor::HessianFactor(Index j, const Vector& mu, const Matrix& Sigma) : + GaussianFactor(j), info_(matrix_) { + if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + Matrix G = inverse(Sigma); + Vector g = G * mu; + double f = dot(mu, g); + size_t dims[] = { G.rows(), 1 }; + InfoMatrix fullMatrix(G.rows() + 1, G.rows() + 1); + BlockInfo infoMatrix(fullMatrix, dims, dims + 2); + infoMatrix(0, 0) = G; + infoMatrix.column(0, 1, 0) = g; + infoMatrix(1, 1)(0, 0) = f; + infoMatrix.swap(info_); + assertInvariants(); +} + /* ************************************************************************* */ HessianFactor::HessianFactor(Index j1, Index j2, const Matrix& G11, const Matrix& G12, const Vector& g1, diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index ec9fc2910..e613992ac 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -25,6 +25,7 @@ // Forward declarations for friend unit tests class ConversionConstructorHessianFactorTest; class Constructor1HessianFactorTest; +class Constructor1bHessianFactorTest; class combineHessianFactorTest; @@ -78,6 +79,11 @@ namespace gtsam { */ HessianFactor(Index j, const Matrix& G, const Vector& g, double f); + /** Construct a unary factor, given a mean and covariance matrix. + * error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) + */ + HessianFactor(Index j, const Vector& mu, const Matrix& Sigma); + /** Construct a binary factor. Gxx are the upper-triangle blocks of the * quadratic term (the Hessian matrix), gx the pieces of the linear vector * term, and f the constant term. The quadratic error is: @@ -133,6 +139,7 @@ namespace gtsam { // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; friend class ::Constructor1HessianFactorTest; + friend class ::Constructor1bHessianFactorTest; friend class ::combineHessianFactorTest; // Friend JacobianFactor for conversion diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 43165f6ac..a4cf88487 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -134,6 +134,26 @@ TEST(HessianFactor, Constructor1) DOUBLES_EQUAL(expected, actual, 1e-10); } +/* ************************************************************************* */ +TEST(HessianFactor, Constructor1b) +{ + Vector mu = Vector_(2,1.0,2.0); + Matrix Sigma = eye(2,2); + + HessianFactor factor(0, mu, Sigma); + + Matrix G = eye(2,2); + Vector g = G*mu; + double f = dot(g,mu); + + // Check + Matrix info_matrix = factor.info_.range(0, 1, 0, 1); + EXPECT(assert_equal(Matrix(G), info_matrix)); + EXPECT_DOUBLES_EQUAL(f, factor.constant_term(), 1e-10); + EXPECT(assert_equal(g, Vector(factor.linear_term()), 1e-10)); + EXPECT_LONGS_EQUAL(1, factor.size()); +} + /* ************************************************************************* */ TEST(HessianFactor, Constructor2) {