diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 734c3fb72..9c57ea5ea 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -210,12 +210,12 @@ namespace gtsam { /* ************************************************************************* */ double HessianFactor::error(const VectorValues& c) const { - return ublas::inner_prod(c.vector(), + return 0.5 * (ublas::inner_prod(c.vector(), ublas::prod( ublas::symmetric_adaptor(info_.range(0, this->size(), 0, this->size())), c.vector())) - 2.0*ublas::inner_prod(c.vector(), info_.rangeColumn(0, this->size(), this->size(), 0)) + - info_(this->size(), this->size())(0,0); + info_(this->size(), this->size())(0,0)); } void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) { diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 2906a231a..c1ca9616d 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -47,7 +47,7 @@ namespace gtsam { typedef MatrixColMajor InfoMatrix; typedef SymmetricBlockView BlockInfo; - InfoMatrix matrix_; // The full information matrix, s.t. the quadratic error is [x -1]'*H*[x -1] + InfoMatrix matrix_; // The full information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] BlockInfo info_; // The block view of the full information matrix. void updateATA(const JacobianFactor& update, const Scatter& scatter); @@ -68,14 +68,14 @@ namespace gtsam { /** Construct a unary factor. G is the quadratic term (Hessian matrix), g * the linear term (a vector), and f the constant term. The quadratic * error is: - * f - 2*x'*g + x'*G*x + * 0.5*(f - 2*x'*g + x'*G*x) */ HessianFactor(Index j, const Matrix& G, const Vector& g, double f); /** 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: - * f - 2*x1'*g1 - 2*x2'*g2 + x1'*G11*x1 + x1'*G12*x2 + * 0.5*(f - 2*x1'*g1 - 2*x2'*g2 + x1'*G11*x1 + x1'*G12*x2) */ HessianFactor(Index j1, Index j2, const Matrix& G11, const Matrix& G12, const Vector& g1, @@ -98,7 +98,7 @@ namespace gtsam { virtual void print(const std::string& s = "") const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; - virtual double error(const VectorValues& c) const; /** [x -1]'*H*[x -1] (also see constructor documentation) */ + virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */ /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables? diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 64ae7b845..54736b974 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -31,7 +31,6 @@ using namespace gtsam; using namespace std; /* ************************************************************************* */ -#ifdef BROKEN // because accesses keys_, now private TEST(HessianFactor, ConversionConstructor) { HessianFactor expected; @@ -84,9 +83,13 @@ TEST(HessianFactor, ConversionConstructor) { HessianFactor actual(combined); + VectorValues values(std::vector(dims, dims+2)); + values[0] = Vector_(2, 1.0, 2.0); + values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.0); + + DOUBLES_EQUAL(combined.error(values), actual.error(values), 1e-9); EXPECT(assert_equal(expected, actual, 1e-9)); } -#endif /* ************************************************************************* */ TEST(HessianFactor, Constructor1) @@ -105,7 +108,7 @@ TEST(HessianFactor, Constructor1) HessianFactor factor(0, G, g, f); - double expected = 160.75; + double expected = 80.375; double actual = factor.error(dx); DOUBLES_EQUAL(expected, actual, 1e-10); @@ -134,7 +137,7 @@ TEST(HessianFactor, Constructor2) HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); - double expected = 181.0; + double expected = 90.5; double actual = factor.error(dx); DOUBLES_EQUAL(expected, actual, 1e-10);