estimation_frame.hpp

#pragma once
 
#include <memory>
#include <unordered_map>
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <gtsam_points/types/point_cloud.hpp>
#include <gtsam_points/types/gaussian_voxelmap.hpp>
#include <glim/preprocess/preprocessed_frame.hpp>
 
namespace glim {
 
enum class FrameID { WORLD, LIDAR, IMU };
 
struct EstimationFrame {
  using Ptr = std::shared_ptr<EstimationFrame>;
  using ConstPtr = std::shared_ptr<const EstimationFrame>;
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  EstimationFrame::Ptr clone() const;
 
  EstimationFrame::Ptr clone_wo_points() const;
 
  const Eigen::Isometry3d T_world_sensor() const;
 
  void set_T_world_sensor(FrameID frame_id, const Eigen::Isometry3d& T);
 
  template <typename T>
  T* get_custom_data(const std::string& key) {
    const auto found = custom_data.find(key);
    if (found == custom_data.end()) {
      return nullptr;
    }
    return reinterpret_cast<T*>(found->second.get());
  }
 
  template <typename T>
  const T* get_custom_data(const std::string& key) const {
    const auto found = custom_data.find(key);
    if (found == custom_data.end()) {
      return nullptr;
    }
    return reinterpret_cast<const T*>(found->second.get());
  }
 
public:
  long id;       
  double stamp;  
 
  Eigen::Isometry3d T_lidar_imu;    
  Eigen::Isometry3d T_world_lidar;  
  Eigen::Isometry3d T_world_imu;    
 
  Eigen::Vector3d v_world_imu;           
  Eigen::Matrix<double, 6, 1> imu_bias;  
 
  PreprocessedFrame::ConstPtr raw_frame;             
  Eigen::Matrix<double, 8, -1> imu_rate_trajectory;  
 
  FrameID frame_id;                                            
  gtsam_points::PointCloud::ConstPtr frame;                    
  std::vector<gtsam_points::GaussianVoxelMap::Ptr> voxelmaps;  
 
  std::unordered_map<std::string, std::shared_ptr<void>> custom_data;  
};
}  // namespace glim