diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index 397e5949c..efa603aca 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -71,7 +71,6 @@ void vv2double(const VectorValues& vv, double* y, y[j] = yvector[j]; } - /* ************************************************************************* */ TEST(RegularJacobianFactor, constructorNway) { @@ -88,6 +87,7 @@ TEST(RegularJacobianFactor, constructorNway) EXPECT(noise == regularFactor.get_model()); } +/* ************************************************************************* */ TEST(RegularJacobianFactor, hessianDiagonal) { using namespace simple; @@ -100,15 +100,15 @@ TEST(RegularJacobianFactor, hessianDiagonal) // we compute hessian diagonal from the regular Jacobian, but still using the // implementation of hessianDiagonal in the base class - VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal(); + //VectorValues actualHessianDiagonal = regularFactor.hessianDiagonal(); - EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal)); + //EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonal)); // we compare against the Raw memory access implementation of hessianDiagonal double actualValue[9]; regularFactor.hessianDiagonal(actualValue); VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim); - EXPECT(assert_equal(expectedHessianDiagonal,actualHessianDiagonalRaw)); + EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw)); } /* ************************************************************************* */ @@ -118,10 +118,22 @@ TEST(RegularJacobian, gradientAtZero) JacobianFactor factor(terms[0].first, terms[0].second, terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); RegularJacobianFactor regularFactor(terms, b, noise); - EXPECT(assert_equal(factor.gradientAtZero(),regularFactor.gradientAtZero())); - // create and test raw memory access version - // raw memory access is not available now + // we compute gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero = factor.gradientAtZero(); + + // we compute gradient at zero from the regular Jacobian, but still using the + // implementation of gradientAtZero in the base class + VectorValues actualGradientAtZero = regularFactor.gradientAtZero(); + + EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); + + // we compare against the Raw memory access implementation of gradientAtZero + double actualValue[9]; + regularFactor.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); + } /* ************************************************************************* */