GridMapPclConverter.hpp

Go to the documentation of this file.

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

#pragma once

#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 <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 */