From 7c22c2c4027fe552831b295955011ddd9d9936e1 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Fri, 27 Nov 2020 16:18:36 -0500
Subject: [PATCH] simplified small test to make it more understandable

---
 tests/smallExample.h       | 17 +++++++++--------
 tests/testGncOptimizer.cpp |  6 +++---
 2 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/tests/smallExample.h b/tests/smallExample.h
index 271fb0581..70cda1eb0 100644
--- a/tests/smallExample.h
+++ b/tests/smallExample.h
@@ -369,8 +369,9 @@ inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() {
   boost::shared_ptr<NonlinearFactorGraph> fg(new NonlinearFactorGraph);
   Point2 z(0.0, 0.0);
   double sigma = 0.1;
-  boost::shared_ptr<smallOptimize::UnaryFactor> factor(
-      new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
+
+  boost::shared_ptr<PriorFactor<Point2>> factor(
+      new PriorFactor<Point2>(X(1), z, noiseModel::Isotropic::Sigma(2,sigma)));
   // 3 noiseless inliers
   fg->push_back(factor);
   fg->push_back(factor);
@@ -378,8 +379,8 @@ inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() {
 
   // 1 outlier
   Point2 z_out(1.0, 0.0);
-  boost::shared_ptr<smallOptimize::UnaryFactor> factor_out(
-      new smallOptimize::UnaryFactor(z_out, noiseModel::Isotropic::Sigma(2,sigma), X(1)));
+  boost::shared_ptr<PriorFactor<Point2>> factor_out(
+      new PriorFactor<Point2>(X(1), z_out, noiseModel::Isotropic::Sigma(2,sigma)));
   fg->push_back(factor_out);
 
   return *fg;
@@ -393,8 +394,8 @@ inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() {
   double sigma = 0.1;
   auto gmNoise = noiseModel::Robust::Create(
             noiseModel::mEstimator::GemanMcClure::Create(1.0), noiseModel::Isotropic::Sigma(2,sigma));
-  boost::shared_ptr<smallOptimize::UnaryFactor> factor(
-      new smallOptimize::UnaryFactor(z, gmNoise, X(1)));
+  boost::shared_ptr<PriorFactor<Point2>> factor(
+      new PriorFactor<Point2>(X(1), z, gmNoise));
   // 3 noiseless inliers
   fg->push_back(factor);
   fg->push_back(factor);
@@ -402,8 +403,8 @@ inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() {
 
   // 1 outlier
   Point2 z_out(1.0, 0.0);
-  boost::shared_ptr<smallOptimize::UnaryFactor> factor_out(
-      new smallOptimize::UnaryFactor(z_out, gmNoise, X(1)));
+  boost::shared_ptr<PriorFactor<Point2>> factor_out(
+      new PriorFactor<Point2>(X(1), z_out, gmNoise));
   fg->push_back(factor_out);
 
   return *fg;
diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp
index bfea5977a..d770f58a8 100644
--- a/tests/testGncOptimizer.cpp
+++ b/tests/testGncOptimizer.cpp
@@ -367,20 +367,20 @@ TEST(GncOptimizer, optimize) {
   GaussNewtonOptimizer gn(fg, initial, gnParams);
   Values gn_results = gn.optimize();
   // converges to incorrect point due to lack of robustness to an outlier, ideal solution is Point2(0,0)
-  CHECK(assert_equal(gn_results.at<Point2>(X(1)), Point2(1.31812,0.0), 1e-3));
+  CHECK(assert_equal(Point2(0.25,0.0), gn_results.at<Point2>(X(1)), 1e-3));
 
   // try with robust loss function and standard GN
   auto fg_robust = example::sharedRobustFactorGraphWithOutliers(); // same as fg, but with factors wrapped in Geman McClure losses
   GaussNewtonOptimizer gn2(fg_robust, initial, gnParams);
   Values gn2_results = gn2.optimize();
   // converges to incorrect point, this time due to the nonconvexity of the loss
-  CHECK(assert_equal(gn2_results.at<Point2>(X(1)), Point2(1.18712,0.0), 1e-3));
+  CHECK(assert_equal(Point2(0.999706,0.0), gn2_results.at<Point2>(X(1)), 1e-3));
 
   // .. but graduated nonconvexity ensures both robustness and convergence in the face of nonconvexity
   GncParams<GaussNewtonParams> gncParams(gnParams);
   auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, gncParams);
   Values gnc_result = gnc.optimize();
-  CHECK(assert_equal(gnc_result.at<Point2>(X(1)), Point2(0.0,0.0), 1e-3));
+  CHECK(assert_equal(Point2(0.0,0.0), gnc_result.at<Point2>(X(1)), 1e-3));
 }
 
 /* ************************************************************************* */