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 @@ -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 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