From 3975d0a6420dba73a4a7123f406c86a264acf934 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sat, 16 Jan 2016 18:33:56 -0800
Subject: [PATCH] New tests

---
 gtsam/geometry/tests/testPoint3.cpp | 11 +++++++++++
 gtsam/geometry/tests/testRot3.cpp   | 26 +++++++++++++++++---------
 2 files changed, 28 insertions(+), 9 deletions(-)

diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
index 6811ed122..246d23c9a 100644
--- a/gtsam/geometry/tests/testPoint3.cpp
+++ b/gtsam/geometry/tests/testPoint3.cpp
@@ -133,6 +133,17 @@ TEST (Point3, distance) {
   EXPECT(assert_equal(numH2, H2, 1e-8));
 }
 
+/* ************************************************************************* */
+TEST(Point3, cross) {
+  Matrix aH1, aH2;
+  boost::function<Point3(const Point3&, const Point3&)> f =
+      boost::bind(&Point3::cross, _1, _2, boost::none, boost::none);
+  const Point3 omega(0, 1, 0), theta(4, 6, 8);
+  omega.cross(theta, aH1, aH2);
+  EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
+  EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index eb6573c30..59b365b2a 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -270,19 +270,27 @@ TEST( Rot3, ExpmapDerivative) {
 /* ************************************************************************* */
 TEST( Rot3, ExpmapDerivative2)
 {
-  const Vector3 thetahat(0.1, 0, 0.1);
+  const Vector3 theta(0.1, 0, 0.1);
   const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
+      boost::bind(&Rot3::Expmap, _1, boost::none), theta);
 
-  const Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
-  CHECK(assert_equal(Jexpected, Jactual));
-
-  const Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
-  CHECK(assert_equal(Jexpected, Jactual2));
+  CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
+  CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
 }
 
 /* ************************************************************************* */
-TEST(Rot3, ExpmapDerivative3) {
+TEST( Rot3, ExpmapDerivative3)
+{
+  const Vector3 theta(10, 20, 30);
+  const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
+      boost::bind(&Rot3::Expmap, _1, boost::none), theta);
+
+  CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
+  CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
+}
+
+/* ************************************************************************* */
+TEST(Rot3, ExpmapDerivative4) {
   // Iserles05an (Lie-group Methods) says:
   // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
   // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
@@ -309,7 +317,7 @@ TEST(Rot3, ExpmapDerivative3) {
 }
 
 /* ************************************************************************* */
-TEST(Rot3, ExpmapDerivative4) {
+TEST(Rot3, ExpmapDerivative5) {
   auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
   auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };