global_mapping.hpp

#pragma once
 
#include <any>
#include <memory>
#include <random>
#include <glim/mapping/global_mapping_base.hpp>
 
namespace gtsam {
class Values;
class NonlinearFactorGraph;
}  // namespace gtsam
 
namespace gtsam_points {
class ISAM2Ext;
class StreamTempBufferRoundRobin;
struct ISAM2ResultExt;
}  // namespace gtsam_points
 
namespace glim {
 
class IMUIntegration;
 
struct GlobalMappingParams {
public:
  GlobalMappingParams();
  ~GlobalMappingParams();
 
public:
  bool enable_gpu;
  bool enable_imu;
  bool enable_optimization;
  bool enable_between_factors;
  std::string between_registration_type;
 
  std::string registration_error_factor_type;
  double submap_voxel_resolution;
  double submap_voxel_resolution_max;
  double submap_voxel_resolution_dmin;
  double submap_voxel_resolution_dmax;
  int submap_voxelmap_levels;
  double submap_voxelmap_scaling_factor;
 
  double randomsampling_rate;
  double max_implicit_loop_distance;
  double min_implicit_loop_overlap;
 
  bool use_isam2_dogleg;
  double isam2_relinearize_skip;
  double isam2_relinearize_thresh;
 
  double init_pose_damping_scale;
};
 
class GlobalMapping : public GlobalMappingBase {
public:
  GlobalMapping(const GlobalMappingParams& params = GlobalMappingParams());
  virtual ~GlobalMapping();
 
  virtual void insert_imu(const double stamp, const Eigen::Vector3d& linear_acc, const Eigen::Vector3d& angular_vel) override;
  virtual void insert_submap(const SubMap::Ptr& submap) override;
 
  virtual void find_overlapping_submaps(double min_overlap) override;
  virtual void optimize() override;
 
  virtual void save(const std::string& path) override;
  virtual gtsam_points::PointCloud::Ptr export_points() override;
 
  bool load(const std::string& path);
 
private:
  void insert_submap(int current, const SubMap::Ptr& submap);
 
  std::shared_ptr<gtsam::NonlinearFactorGraph> create_between_factors(int current) const;
  std::shared_ptr<gtsam::NonlinearFactorGraph> create_matching_cost_factors(int current) const;
 
  void update_submaps();
  gtsam_points::ISAM2ResultExt update_isam2(const gtsam::NonlinearFactorGraph& new_factors, const gtsam::Values& new_values);
 
  void recover_graph() override;
  std::pair<gtsam::NonlinearFactorGraph, gtsam::Values> recover_graph(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, int start_from_frame_id) const;
 
private:
  using Params = GlobalMappingParams;
  Params params;
 
  std::mt19937 mt;
  int session_id;
 
  std::unique_ptr<IMUIntegration> imu_integration;
  std::any stream_buffer_roundrobin;
 
  std::vector<SubMap::Ptr> submaps;
  std::vector<gtsam_points::PointCloud::ConstPtr> subsampled_submaps;
 
  std::unique_ptr<gtsam::Values> new_values;
  std::unique_ptr<gtsam::NonlinearFactorGraph> new_factors;
 
  std::unique_ptr<gtsam_points::ISAM2Ext> isam2;
 
  std::shared_ptr<void> tbb_task_arena;
};
}  // namespace glim