Merge pull request #816 from ScottMcMichael/fix/isam2-serialize

Fix serialization of ISAM2 class
borglab · Jul 19, 2021 · 4cd3eae · 4cd3eae
2 parents fda6596 + 1d75a07
commit 4cd3eae
Show file tree

Hide file tree

Showing 3 changed files with 110 additions and 0 deletions.
diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h
@@ -182,6 +182,16 @@ class GTSAM_EXPORT VariableIndex {
     return item->second; 
   }
 
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(index_);
+    ar & BOOST_SERIALIZATION_NVP(nFactors_);
+    ar & BOOST_SERIALIZATION_NVP(nEntries_);
+  }
+
   /// @}
 };
 

diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
@@ -315,6 +315,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
   void removeVariables(const KeySet& unusedKeys);
 
   void updateDelta(bool forceFullSolve = false) const;
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
+      ar & BOOST_SERIALIZATION_NVP(theta_);
+      ar & BOOST_SERIALIZATION_NVP(variableIndex_);
+      ar & BOOST_SERIALIZATION_NVP(delta_);
+      ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
+      ar & BOOST_SERIALIZATION_NVP(RgProd_);
+      ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
+      ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
+      ar & BOOST_SERIALIZATION_NVP(linearFactors_);
+      ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
+      ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
+      ar & BOOST_SERIALIZATION_NVP(update_count_);
+  }
+
 };  // ISAM2
 
 /// traits

diff --git 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,61 @@ 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<gtsam::FactorIndex>());
+
+  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); }
 /* ************************************************************************* */