point_cloud.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <memory>
#include <vector>
#include <unordered_map>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
 
namespace gtsam_points {
 
struct PointCloud {
public:
  using Ptr = std::shared_ptr<PointCloud>;
  using ConstPtr = std::shared_ptr<const PointCloud>;
 
  PointCloud()
  : num_points(0),
    times(nullptr),
    points(nullptr),
    normals(nullptr),
    covs(nullptr),
    intensities(nullptr),
    times_gpu(nullptr),
    points_gpu(nullptr),
    normals_gpu(nullptr),
    covs_gpu(nullptr),
    intensities_gpu(nullptr) {}
  virtual ~PointCloud() {}
 
  // Forbid copy
  PointCloud(const PointCloud&) = delete;
  PointCloud& operator=(PointCloud const&) = delete;
 
  size_t size() const { return num_points; }
 
  bool has_times() const;        
  bool has_points() const;       
  bool has_normals() const;      
  bool has_covs() const;         
  bool has_intensities() const;  
 
  bool check_times() const;        
  bool check_points() const;       
  bool check_normals() const;      
  bool check_covs() const;         
  bool check_intensities() const;  
 
  bool has_times_gpu() const;        
  bool has_points_gpu() const;       
  bool has_normals_gpu() const;      
  bool has_covs_gpu() const;         
  bool has_intensities_gpu() const;  
 
  bool check_times_gpu() const;        
  bool check_points_gpu() const;       
  bool check_normals_gpu() const;      
  bool check_covs_gpu() const;         
  bool check_intensities_gpu() const;  
 
  template <typename T>
  const T* aux_attribute(const std::string& attrib) const {
    const auto found = aux_attributes.find(attrib);
    if (found == aux_attributes.end()) {
      std::cerr << "warning: attribute " << attrib << " not found!!" << std::endl;
      return nullptr;
    }
 
    if (sizeof(T) != found->second.first) {
      std::cerr << "warning: attribute element size mismatch!! attrib:" << attrib << " size:" << found->second.first << " requested:" << sizeof(T)
                << std::endl;
    }
 
    return static_cast<const T*>(found->second.second);
  }
 
  void save(const std::string& path) const;
 
  void save_compact(const std::string& path) const;
 
public:
  size_t num_points;  
 
  double* times;             
  Eigen::Vector4d* points;   
  Eigen::Vector4d* normals;  
  Eigen::Matrix4d* covs;     
  double* intensities;       
 
  std::unordered_map<std::string, std::pair<size_t, void*>> aux_attributes;
 
  float* times_gpu;              
  Eigen::Vector3f* points_gpu;   
  Eigen::Vector3f* normals_gpu;  
  Eigen::Matrix3f* covs_gpu;     
  float* intensities_gpu;        
};
}  // namespace gtsam_points
 
#ifndef DONT_DEFINE_FRAME_TRAITS
#include <gtsam_points/types/frame_traits.hpp>
 
namespace gtsam_points {
namespace frame {
 
template <>
struct traits<PointCloud> {
  static int size(const PointCloud& frame) { return frame.size(); }
 
  static bool has_times(const PointCloud& frame) { return frame.has_times(); }
  static bool has_points(const PointCloud& frame) { return frame.has_points(); }
  static bool has_normals(const PointCloud& frame) { return frame.has_normals(); }
  static bool has_covs(const PointCloud& frame) { return frame.has_covs(); }
  static bool has_intensities(const PointCloud& frame) { return frame.has_intensities(); }
 
  static double time(const PointCloud& frame, size_t i) { return frame.times[i]; }
  static const Eigen::Vector4d& point(const PointCloud& frame, size_t i) { return frame.points[i]; }
  static const Eigen::Vector4d& normal(const PointCloud& frame, size_t i) { return frame.normals[i]; }
  static const Eigen::Matrix4d& cov(const PointCloud& frame, size_t i) { return frame.covs[i]; }
  static double intensity(const PointCloud& frame, size_t i) { return frame.intensities[i]; }
 
  static const Eigen::Vector4d* points_ptr(const PointCloud& frame) { return frame.points; }
};
 
}  // namespace frame
}  // namespace gtsam_points
#endif