From ab0fb0d4a50dc43f212a32efe54c6ee4242f7a47 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 18 Dec 2018 13:56:38 -0500
Subject: [PATCH] Added iSAM2 test to check issue #412

---
 tests/testVisualISAM2.cpp | 125 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 125 insertions(+)
 create mode 100644 tests/testVisualISAM2.cpp

diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp
new file mode 100644
index 000000000..cd04c4b31
--- /dev/null
+++ b/tests/testVisualISAM2.cpp
@@ -0,0 +1,125 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testVisualISAM2.cpp
+ * @brief   Test convergence of visualSLAM example.
+ * @author  Duy-Nguyen Ta
+ * @author  Frank Dellaert
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <examples/SFMdata.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(testVisualISAM2, all)
+{
+    Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+
+    auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+
+    vector<Point3> points = createPoints();
+
+    // Create the set of ground-truth poses
+    vector<Pose3> poses = createPoses();
+
+    // Set the parameters
+    ISAM2Params parameters;
+    parameters.relinearizeThreshold = 0.01;
+    parameters.relinearizeSkip = 1;
+    ISAM2 isam(parameters);
+
+    // Create a Factor Graph and Values to hold the new data
+    NonlinearFactorGraph graph;
+    Values initialEstimate;
+
+    // Loop over the poses, adding the observations to iSAM incrementally
+    for (size_t i = 0; i < poses.size(); ++i)
+    {
+        // Add factors for each landmark observation
+        for (size_t j = 0; j < points.size(); ++j)
+        {
+            SimpleCamera camera(poses[i], *K);
+            Point2 measurement = camera.project(points[j]);
+            graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2>>(
+                measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
+        }
+
+        // Add an initial guess for the current pose
+        // Intentionally initialize the variables off from the ground truth
+        static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                Point3(0.05, -0.10, 0.20));
+        initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
+
+        // Treat first iteration as special case
+        if (i == 0)
+        {
+            // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
+            static auto kPosePrior = noiseModel::Diagonal::Sigmas(
+                (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
+                    .finished());
+            graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
+                                                     kPosePrior);
+
+            // Add a prior on landmark l0
+            static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
+            graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0],
+                                                      kPointPrior);
+
+            // Add initial guesses to all observed landmarks
+            // Intentionally initialize the variables off from the ground truth
+            static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
+            for (size_t j = 0; j < points.size(); ++j)
+                initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
+        }
+        else
+        {
+            // Update iSAM with the new factors
+            isam.update(graph, initialEstimate);
+
+            // Optimize
+            Values currentEstimate = isam.calculateEstimate();
+
+            // reset for next iteration
+            graph.resize(0);
+            initialEstimate.clear();
+        }
+    } // for loop
+
+    auto result = isam.calculateEstimate();
+    EXPECT_LONGS_EQUAL(16, result.size());
+    for (size_t j = 0; j < points.size(); ++j)
+    {
+        Symbol key('l', j);
+        EXPECT(assert_equal(points[j], result.at<Point3>(key), 0.01));
+    }
+}
+
+/* ************************************************************************* */
+int main()
+{
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */