Program Listing for File GridMapPclConverter.hpp

Return to documentation for file (/tmp/ws/src/grid_map/grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp)

/*
 * GridMapPclConverter.hpp
 *
 *  Created on: Apr 14, 2016
 *      Author: Dominic Jud
 *   Institute: ETH Zurich, ANYbotics
 */

#ifndef GRID_MAP_PCL__GRIDMAPPCLCONVERTER_HPP_
#define GRID_MAP_PCL__GRIDMAPPCLCONVERTER_HPP_

#include <grid_map_core/grid_map_core.hpp>

// PCL
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl/PolygonMesh.h>
#include <pcl/common/common.h>

// STD
#include <iostream>
#include <string>
#include <vector>
#include <algorithm>
#include <cmath>

namespace grid_map
{

class GridMapPclConverter
{
public:
  GridMapPclConverter();

  virtual ~GridMapPclConverter();

  static bool initializeFromPolygonMesh(
    const pcl::PolygonMesh & mesh, const double resolution,
    grid_map::GridMap & gridMap);

  static bool addLayerFromPolygonMesh(
    const pcl::PolygonMesh & mesh, const std::string & layer,
    grid_map::GridMap & gridMap);

private:
  static bool rayTriangleIntersect(
    const Eigen::Vector3f & point,
    const Eigen::Vector3f & ray,
    const Eigen::Matrix3f & triangleVertices,
    Eigen::Vector3f & intersectionPoint);
};

}   // namespace grid_map
#endif  // GRID_MAP_PCL__GRIDMAPPCLCONVERTER_HPP_