From 8c68e215215f640a40cbf4b2d863fea64124f450 Mon Sep 17 00:00:00 2001
From: Scott <scott.t.mcmichael@nasa.gov>
Date: Fri, 9 Jul 2021 18:30:39 -0700
Subject: [PATCH] Added ISAM2 serialize test

---
 .../tests/testSerializationNonlinear.cpp      | 79 +++++++++++++++++++
 1 file changed, 79 insertions(+)

diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
index 90ebcbbba..037582654 100644
--- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
+++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp
@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
@@ -31,6 +32,30 @@ using namespace std;
 using namespace gtsam;
 using namespace gtsam::serializationTestHelpers;
 
+
+/* ************************************************************************* */
+// Create GUIDs for Noisemodels
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic");
+BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
+BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
+
+/* ************************************************************************* */
+// Create GUIDs for factors
+BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>");
+BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
+BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional");
+
+
 /* ************************************************************************* */
 // Export all classes derived from Value
 GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
@@ -82,6 +107,60 @@ TEST (Serialization, TemplatedValues) {
   EXPECT(equalsBinary(values));
 }
 
+TEST(Serialization, ISAM2) {
+
+  gtsam::ISAM2Params parameters;
+  gtsam::ISAM2 solver(parameters);
+  gtsam::NonlinearFactorGraph graph;
+  gtsam::Values initialValues;
+  initialValues.clear();
+
+  gtsam::Vector6 temp6;
+  for (int i = 0; i < 6; ++i) {
+    temp6[i] = 0.0001;
+  }
+  gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6);
+
+  gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0));
+  gtsam::Symbol symbol0('x', 0);
+  graph.add(gtsam::PriorFactor<gtsam::Pose3>(symbol0, pose0, noiseModel));
+  initialValues.insert(symbol0, pose0);
+
+  solver.update(graph, initialValues, gtsam::FastVector<size_t>());
+
+  std::string binaryPath = "saved_solver.dat";
+  try {
+    std::ofstream outputStream(binaryPath);
+    boost::archive::binary_oarchive outputArchive(outputStream);
+    outputArchive << solver;
+  } catch(...) {
+    EXPECT(false);
+  }
+
+  gtsam::ISAM2 solverFromDisk;
+  try {
+    std::ifstream ifs(binaryPath);
+    boost::archive::binary_iarchive inputArchive(ifs);
+    inputArchive >> solverFromDisk;
+  } catch(...) {
+    EXPECT(false);
+  }
+
+  gtsam::Pose3 p1, p2;
+  try {
+    p1 = solver.calculateEstimate<gtsam::Pose3>(symbol0);
+  } catch(std::exception &e) {
+    EXPECT(false);
+  }
+
+  try {
+    p2 = solverFromDisk.calculateEstimate<gtsam::Pose3>(symbol0);
+  } catch(std::exception &e) {
+    EXPECT(false);
+  }
+  EXPECT(assert_equal(p1, p2));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */