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/PolygonMesh.h>
#include <pcl/common/common.h>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

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

namespace grid_map {

class GridMapPclConverter {
 public:
  GridMapPclConverter() = default;

  virtual ~GridMapPclConverter() = default;

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