From a272264e7d909e2ce35338ba21a9e5f222efb2bf Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 14 Feb 2014 01:38:08 -0500
Subject: [PATCH] Added unit test for hessianDiagonal

---
 .cproject                                          | 4 ++--
 gtsam/linear/tests/testJacobianFactorUnordered.cpp | 8 ++++++++
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/.cproject b/.cproject
index 95f915001..88abf2f1e 100644
--- a/.cproject
+++ b/.cproject
@@ -2164,10 +2164,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testJacobianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testJacobianFactorUnordered.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>testJacobianFactor.run</buildTarget>
+				<buildTarget>testJacobianFactorUnordered.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactorUnordered.cpp
index 72afa3ae9..a812e1768 100644
--- a/gtsam/linear/tests/testJacobianFactorUnordered.cpp
+++ b/gtsam/linear/tests/testJacobianFactorUnordered.cpp
@@ -232,6 +232,14 @@ TEST(JacobianFactor, matrices)
   EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first));
   EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
   EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));
+
+  // hessianDiagonal
+  VectorValues expectDiagonal;
+  // below we divide by the variance 0.5^2
+  expectDiagonal.insert(5, (Vector(3) << 1, 1, 1)/0.25);
+  expectDiagonal.insert(10, (Vector(3) << 4, 4, 4)/0.25);
+  expectDiagonal.insert(15, (Vector(3) << 9, 9, 9)/0.25);
+  EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal()));
 }
 
 /* ************************************************************************* */