From aa7eb6306e996abc9405eb92f72486f6b5fb255b Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Sat, 24 Mar 2012 20:49:35 +0000
Subject: [PATCH] Unit tests for camera range factors

---
 gtsam/slam/tests/testGeneralSFMFactor.cpp | 50 +++++++++++++++++++++++
 gtsam/slam/tests/testVSLAM.cpp            |  8 ++--
 2 files changed, 54 insertions(+), 4 deletions(-)

diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp
index d24624d21..b4fb6e2af 100644
--- a/gtsam/slam/tests/testGeneralSFMFactor.cpp
+++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp
@@ -21,6 +21,7 @@ using namespace boost;
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/RangeFactor.h>
+#include <gtsam/slam/PriorFactor.h>
 
 using namespace std;
 using namespace gtsam;
@@ -359,6 +360,55 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
   EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
 }
 
+/* ************************************************************************* */
+TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
+  // Tests range factor between a GeneralCamera and a Pose3
+  Graph graph;
+  graph.addCameraConstraint(0, GeneralCamera());
+  graph.add(RangeFactor<GeneralCamera, Pose3>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0)));
+  graph.add(PriorFactor<Pose3>(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0)));
+
+  Values init;
+  init.insert(Symbol('x',0), GeneralCamera());
+  init.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
+
+  // The optimal value between the 2m range factor and 1m prior is 1.5m
+  Values expected;
+  expected.insert(Symbol('x',0), GeneralCamera());
+  expected.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.5,0.0,0.0)));
+
+  LevenbergMarquardtParams params;
+  params.absoluteErrorTol = 1e-9;
+  params.relativeErrorTol = 1e-9;
+  Values actual = *LevenbergMarquardtOptimizer(graph, init, params).optimized();
+
+  EXPECT(assert_equal(expected, actual, 1e-4));
+}
+
+/* ************************************************************************* */
+TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
+  // Tests range factor between a CalibratedCamera and a Pose3
+  NonlinearFactorGraph graph;
+  graph.add(PriorFactor<CalibratedCamera>(Symbol('x',0), CalibratedCamera(), sharedSigma(6, 1.0)));
+  graph.add(RangeFactor<CalibratedCamera, Pose3>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0)));
+  graph.add(PriorFactor<Pose3>(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0)));
+
+  Values init;
+  init.insert(Symbol('x',0), CalibratedCamera());
+  init.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
+
+  Values expected;
+  expected.insert(Symbol('x',0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
+  expected.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.333333333333, 0, 0)));
+
+  LevenbergMarquardtParams params;
+  params.absoluteErrorTol = 1e-9;
+  params.relativeErrorTol = 1e-9;
+  Values actual = *LevenbergMarquardtOptimizer(graph, init, params).optimized();
+
+  EXPECT(assert_equal(expected, actual, 1e-4));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp
index a5eba9715..5f65b757d 100644
--- a/gtsam/slam/tests/testVSLAM.cpp
+++ b/gtsam/slam/tests/testVSLAM.cpp
@@ -23,7 +23,7 @@
 using namespace boost;
 
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/visualSLAM.h>
 
 using namespace std;
@@ -102,7 +102,7 @@ TEST( Graph, optimizeLM)
 
   // Create an optimizer and check its error
   // We expect the initial to be zero because config is the ground truth
-  NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, ordering));
+  NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate, ordering));
   DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
 
   // Iterate once, and the config should not have changed because we started
@@ -139,7 +139,7 @@ TEST( Graph, optimizeLM2)
 
   // Create an optimizer and check its error
   // We expect the initial to be zero because config is the ground truth
-  NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, ordering));
+  NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate, ordering));
   DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
 
   // Iterate once, and the config should not have changed because we started
@@ -172,7 +172,7 @@ TEST( Graph, CHECK_ORDERING)
 
   // Create an optimizer and check its error
   // We expect the initial to be zero because config is the ground truth
-  NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate));
+  NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate));
   DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
 
   // Iterate once, and the config should not have changed because we started