State.h

Go to the documentation of this file.

/*
 * OpenVINS: An Open Platform for Visual-Inertial Research
 * Copyright (C) 2018-2023 Patrick Geneva
 * Copyright (C) 2018-2023 Guoquan Huang
 * Copyright (C) 2018-2023 OpenVINS Contributors
 * Copyright (C) 2018-2019 Kevin Eckenhoff
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
 
#ifndef OV_MSCKF_STATE_H
#define OV_MSCKF_STATE_H
 
#include <map>
#include <memory>
#include <mutex>
#include <unordered_map>
#include <vector>
 
#include "StateOptions.h"
#include "cam/CamBase.h"
#include "types/IMU.h"
#include "types/Landmark.h"
#include "types/PoseJPL.h"
#include "types/Type.h"
#include "types/Vec.h"
 
namespace ov_msckf {
 
class State {
 
public:
  State(StateOptions &options_);
 
  ~State() {}
 
  double margtimestep() {
    std::lock_guard<std::mutex> lock(_mutex_state);
    double time = INFINITY;
    for (const auto &clone_imu : _clones_IMU) {
      if (clone_imu.first < time) {
        time = clone_imu.first;
      }
    }
    return time;
  }
 
  int max_covariance_size() { return (int)_Cov.rows(); }
 
  static Eigen::Matrix3d Dm(StateOptions::ImuModel imu_model, const Eigen::MatrixXd &vec) {
    assert(vec.rows() == 6);
    assert(vec.cols() == 1);
    Eigen::Matrix3d D_matrix = Eigen::Matrix3d::Identity();
    if (imu_model == StateOptions::ImuModel::KALIBR) {
      D_matrix << vec(0), 0, 0, vec(1), vec(3), 0, vec(2), vec(4), vec(5);
    } else {
      D_matrix << vec(0), vec(1), vec(3), 0, vec(2), vec(4), 0, 0, vec(5);
    }
    return D_matrix;
  }
 
  static Eigen::Matrix3d Tg(const Eigen::MatrixXd &vec) {
    assert(vec.rows() == 9);
    assert(vec.cols() == 1);
    Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero();
    Tg << vec(0), vec(3), vec(6), vec(1), vec(4), vec(7), vec(2), vec(5), vec(8);
    return Tg;
  }
 
  int imu_intrinsic_size() const {
    int sz = 0;
    if (_options.do_calib_imu_intrinsics) {
      sz += 15;
      if (_options.do_calib_imu_g_sensitivity) {
        sz += 9;
      }
    }
    return sz;
  }
 
  std::mutex _mutex_state;
 
  double _timestamp = -1;
 
  StateOptions _options;
 
  std::shared_ptr<ov_type::IMU> _imu;
 
  std::map<double, std::shared_ptr<ov_type::PoseJPL>> _clones_IMU;
 
  std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> _features_SLAM;
 
  std::shared_ptr<ov_type::Vec> _calib_dt_CAMtoIMU;
 
  std::unordered_map<size_t, std::shared_ptr<ov_type::PoseJPL>> _calib_IMUtoCAM;
 
  std::unordered_map<size_t, std::shared_ptr<ov_type::Vec>> _cam_intrinsics;
 
  std::unordered_map<size_t, std::shared_ptr<ov_core::CamBase>> _cam_intrinsics_cameras;
 
  std::shared_ptr<ov_type::Vec> _calib_imu_dw;
 
  std::shared_ptr<ov_type::Vec> _calib_imu_da;
 
  std::shared_ptr<ov_type::Vec> _calib_imu_tg;
 
  std::shared_ptr<ov_type::JPLQuat> _calib_imu_GYROtoIMU;
 
  std::shared_ptr<ov_type::JPLQuat> _calib_imu_ACCtoIMU;
 
private:
  // Define that the state helper is a friend class of this class
  // This will allow it to access the below functions which should normally not be called
  // This prevents a developer from thinking that the "insert clone" will actually correctly add it to the covariance
  friend class StateHelper;
 
  Eigen::MatrixXd _Cov;
 
  std::vector<std::shared_ptr<ov_type::Type>> _variables;
};
 
} // namespace ov_msckf
 
#endif // OV_MSCKF_STATE_H