ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added backup funtions to types

This commit is contained in:
Vladyslav Usenko 2019-08-07 16:45:25 +02:00
parent b3304884c6
commit 29e1a76407
3 changed files with 46 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
include/basalt/utils/imu_types.h
View File

@ -116,13 +116,27 @@ struct PoseVelBiasStateWithLin {
friend struct PoseStateWithLin; 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 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private: private:
bool linearized; bool linearized;
VecN delta; VecN delta;
PoseVelBiasState state_linearized, state_current; PoseVelBiasState state_linearized, state_current;
VecN backup_delta;
PoseVelBiasState backup_state_linearized, backup_state_current;
friend class cereal::access; friend class cereal::access;
template <class Archive> template <class Archive>
@ -190,14 +204,29 @@ struct PoseStateWithLin {
inline const VecN& getDelta() const { return delta; } inline const VecN& getDelta() const { return delta; }
inline int64_t getT_ns() const { return pose_linearized.t_ns; } 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 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private: private:
bool linearized; bool linearized;
VecN delta; VecN delta;
PoseState pose_linearized; PoseState pose_linearized;
Sophus::SE3d T_w_i_current; Sophus::SE3d T_w_i_current;
VecN backup_delta;
PoseState backup_pose_linearized;
Sophus::SE3d backup_T_w_i_current;
friend class cereal::access; friend class cereal::access;
template <class Archive> template <class Archive>

14
include/basalt/vi_estimator/landmark_database.h
View File

@ -44,7 +44,21 @@ struct KeypointPosition {
Eigen::Vector2d dir; Eigen::Vector2d dir;
double id; 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 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
Eigen::Vector2d backup_dir;
double backup_id;
}; };
struct KeypointObservation { struct KeypointObservation {

2
thirdparty/basalt-headers vendored

@ -1 +1 @@
Subproject commit 2ae558ce2b494d21908391b89cf13df93d0dcfa9 Subproject commit df6b2ca48658a3261370cbf9799ed69f7692525a