From 0e309baab6218d522b8c0c811a29d2e094feb33e Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Tue, 28 May 2019 17:42:30 -0400
Subject: [PATCH 1/2] Make test with hard constraints

---
 tests/testDoglegOptimizer.cpp | 40 +++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp
index 954afa2203..0a33473473 100644
--- a/tests/testDoglegOptimizer.cpp
+++ b/tests/testDoglegOptimizer.cpp
@@ -18,7 +18,11 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <tests/smallExample.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/DoglegOptimizer.h>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianBayesTree.h>
@@ -147,6 +151,42 @@ TEST(DoglegOptimizer, Iterate) {
   }
 }
 
+/* ************************************************************************* */
+TEST(DoglegOptimizer, Constraint) {
+  // Create a pose-graph graph with a constraint on the first pose
+  NonlinearFactorGraph graph;
+  const Pose2 origin(0, 0, 0), pose2(2, 0, 0);
+  graph.emplace_shared<NonlinearEquality<Pose2> >(1, origin);
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, pose2, model);
+
+  // Create feasible initial estimate
+  Values initial;
+  initial.insert(1, origin); // feasible !
+  initial.insert(2, Pose2(2.3, 0.1, -0.2));
+
+  // Optimize the initial values using DoglegOptimizer
+  DoglegParams params;
+  params.setVerbosityDL("VERBOSITY");
+  DoglegOptimizer optimizer(graph, initial, params);
+  Values result = optimizer.optimize();
+
+  // Check result
+  EXPECT(assert_equal(pose2, result.at<Pose2>(2)));
+
+  // Create infeasible initial estimate
+  Values infeasible;
+  infeasible.insert(1, Pose2(0.1, 0, 0)); // infeasible !
+  infeasible.insert(2, Pose2(2.3, 0.1, -0.2));
+
+  // Try optimizing with infeasible initial estimate
+  DoglegOptimizer optimizer2(graph, infeasible, params);
+  Values result2 = optimizer2.optimize();
+
+  // Check result
+  EXPECT(assert_equal(pose2, result2.at<Pose2>(2)));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

From 184324a2dce329e7c6caa9ad3bb42f9037b3e40f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 30 May 2019 16:43:34 -0400
Subject: [PATCH 2/2] Check exception type

---
 tests/testDoglegOptimizer.cpp | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp
index 0a33473473..f1f847e593 100644
--- a/tests/testDoglegOptimizer.cpp
+++ b/tests/testDoglegOptimizer.cpp
@@ -181,10 +181,7 @@ TEST(DoglegOptimizer, Constraint) {
 
   // Try optimizing with infeasible initial estimate
   DoglegOptimizer optimizer2(graph, infeasible, params);
-  Values result2 = optimizer2.optimize();
-
-  // Check result
-  EXPECT(assert_equal(pose2, result2.at<Pose2>(2)));
+  CHECK_EXCEPTION(optimizer2.optimize(), std::invalid_argument);
 }
 
 /* ************************************************************************* */