State.h
Go to the documentation of this file.
1 /*
2  * OpenVINS: An Open Platform for Visual-Inertial Research
3  * Copyright (C) 2018-2023 Patrick Geneva
4  * Copyright (C) 2018-2023 Guoquan Huang
5  * Copyright (C) 2018-2023 OpenVINS Contributors
6  * Copyright (C) 2018-2019 Kevin Eckenhoff
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program. If not, see <https://www.gnu.org/licenses/>.
20  */
21 
22 #ifndef OV_MSCKF_STATE_H
23 #define OV_MSCKF_STATE_H
24 
25 #include <map>
26 #include <memory>
27 #include <mutex>
28 #include <unordered_map>
29 #include <vector>
30 
31 #include "StateOptions.h"
32 #include "cam/CamBase.h"
33 #include "types/IMU.h"
34 #include "types/Landmark.h"
35 #include "types/PoseJPL.h"
36 #include "types/Type.h"
37 #include "types/Vec.h"
38 
39 namespace ov_msckf {
40 
49 class State {
50 
51 public:
56  State(StateOptions &options_);
57 
58  ~State() {}
59 
66  double margtimestep() {
67  std::lock_guard<std::mutex> lock(_mutex_state);
68  double time = INFINITY;
69  for (const auto &clone_imu : _clones_IMU) {
70  if (clone_imu.first < time) {
71  time = clone_imu.first;
72  }
73  }
74  return time;
75  }
76 
81  int max_covariance_size() { return (int)_Cov.rows(); }
82 
91  static Eigen::Matrix3d Dm(StateOptions::ImuModel imu_model, const Eigen::MatrixXd &vec) {
92  assert(vec.rows() == 6);
93  assert(vec.cols() == 1);
94  Eigen::Matrix3d D_matrix = Eigen::Matrix3d::Identity();
95  if (imu_model == StateOptions::ImuModel::KALIBR) {
96  D_matrix << vec(0), 0, 0, vec(1), vec(3), 0, vec(2), vec(4), vec(5);
97  } else {
98  D_matrix << vec(0), vec(1), vec(3), 0, vec(2), vec(4), 0, 0, vec(5);
99  }
100  return D_matrix;
101  }
102 
110  static Eigen::Matrix3d Tg(const Eigen::MatrixXd &vec) {
111  assert(vec.rows() == 9);
112  assert(vec.cols() == 1);
113  Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero();
114  Tg << vec(0), vec(3), vec(6), vec(1), vec(4), vec(7), vec(2), vec(5), vec(8);
115  return Tg;
116  }
117 
126  int imu_intrinsic_size() const {
127  int sz = 0;
129  sz += 15;
131  sz += 9;
132  }
133  }
134  return sz;
135  }
136 
138  std::mutex _mutex_state;
139 
141  double _timestamp = -1;
142 
145 
147  std::shared_ptr<ov_type::IMU> _imu;
148 
150  std::map<double, std::shared_ptr<ov_type::PoseJPL>> _clones_IMU;
151 
153  std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> _features_SLAM;
154 
156  std::shared_ptr<ov_type::Vec> _calib_dt_CAMtoIMU;
157 
159  std::unordered_map<size_t, std::shared_ptr<ov_type::PoseJPL>> _calib_IMUtoCAM;
160 
162  std::unordered_map<size_t, std::shared_ptr<ov_type::Vec>> _cam_intrinsics;
163 
165  std::unordered_map<size_t, std::shared_ptr<ov_core::CamBase>> _cam_intrinsics_cameras;
166 
168  std::shared_ptr<ov_type::Vec> _calib_imu_dw;
169 
171  std::shared_ptr<ov_type::Vec> _calib_imu_da;
172 
174  std::shared_ptr<ov_type::Vec> _calib_imu_tg;
175 
177  std::shared_ptr<ov_type::JPLQuat> _calib_imu_GYROtoIMU;
178 
180  std::shared_ptr<ov_type::JPLQuat> _calib_imu_ACCtoIMU;
181 
182 private:
183  // Define that the state helper is a friend class of this class
184  // This will allow it to access the below functions which should normally not be called
185  // This prevents a developer from thinking that the "insert clone" will actually correctly add it to the covariance
186  friend class StateHelper;
187 
189  Eigen::MatrixXd _Cov;
190 
192  std::vector<std::shared_ptr<ov_type::Type>> _variables;
193 };
194 
195 } // namespace ov_msckf
196 
197 #endif // OV_MSCKF_STATE_H
ov_msckf::State::_calib_imu_da
std::shared_ptr< ov_type::Vec > _calib_imu_da
Accelerometer IMU intrinsics (scale imperfection and axis misalignment)
Definition: State.h:171
ov_msckf::State::margtimestep
double margtimestep()
Will return the timestep that we will marginalize next. As of right now, since we are using a sliding...
Definition: State.h:66
StateOptions.h
ov_msckf::State::imu_intrinsic_size
int imu_intrinsic_size() const
Calculates the error state size for imu intrinsics.
Definition: State.h:126
ov_msckf::State::_cam_intrinsics
std::unordered_map< size_t, std::shared_ptr< ov_type::Vec > > _cam_intrinsics
Camera intrinsics.
Definition: State.h:162
ov_msckf::State::Dm
static Eigen::Matrix3d Dm(StateOptions::ImuModel imu_model, const Eigen::MatrixXd &vec)
Gyroscope and accelerometer intrinsic matrix (scale imperfection and axis misalignment)
Definition: State.h:91
IMU.h
ov_msckf::State::_cam_intrinsics_cameras
std::unordered_map< size_t, std::shared_ptr< ov_core::CamBase > > _cam_intrinsics_cameras
Camera intrinsics camera objects.
Definition: State.h:165
ov_msckf::StateOptions::ImuModel
ImuModel
IMU intrinsic models.
Definition: StateOptions.h:62
ov_msckf
Extended Kalman Filter estimator.
Definition: VioManager.h:46
ov_msckf::State::_calib_imu_ACCtoIMU
std::shared_ptr< ov_type::JPLQuat > _calib_imu_ACCtoIMU
Rotation from accelerometer to the "IMU" gyroscope frame frame (rpng model)
Definition: State.h:180
ov_msckf::State::_calib_dt_CAMtoIMU
std::shared_ptr< ov_type::Vec > _calib_dt_CAMtoIMU
Time offset base IMU to camera (t_imu = t_cam + t_off)
Definition: State.h:156
ov_msckf::State::_imu
std::shared_ptr< ov_type::IMU > _imu
Pointer to the "active" IMU state (q_GtoI, p_IinG, v_IinG, bg, ba)
Definition: State.h:147
ov_msckf::State::_calib_imu_tg
std::shared_ptr< ov_type::Vec > _calib_imu_tg
Gyroscope gravity sensitivity.
Definition: State.h:174
ov_msckf::State::_Cov
Eigen::MatrixXd _Cov
Covariance of all active variables.
Definition: State.h:189
ov_msckf::State::_mutex_state
std::mutex _mutex_state
Mutex for locking access to the state.
Definition: State.h:138
ov_msckf::StateOptions::do_calib_imu_intrinsics
bool do_calib_imu_intrinsics
Bool to determine whether or not to calibrate the IMU intrinsics.
Definition: StateOptions.h:56
ov_msckf::State::State
State(StateOptions &options_)
Default Constructor (will initialize variables to defaults)
Definition: State.cpp:28
PoseJPL.h
ov_msckf::State::_variables
std::vector< std::shared_ptr< ov_type::Type > > _variables
Vector of variables.
Definition: State.h:192
ov_msckf::State::_calib_IMUtoCAM
std::unordered_map< size_t, std::shared_ptr< ov_type::PoseJPL > > _calib_IMUtoCAM
Calibration poses for each camera (R_ItoC, p_IinC)
Definition: State.h:159
ov_msckf::State::_features_SLAM
std::unordered_map< size_t, std::shared_ptr< ov_type::Landmark > > _features_SLAM
Our current set of SLAM features (3d positions)
Definition: State.h:153
Vec.h
ov_msckf::State::_options
StateOptions _options
Struct containing filter options.
Definition: State.h:144
ov_msckf::State::~State
~State()
Definition: State.h:58
ov_msckf::State::_clones_IMU
std::map< double, std::shared_ptr< ov_type::PoseJPL > > _clones_IMU
Map between imaging times and clone poses (q_GtoIi, p_IiinG)
Definition: State.h:150
ov_msckf::StateOptions
Struct which stores all our filter options.
Definition: StateOptions.h:35
ov_msckf::State::_calib_imu_GYROtoIMU
std::shared_ptr< ov_type::JPLQuat > _calib_imu_GYROtoIMU
Rotation from gyroscope frame to the "IMU" accelerometer frame (kalibr model)
Definition: State.h:177
ov_msckf::State::Tg
static Eigen::Matrix3d Tg(const Eigen::MatrixXd &vec)
Gyroscope gravity sensitivity.
Definition: State.h:110
Landmark.h
ov_msckf::StateHelper
Helper which manipulates the State and its covariance.
Definition: StateHelper.h:45
ov_msckf::State::_calib_imu_dw
std::shared_ptr< ov_type::Vec > _calib_imu_dw
Gyroscope IMU intrinsics (scale imperfection and axis misalignment)
Definition: State.h:168
CamBase.h
ov_msckf::State
State of our filter.
Definition: State.h:49
ov_msckf::StateOptions::do_calib_imu_g_sensitivity
bool do_calib_imu_g_sensitivity
Bool to determine whether or not to calibrate the Gravity sensitivity.
Definition: StateOptions.h:59
Type.h
ov_msckf::State::_timestamp
double _timestamp
Current timestamp (should be the last update time in camera clock frame!)
Definition: State.h:141
ov_msckf::State::max_covariance_size
int max_covariance_size()
Calculates the current max size of the covariance.
Definition: State.h:81


ov_msckf
Author(s): Patrick Geneva , Kevin Eckenhoff , Guoquan Huang
autogenerated on Mon Dec 16 2024 03:06:54