From e52dca63f6807ddfdbbd4da34b6a3de19c9b8112 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sat, 11 Jul 2015 18:32:43 -0700
Subject: [PATCH] 3D version with Unit3 measurement now works

---
 gtsam/sam/tests/testBearingFactor.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp
index 7cdb405fa..93ad9db7f 100644
--- a/gtsam/sam/tests/testBearingFactor.cpp
+++ b/gtsam/sam/tests/testBearingFactor.cpp
@@ -20,6 +20,7 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/factorTesting.h>
+#include <gtsam/nonlinear/expressionTesting.h>
 #include <gtsam/base/serialization.h>
 #include <gtsam/base/serializationTestHelpers.h>
 
@@ -37,7 +38,7 @@ static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
 BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
 
 typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
-Unit3 measurement3D(1,2,3);
+Unit3 measurement3D = Pose3().bearing(Point3(1,0,0)); // has to match values!
 static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
 BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
 
@@ -45,9 +46,6 @@ BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
 // Export Noisemodels
 // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
 BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
-//BOOST_CLASS_EXPORT(BearingFactor2D);
-//BOOST_CLASS_EXPORT(ExpressionFactor<Rot2>);
-//BOOST_CLASS_EXPORT_GUID(gtsam::ExpressionFactor<Rot2>, "ExpressionFactorRot2")
 
 /* ************************************************************************* */
 TEST(BearingFactor, Serialization2D) {
@@ -70,7 +68,8 @@ TEST(BearingFactor, 2D) {
   values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
   values.insert(pointKey, Point2(-4.0, 11.0));
 
-  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(), values, 1e-7,1e-5);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 }
 
 /* ************************************************************************* */
@@ -91,10 +90,11 @@ TEST(BearingFactor, 3D) {
 
   // Set the linearization point
   Values values;
-  values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0,-3.0)));
-  values.insert(pointKey, Point3(-2.0, 11.0, 1.0));
+  values.insert(poseKey, Pose3());
+  values.insert(pointKey, Point3(1,0,0));
 
-  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(), values, 1e-7,1e-5);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 }
 
 /* ************************************************************************* */