From 29e1a76407fbee1c5041b70e038da4193b5a11fc Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <vlad.usenko@tum.de>
Date: Wed, 7 Aug 2019 16:45:25 +0200
Subject: [PATCH] added backup funtions to types

---
 include/basalt/utils/imu_types.h              | 33 +++++++++++++++++--
 .../basalt/vi_estimator/landmark_database.h   | 14 ++++++++
 thirdparty/basalt-headers                     |  2 +-
 3 files changed, 46 insertions(+), 3 deletions(-)

diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h
index a8b3c87..386cbb0 100644
--- a/include/basalt/utils/imu_types.h
+++ b/include/basalt/utils/imu_types.h
@@ -116,13 +116,27 @@ struct PoseVelBiasStateWithLin {
 
   friend struct PoseStateWithLin;
 
+  inline void backup() {
+    backup_delta = delta;
+    backup_state_linearized = state_linearized;
+    backup_state_current = state_current;
+  }
+
+  inline void restore() {
+    delta = backup_delta;
+    state_linearized = backup_state_linearized;
+    state_current = backup_state_current;
+  }
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  private:
   bool linearized;
   VecN delta;
-
   PoseVelBiasState state_linearized, state_current;
 
+  VecN backup_delta;
+  PoseVelBiasState backup_state_linearized, backup_state_current;
+
   friend class cereal::access;
 
   template <class Archive>
@@ -190,14 +204,29 @@ struct PoseStateWithLin {
   inline const VecN& getDelta() const { return delta; }
   inline int64_t getT_ns() const { return pose_linearized.t_ns; }
 
+  inline void backup() {
+    backup_delta = delta;
+    backup_pose_linearized = pose_linearized;
+    backup_T_w_i_current = T_w_i_current;
+  }
+
+  inline void restore() {
+    delta = backup_delta;
+    pose_linearized = backup_pose_linearized;
+    T_w_i_current = backup_T_w_i_current;
+  }
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  private:
   bool linearized;
   VecN delta;
-
   PoseState pose_linearized;
   Sophus::SE3d T_w_i_current;
 
+  VecN backup_delta;
+  PoseState backup_pose_linearized;
+  Sophus::SE3d backup_T_w_i_current;
+
   friend class cereal::access;
 
   template <class Archive>
diff --git a/include/basalt/vi_estimator/landmark_database.h b/include/basalt/vi_estimator/landmark_database.h
index 40543af..f506b8c 100644
--- a/include/basalt/vi_estimator/landmark_database.h
+++ b/include/basalt/vi_estimator/landmark_database.h
@@ -44,7 +44,21 @@ struct KeypointPosition {
   Eigen::Vector2d dir;
   double id;
 
+  inline void backup() {
+    backup_dir = dir;
+    backup_id = id;
+  }
+
+  inline void restore() {
+    dir = backup_dir;
+    id = backup_id;
+  }
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  Eigen::Vector2d backup_dir;
+  double backup_id;
 };
 
 struct KeypointObservation {
diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers
index 2ae558c..df6b2ca 160000
--- a/thirdparty/basalt-headers
+++ b/thirdparty/basalt-headers
@@ -1 +1 @@
-Subproject commit 2ae558ce2b494d21908391b89cf13df93d0dcfa9
+Subproject commit df6b2ca48658a3261370cbf9799ed69f7692525a