types.hpp

// Copyright 2023 Jacob Hartzer
//
// 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 EKF__TYPES_HPP_
#define EKF__TYPES_HPP_
 
#include <eigen3/Eigen/Eigen>
 
#include <map>
#include <vector>
 
#include "ekf/constants.hpp"
 
#include <opencv2/opencv.hpp>
 
enum class SensorType
{
  IMU,
  Camera,
  Tracker,
  GPS
};
 
enum class AugmentationType
{
  NONE,     // No camera frames are augmented (fiducial detection only)
  ALL,      // All camera frames are augmented
  PRIMARY,  // Only the primary camera frames are augmented
  TIME,     // Time-based frame augmentation
  ERROR     // Linear Error-based frame augmentation
};
 
class Intrinsics
{
public:
  Intrinsics() {}
 
  cv::Mat ToCameraMatrix();
 
  cv::Mat ToDistortionVector();
 
  double f_x {0.01};           
  double f_y {0.01};           
  double k_1 {0.0};            
  double k_2 {0.0};            
  double p_1 {0.0};            
  double p_2 {0.0};            
  double width {1920};         
  double height {1080};        
  double pixel_size {5.0e-6};  
};
 
class BodyState
{
public:
  BodyState() {}
 
  Eigen::VectorXd ToVector() const;
 
  void SetState(const Eigen::VectorXd & state);
 
  Eigen::Vector3d pos_b_in_l{0.0, 0.0, 0.0};          
  Eigen::Vector3d vel_b_in_l{0.0, 0.0, 0.0};          
  Eigen::Vector3d acc_b_in_l{g_gravity};              
  Eigen::Quaterniond ang_b_to_l{1.0, 0.0, 0.0, 0.0};  
  Eigen::Vector3d ang_vel_b_in_l{0.0, 0.0, 0.0};      
  Eigen::Vector3d ang_acc_b_in_l{0.0, 0.0, 0.0};      
  unsigned int size{g_body_state_size};               
  int index{-1};                                      
};
 
class ImuState
{
public:
  ImuState() {}
 
  Eigen::VectorXd ToVector() const;
 
  bool GetIsExtrinsic() const;
  bool GetIsIntrinsic() const;
  void SetIsExtrinsic(bool extrinsic);
  void SetIsIntrinsic(bool intrinsic);
 
  double pos_stability {1e-9};                        
  double ang_stability {1e-9};                        
  double acc_bias_stability {1e-9};                   
  double omg_bias_stability {1e-9};                   
  Eigen::Vector3d pos_i_in_b{0.0, 0.0, 0.0};          
  Eigen::Quaterniond ang_i_to_b{1.0, 0.0, 0.0, 0.0};  
  Eigen::Vector3d acc_bias{0.0, 0.0, 0.0};            
  Eigen::Vector3d omg_bias{0.0, 0.0, 0.0};            
  unsigned int size{0};                               
  int index{-1};                                      
  int index_intrinsic{-1};                            
  int index_extrinsic{-1};                            
 
private:
  void refresh_size();
  bool is_extrinsic{false};                           
  bool is_intrinsic{false};                           
};
 
class GpsState
{
public:
  GpsState() {}
 
  Eigen::VectorXd ToVector() const;
 
  bool GetIsExtrinsic() const;
  void SetIsExtrinsic(bool extrinsic);
 
  Eigen::Vector3d pos_a_in_b{0.0, 0.0, 0.0};  
  double pos_stability {1e-9};                
  unsigned int size{0};                       
  int index{-1};                              
 
private:
  void refresh_size();
  bool is_extrinsic{false};                   
};
 
class AugState
{
public:
  AugState() {}
 
  Eigen::VectorXd ToVector() const;
 
  int frame_id {-1};                                  
  double time {0.0};                                  
  Eigen::Vector3d pos_b_in_l{0.0, 0.0, 0.0};          
  Eigen::Quaterniond ang_b_to_l{1.0, 0.0, 0.0, 0.0};  
  unsigned int size{g_aug_state_size};                
  int index{-1};                                      
  double alpha {0.0};                                 
};
 
