async_global_mapping.hpp

#pragma once
 
#include <atomic>
#include <mutex>
#include <thread>
#include <glim/mapping/global_mapping.hpp>
#include <glim/util/concurrent_vector.hpp>
 
namespace spdlog {
class logger;
}
 
namespace glim {
 
class AsyncGlobalMapping {
public:
  AsyncGlobalMapping(const std::shared_ptr<glim::GlobalMappingBase>& global_mapping, const int optimization_interval_sec = 5);
 
  ~AsyncGlobalMapping();
 
#ifdef GLIM_USE_OPENCV
  void insert_image(const double stamp, const cv::Mat& image);
#endif
 
  void insert_imu(const double stamp, const Eigen::Vector3d& linear_acc, const Eigen::Vector3d& angular_vel);
 
  void insert_submap(const SubMap::Ptr& submap);
 
  void join();
 
  int workload() const;
 
  void save(const std::string& path);
 
  gtsam_points::PointCloud::Ptr export_points();
 
  std::shared_ptr<glim::GlobalMappingBase> get_global_mapping() {
    std::lock_guard<std::mutex> lock(global_mapping_mutex);
    return global_mapping;
  }
 
private:
  void run();
 
private:
  std::atomic_bool kill_switch;      
  std::atomic_bool end_of_sequence;  
  std::thread thread;
 
#ifdef GLIM_USE_OPENCV
  ConcurrentVector<std::pair<double, cv::Mat>> input_image_queue;
#endif
  ConcurrentVector<Eigen::Matrix<double, 7, 1>> input_imu_queue;
  ConcurrentVector<SubMap::Ptr> input_submap_queue;
 
  int optimization_interval;
  std::atomic_bool request_to_optimize;
  std::atomic_bool request_to_recover;
  std::atomic<double> request_to_find_overlapping_submaps;
 
  std::mutex global_mapping_mutex;
  std::shared_ptr<glim::GlobalMappingBase> global_mapping;
 
  // Logging
  std::shared_ptr<spdlog::logger> logger;
};
 
}  // namespace  glim