sub_map.hpp

#pragma once
 
#include <memory>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <glim/odometry/estimation_frame.hpp>
 
namespace glim {
 
struct SubMap {
public:
  using Ptr = std::shared_ptr<SubMap>;
  using ConstPtr = std::shared_ptr<const SubMap>;
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  SubMap() : id(-1), session_id(0) {}
 
  void drop_frame_points();
 
  EstimationFrame::ConstPtr origin_frame() const { return frames[frames.size() / 2]; }
 
  EstimationFrame::ConstPtr origin_odom_frame() const { return odom_frames[frames.size() / 2]; }
 
  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());
  }
 
  void save(const std::string& path) const;
 
  static SubMap::Ptr load(const std::string& path);
 
public:
  int id;          
  int session_id;  
 
  Eigen::Isometry3d T_world_origin;       
  Eigen::Isometry3d T_origin_endpoint_L;  
  Eigen::Isometry3d T_origin_endpoint_R;  
 
  gtsam_points::PointCloud::Ptr frame;                         
  std::vector<gtsam_points::GaussianVoxelMap::Ptr> voxelmaps;  
 
  std::vector<EstimationFrame::ConstPtr> frames;       
  std::vector<EstimationFrame::ConstPtr> odom_frames;  
 
  std::unordered_map<std::string, std::shared_ptr<void>> custom_data;  
};
 
}  // namespace  glim