lidar.hpp

Go to the documentation of this file.

// Original Author of FLOAM: Wang Han 
// Email wh200720041@gmail.com
// Homepage https://wanghan.pro
#ifndef FLOAM__LIDAR_HPP_
#define FLOAM__LIDAR_HPP_
#include <pcl/features/organized_edge_detection.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/filters/conditional_removal.h>

#include "floam/lidar_utils.hpp"

namespace floam
{
namespace lidar
{

// TODO(flynneva): template the point type
template <class T>
class Lidar
{
public:
  void detectSurfaces(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr & points,
    pcl::PointCloud<pcl::PointNormal>::Ptr & normals);

  void detectEdges(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr & points,
    pcl::PointCloud<pcl::PointXYZL>::Ptr & edges);

  T m_settings;
  floam::lidar::Total m_total;

  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_lidarScans;
  std::vector<int> m_addedPoints;

  pcl::KdTreeFLANN<pcl::PointXYZ> m_kdTree;
  std::vector<int> pointSearch;
  std::vector<float> pointSquaredDistance;

  Eigen::Matrix<std::complex<double>, 3, 3> m_covariance;
  Eigen::Matrix<std::complex<double>, 3, 1> m_eigenValues;

};

template <>
void Lidar<floam::lidar::Scanner>::detectSurfaces(
  const pcl::PointCloud<pcl::PointXYZ>::Ptr & points,
  pcl::PointCloud<pcl::PointNormal>::Ptr & normals);

template <>
void Lidar<floam::lidar::Imager>::detectSurfaces(
  const pcl::PointCloud<pcl::PointXYZ>::Ptr & points,
  pcl::PointCloud<pcl::PointNormal>::Ptr & normals);

template <>
void Lidar<floam::lidar::Scanner>::detectEdges(
  const pcl::PointCloud<pcl::PointXYZ>::Ptr & points,
  pcl::PointCloud<pcl::PointXYZL>::Ptr  & edges);

template <>
void Lidar<floam::lidar::Imager>::detectEdges(
  const pcl::PointCloud<pcl::PointXYZ>::Ptr & points,
  pcl::PointCloud<pcl::PointXYZL>::Ptr & edges);

template <typename PointT>
class GenericCondition : public pcl::ConditionBase<PointT>
{
public:
  typedef std::shared_ptr<GenericCondition<PointT>> Ptr;
  typedef std::shared_ptr<const GenericCondition<PointT>> ConstPtr;
  typedef std::function<bool(const PointT&)> FunctorT;

  GenericCondition(FunctorT evaluator): 
    pcl::ConditionBase<PointT>(),_evaluator( evaluator ) 
  {}

  virtual bool evaluate (const PointT &point) const {
    // just delegate ALL the work to the injected std::function
    return _evaluator(point);
  }
private:
  FunctorT _evaluator;
};

}  // namespace lidar
}  // namespace floam

#endif  // FLOAM__LIDAR_HPP_