odometry_estimation_cpu.hpp

#pragma once
 
#include <glim/odometry/odometry_estimation_imu.hpp>
 
namespace gtsam_points {
 
class GaussianVoxelMapCPU;
struct FlatContainer;
template <typename VoxelContents>
class IncrementalVoxelMap;
using iVox = IncrementalVoxelMap<FlatContainer>;
}  // namespace gtsam_points
 
namespace glim {
 
struct OdometryEstimationCPUParams : public OdometryEstimationIMUParams {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  OdometryEstimationCPUParams();
  virtual ~OdometryEstimationCPUParams();
 
public:
  // Registration params
  std::string registration_type;    
  int max_iterations;               
  int lru_thresh;                   
  double target_downsampling_rate;  
 
  double ivox_resolution;  
  double ivox_min_dist;    
 
  double vgicp_resolution;               
  int vgicp_voxelmap_levels;             
  double vgicp_voxelmap_scaling_factor;  
};
 
class OdometryEstimationCPU : public OdometryEstimationIMU {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  OdometryEstimationCPU(const OdometryEstimationCPUParams& params = OdometryEstimationCPUParams());
  virtual ~OdometryEstimationCPU() override;
 
private:
  virtual gtsam::NonlinearFactorGraph create_factors(const int current, const gtsam_points::shared_ptr<gtsam::ImuFactor>& imu_factor, gtsam::Values& new_values) override;
 
  virtual void fallback_smoother() override;
 
  void update_target(const int current, const Eigen::Isometry3d& T_target_imu);
 
private:
  // Registration params
  std::mt19937 mt;                                                                   
  Eigen::Isometry3d last_T_target_imu;                                               
  std::vector<std::shared_ptr<gtsam_points::GaussianVoxelMapCPU>> target_voxelmaps;  
  std::shared_ptr<gtsam_points::iVox> target_ivox;                                   
  EstimationFrame::ConstPtr target_ivox_frame;                                       
};
 
}  // namespace glim