diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 8eb34d10c..679ea5879 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -65,6 +65,11 @@ HessianFactor::HessianFactor(const HessianFactor& gf) : /* ************************************************************************* */ HessianFactor::HessianFactor() : info_(matrix_) { + // The empty HessianFactor has only a constant error term of zero + FastVector dims; + dims.push_back(1); + info_.resize(dims.begin(), dims.end(), false); + info_(0,0)(0,0) = 0.0; assertInvariants(); } diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 3544f938a..aefdeef99 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -38,7 +38,10 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(HessianFactor, emptyConstructor) { HessianFactor f; -// f.print(); + DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero + EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty + EXPECT(assert_equal(zeros(1,1), f.info())); // Full matrix should be 1-by-1 zero matrix + DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error } /* ************************************************************************* */