class CamState
{
public:
  CamState() {}
 
  Eigen::VectorXd ToVector() const;
 
  bool GetIsExtrinsic() const;
  void SetIsExtrinsic(bool extrinsic);
 
  double pos_stability {1e-9};                        
  double ang_stability {1e-9};                        
  Eigen::Vector3d pos_c_in_b{0.0, 0.0, 0.0};          
  Eigen::Quaterniond ang_c_to_b{1.0, 0.0, 0.0, 0.0};  
  Intrinsics intrinsics;                              
  double rate{1.0};                                   
  unsigned int size{0};                               
  int index{-1};                                      
 
private:
  void refresh_size();
  bool is_extrinsic{false};
};
 
typedef struct FeaturePoint
{
  int frame_id;            
  double frame_time;       
  cv::KeyPoint key_point;  
} FeaturePoint;
 
typedef struct FeatureTrack
{
  std::vector<FeaturePoint> track;        
  Eigen::Vector3d true_feature_position;  
} FeatureTrack;
 
typedef std::vector<FeatureTrack> FeatureTracks;
 
typedef struct BoardDetection
{
  int frame_id{-1};                
  double frame_time{-1.0};         
  Eigen::Vector3d pos_f_in_c;      
  Eigen::Quaterniond ang_f_to_c;   
  Eigen::Vector3d pos_error{1e-9, 1e-9, 1e-9};  
  Eigen::Vector3d ang_error{1e-9, 1e-9, 1e-9};  
} BoardDetection;
 
class FidState
{
public:
  FidState() {}
 
  Eigen::VectorXd ToVector() const;
 
  bool GetIsExtrinsic() const;
  void SetIsExtrinsic(bool extrinsic);
 
  int frame_id{-1};               
  Eigen::Vector3d pos_f_in_l;     
  Eigen::Quaterniond ang_f_to_l;  
  double pos_stability {1e-9};    
  double ang_stability {1e-9};    
  unsigned int size{0};           
  int index{-1};                  
  unsigned int id{0};             
 
private:
  void refresh_size();
  bool is_extrinsic{false};       
};
 
class State
{
public:
  State() {}
 
  Eigen::VectorXd ToVector() const;
 
  unsigned int GetStateSize() const;
 
  BodyState body_state {};                        
  std::map<unsigned int, ImuState> imu_states{};  
  std::map<unsigned int, GpsState> gps_states{};  
  std::map<unsigned int, CamState> cam_states{};  
  std::map<unsigned int, FidState> fid_states{};  
  std::map<unsigned int, std::vector<AugState>> aug_states{};  
};
 
BodyState & operator+=(BodyState & l_body_state, const BodyState & r_body_state);
BodyState & operator+=(BodyState & l_body_state, const Eigen::VectorXd & r_vector);
ImuState & operator+=(ImuState & l_imu_state, const Eigen::VectorXd & r_vector);
std::map<unsigned int, ImuState> & operator+=(
  std::map<unsigned int, ImuState> & l_imu_state, const Eigen::VectorXd & r_vector);
std::map<unsigned int, GpsState> & operator+=(
  std::map<unsigned int, GpsState> & l_gps_state, const Eigen::VectorXd & r_vector);
std::map<unsigned int, CamState> & operator+=(
  std::map<unsigned int, CamState> & l_cam_state, const Eigen::VectorXd & r_vector);
std::map<unsigned int, FidState> & operator+=(
  std::map<unsigned int, FidState> & l_fid_state, const Eigen::VectorXd & r_vector);
std::vector<AugState> & operator+=(
  std::vector<AugState> & l_augState, const Eigen::VectorXd & r_vector);
 
State & operator+=(State & l_state, State & rState);
State & operator+=(State & l_state, const Eigen::VectorXd & r_vector);
 
enum class GpsInitType
{
  CONSTANT,
  BASELINE_DIST,
  ERROR_THRESHOLD
};
 
#endif  // EKF__TYPES_HPP_