From 779a21c2c40e7176a7b1a67dc1a5d2e19ef3bfa1 Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Wed, 6 Aug 2014 08:49:28 -0400
Subject: [PATCH] A small example showing that Hessian matrices in Lie groups
 might not be symmetric

---
 gtsam/base/tests/testNumericalDerivative.cpp | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp
index f7e4d3baa..6745d9078 100644
--- a/gtsam/base/tests/testNumericalDerivative.cpp
+++ b/gtsam/base/tests/testNumericalDerivative.cpp
@@ -124,6 +124,23 @@ TEST(testNumericalDerivative, numericalHessian311) {
   EXPECT(assert_equal(expected33, actual33, 1e-5));
 }
 
+/* ************************************************************************* */
+/// A small example showing that Hessian matrices in Lie group might not be symmetric!!!
+
+#include <gtsam/geometry/Pose2.h>
+double f5(const Pose2& x) {
+  Point2 p(5, 10);
+  Point2 q = x.transform_from(p);
+  return q.x();
+}
+
+TEST(testNumericalDerivative, numericalHessianPose2) {
+  Pose2 x0(Rot2(20.0*M_PI/180.0), Point2(20, 25));
+  Matrix actualH = numericalHessian<Pose2>(f5, x0);
+  Matrix expectedH = (Matrix(3,3) << 0, 0, -0.34202, 0, 0, -0.939693, 0, 0, -1.27827);
+  EXPECT(assert_equal(expectedH, actualH, 1e-5));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */