layer from polygon mesh

grid_map_pcl/CHANGELOG.rst

@@ -0,0 +1,6 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package grid_map_cv
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Forthcoming
+-----------

grid_map_pcl/CMakeLists.txt

@@ -0,0 +1,88 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(grid_map_pcl)
+
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  grid_map_core
+  pcl_ros
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+    ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    grid_map_core
+    pcl_ros
+  DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+## Declare a cpp library
+add_library(${PROJECT_NAME}
+  src/GridMapPclConverter.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(${PROJECT_NAME}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+#############
+## Install ##
+#############
+
+# Mark executables and/or libraries for installation
+install(
+  TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Mark cpp header files for installation
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.hpp"
+)
+
+#############
+## Testing ##
+#############
+
+#if(CATKIN_ENABLE_TESTING)
+#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+## Add gtest based cpp test target and link libraries
+#catkin_add_gtest(${PROJECT_NAME}-test test/test_grid_map_pcl.cpp test/GridMapPclTest.cpp)
+#endif()
+
+#if(TARGET ${PROJECT_NAME}-test)
+#  target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+#endif()
+

grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp

@@ -0,0 +1,75 @@
+/*
+ * GridMapPclConverter.hpp
+ *
+ *  Created on: Apr 14, 2016
+ *      Author: Dominic Jud
+ *	 Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#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 {
+
+/*!
+ * Conversions between grid maps and PCL types.
+ */
+class GridMapPclConverter
+{
+ public:
+    /*!
+     * Default constructor.
+     */
+    GridMapPclConverter();
+
+    /*!
+     * Destructor.
+     */
+    virtual ~GridMapPclConverter();
+
+    /*!
+     * Initializes the geometry of a grid map from a polygon mesh. This changes
+     * the geometry of the map and deletes all contents of the layers!
+     * @param[in] mesh the mesh.
+     * @param[in] resolution the desired resolution of the grid map [m/cell].
+     * @param[out] gridMap the grid map to be initialized.
+     * @return true if successful, false otherwise.
+     */
+    static bool initializeFromPolygonMesh(const pcl::PolygonMesh& mesh, const double resolution,
+                                          grid_map::GridMap& gridMap);
+
+    /*!
+     * Adds a layer with data from a polygon mesh. The mesh is ray traced from
+     * above (negative z-Direction).
+     * @param[in] mesh the mesh to be added. It can only consist of triangles!
+     * @param[in] layer the layer that is filled with the mesh data.
+     * @param[out] gridMap the grid map to be populated.
+     * @return true if successful, false otherwise.
+     */
+    static bool addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, const std::string& layer,
+                                        grid_map::GridMap& gridMap);
+
+ private:
+    static bool rayTriangleIntersect(const pcl::PointXYZ& point,
+                                     const Eigen::Vector3f& ray,
+                                     const pcl::Vertices& verts,
+                                     const pcl::PointCloud<pcl::PointXYZ>& cloud,
+                                     pcl::PointXYZ& intersectionPoint);
+
+};
+
+} /* namespace */

grid_map_pcl/include/grid_map_pcl/grid_map_pcl.hpp

@@ -0,0 +1,12 @@
+/*
+ * grid_map_pcl.hpp
+ *
+ *  Created on: Oct 19, 2016
+ *      Author: Dominic Jud
+ *	 Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#pragma once
+
+#include <grid_map_core/grid_map_core.hpp>
+#include <grid_map_pcl/GridMapPclConverter.hpp>

grid_map_pcl/package.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>grid_map_pcl</name>
+  <version>1.4.0</version>
+  <description>Conversions between grid maps and PCL types.</description>
+  <maintainer email="[email protected]">Péter Fankhauser</maintainer>
+  <license>BSD</license>
+  <url type="website">http://github.com/ethz-asl/grid_map</url>
+  <url type="bugtracker">http://github.com/ethz-asl/grid_map/issues</url>
+  <author email="[email protected]">Dominic Jud</author>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>grid_map_core</depend>
+  <depend>pcl_ros</depend>
+</package>

grid_map_pcl/src/GridMapPclConverter.cpp

@@ -0,0 +1,131 @@
+/*
+ * GridMapPclConverter.cpp
+ *
+ *  Created on: Oct 19, 2016
+ *      Author: Dominic Jud
+ *	 Institute: ETH Zurich, Robotic Systems Lab
+ */
+
+#include "grid_map_pcl/GridMapPclConverter.hpp"
+
+namespace grid_map {
+
+GridMapPclConverter::GridMapPclConverter()
+{
+}
+
+GridMapPclConverter::~GridMapPclConverter()
+{
+}
+
+bool GridMapPclConverter::initializeFromPolygonMesh(const pcl::PolygonMesh& mesh, const double resolution,
+                                                    grid_map::GridMap& gridMap) {
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+    pcl::fromPCLPointCloud2(mesh.cloud, cloud);
+    pcl::PointXYZ minBound;
+    pcl::PointXYZ maxBound;
+    pcl::getMinMax3D(cloud, minBound, maxBound);
+
+    grid_map::Length length = grid_map::Length(maxBound.x - minBound.x, maxBound.y - minBound.y);
+    grid_map::Position position = grid_map::Position((maxBound.x + minBound.x)/2.0, (maxBound.y + minBound.y)/2.0);
+    gridMap.setGeometry(length, resolution, position);
+
+    return true;
+}
+
+bool GridMapPclConverter::addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, const std::string& layer,
+                                                  grid_map::GridMap& gridMap) {
+    const Eigen::Vector3f ray(0.0,0.0,-1.0);
+
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+    pcl::fromPCLPointCloud2(mesh.cloud, cloud);
+    pcl::PointXYZ minBound;
+    pcl::PointXYZ maxBound;
+    pcl::getMinMax3D(cloud, minBound, maxBound);
+
+    gridMap.add(layer);
+    grid_map::Matrix& data = gridMap[layer];
+
+    for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
+        const Index index(*iterator);
+        grid_map::Position vertexPositionXY;
+        gridMap.getPosition(index, vertexPositionXY);
+
+        pcl::PointXYZ point;
+        point.x = vertexPositionXY.x();
+        point.y = vertexPositionXY.y();
+        point.z = maxBound.z + 1.0;
+
+        std::vector<double> candidatePoints;
+        for (unsigned i = 0; i < mesh.polygons.size(); ++i)
+        {
+            pcl::PointXYZ intersectionPoint;
+            if (rayTriangleIntersect(point, ray, mesh.polygons[i], cloud, intersectionPoint))
+                candidatePoints.push_back(intersectionPoint.z);
+        }
+        if (candidatePoints.size() > 0)
+        {
+            gridMap.at(layer, index) = *(std::max_element(candidatePoints.begin(), candidatePoints.end()));
+        }
+        else
+            gridMap.at(layer, index) = NAN;
+    }
+
+    return true;
+}
+
+bool GridMapPclConverter::rayTriangleIntersect (const pcl::PointXYZ& point,
+                                                const Eigen::Vector3f& ray,
+                                                const pcl::Vertices& verts,
+                                                const pcl::PointCloud<pcl::PointXYZ>& cloud,
+                                                pcl::PointXYZ& intersectionPoint) {
+    // Algorithm here is adapted from:
+    // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
+    //
+    // Original copyright notice:
+    // Copyright 2001, softSurfer (www.softsurfer.com)
+    // This code may be freely used and modified for any purpose
+    // providing that this copyright notice is included with it.
+    //
+    assert (verts.vertices.size () == 3);
+
+    const Eigen::Vector3f p = point.getVector3fMap ();
+    const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
+    const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
+    const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
+    const Eigen::Vector3f u = b - a;
+    const Eigen::Vector3f v = c - a;
+    const Eigen::Vector3f n = u.cross (v);
+    const float n_dot_ray = n.dot (ray);
+
+    if (std::fabs (n_dot_ray) < 1e-9)
+        return false;
+
+    const float r = n.dot (a - p) / n_dot_ray;
+
+    if (r < 0)
+        return false;
+
+    const Eigen::Vector3f w = p + r * ray - a;
+    const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v);
+    const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u);
+    const float s = s_numerator / denominator;
+    if (s < 0 || s > 1)
+        return false;
+
+    const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v);
+    const float t = t_numerator / denominator;
+    if (t < 0 || s+t > 1)
+        return false;
+
+    Eigen::Vector3f intersecPoint = a + s*u + t*v;
+
+    intersectionPoint.x = intersecPoint.x();
+    intersectionPoint.y = intersecPoint.y();
+    intersectionPoint.z = intersecPoint.z();
+
+    return true;
+}
+
+
+} /* namespace */