From 227694d572990d3f25bce6cd45c6396e19580e0d Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <vlad.usenko@tum.de>
Date: Wed, 7 Aug 2019 16:54:35 +0200
Subject: [PATCH] added restore and backup functions

---
 include/basalt/vi_estimator/ba_base.h           | 12 ++++++++++++
 include/basalt/vi_estimator/landmark_database.h |  8 ++++++++
 src/vi_estimator/keypoint_vio.cpp               |  3 +++
 3 files changed, 23 insertions(+)

diff --git a/include/basalt/vi_estimator/ba_base.h b/include/basalt/vi_estimator/ba_base.h
index 19834eb..1c98ea6 100644
--- a/include/basalt/vi_estimator/ba_base.h
+++ b/include/basalt/vi_estimator/ba_base.h
@@ -368,6 +368,18 @@ class BundleAdjustmentBase {
     AccumT accum;
   };
 
+  inline void backup() {
+    for (auto& kv : frame_states) kv.second.backup();
+    for (auto& kv : frame_poses) kv.second.backup();
+    lmdb.backup();
+  }
+
+  inline void restore() {
+    for (auto& kv : frame_states) kv.second.restore();
+    for (auto& kv : frame_poses) kv.second.backup();
+    lmdb.restore();
+  }
+
   // protected:
   PoseStateWithLin getPoseStateWithLin(int64_t t_ns) const {
     auto it = frame_poses.find(t_ns);
diff --git a/include/basalt/vi_estimator/landmark_database.h b/include/basalt/vi_estimator/landmark_database.h
index f506b8c..4ffe783 100644
--- a/include/basalt/vi_estimator/landmark_database.h
+++ b/include/basalt/vi_estimator/landmark_database.h
@@ -106,6 +106,14 @@ class LandmarkDatabase {
 
   void removeObservations(int lm_id, const std::set<TimeCamId>& obs);
 
+  inline void backup() {
+    for (auto& kv : kpts) kv.second.backup();
+  }
+
+  inline void restore() {
+    for (auto& kv : kpts) kv.second.restore();
+  }
+
  private:
   Eigen::unordered_map<int, KeypointPosition> kpts;
   Eigen::map<TimeCamId,
diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp
index f413019..29e181b 100644
--- a/src/vi_estimator/keypoint_vio.cpp
+++ b/src/vi_estimator/keypoint_vio.cpp
@@ -971,6 +971,9 @@ void KeypointVioEstimator::optimize() {
       };
       tbb::parallel_for(keys_range, update_points_func);
 
+      // backup();
+      // restore();
+
       if (config.vio_debug) {
         double after_update_marg_prior_error = 0;
         double after_update_vision_error = 0, after_update_imu_error = 0,