GridMapPclConverter.cpp
Go to the documentation of this file.
00001 /*
00002  * GridMapPclConverter.cpp
00003  *
00004  *  Created on: Oct 19, 2016
00005  *      Author: Dominic Jud
00006  *   Institute: ETH Zurich, ANYbotics
00007  */
00008 
00009 #include "grid_map_pcl/GridMapPclConverter.hpp"
00010 
00011 namespace grid_map {
00012 
00013 GridMapPclConverter::GridMapPclConverter()
00014 {
00015 }
00016 
00017 GridMapPclConverter::~GridMapPclConverter()
00018 {
00019 }
00020 
00021 bool GridMapPclConverter::initializeFromPolygonMesh(const pcl::PolygonMesh& mesh,
00022                                                     const double resolution,
00023                                                     grid_map::GridMap& gridMap)
00024 {
00025   pcl::PointCloud < pcl::PointXYZ > cloud;
00026   pcl::fromPCLPointCloud2(mesh.cloud, cloud);
00027   pcl::PointXYZ minBound;
00028   pcl::PointXYZ maxBound;
00029   pcl::getMinMax3D(cloud, minBound, maxBound);
00030 
00031   grid_map::Length length = grid_map::Length(maxBound.x - minBound.x, maxBound.y - minBound.y);
00032   grid_map::Position position = grid_map::Position((maxBound.x + minBound.x) / 2.0,
00033                                                    (maxBound.y + minBound.y) / 2.0);
00034   gridMap.setGeometry(length, resolution, position);
00035 
00036   return true;
00037 }
00038 
00039 bool GridMapPclConverter::addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh,
00040                                                   const std::string& layer,
00041                                                   grid_map::GridMap& gridMap)
00042 {
00043   // Adding a layer to the grid map to put data into
00044   gridMap.add(layer);
00045   // Converting out of binary cloud data
00046   pcl::PointCloud <pcl::PointXYZ> cloud;
00047   pcl::fromPCLPointCloud2(mesh.cloud, cloud);
00048   // Direction and max height for projection ray
00049   const Eigen::Vector3f ray = -Eigen::Vector3f::UnitZ();
00050   pcl::PointXYZ minBound;
00051   pcl::PointXYZ maxBound;
00052   pcl::getMinMax3D(cloud, minBound, maxBound);
00053 
00054   // Iterating over the triangles in the mesh
00055   for (const pcl::Vertices& polygon : mesh.polygons) {
00056     // Testing this is a triangle
00057     assert(polygon.vertices.size() == 3);
00058     // Getting the vertices of the triangle (as a single matrix)
00059     Eigen::Matrix3f triangleVertexMatrix;
00060     triangleVertexMatrix.row(0) = cloud[polygon.vertices[0]].getVector3fMap();
00061     triangleVertexMatrix.row(1) = cloud[polygon.vertices[1]].getVector3fMap();
00062     triangleVertexMatrix.row(2) = cloud[polygon.vertices[2]].getVector3fMap();
00063     // Getting the bounds in the XY plane (down projection)
00064     float maxX = triangleVertexMatrix.col(0).maxCoeff();
00065     float minX = triangleVertexMatrix.col(0).minCoeff();
00066     float maxY = triangleVertexMatrix.col(1).maxCoeff();
00067     float minY = triangleVertexMatrix.col(1).minCoeff();
00068     // Iterating over the grid cells in the a submap below the triangle
00069     grid_map::Length length(maxX - minX, maxY - minY);
00070     grid_map::Position position((maxX + minX) / 2.0, (maxY + minY) / 2.0);
00071     bool isSuccess;
00072     SubmapGeometry submap(gridMap, position, length, isSuccess);
00073     if (isSuccess) {
00074       for (grid_map::SubmapIterator iterator(submap); !iterator.isPastEnd();
00075            ++iterator) {
00076         // Cell position
00077         const Index index(*iterator);
00078         grid_map::Position vertexPositionXY;
00079         gridMap.getPosition(index, vertexPositionXY);
00080         // Ray origin
00081         Eigen::Vector3f point(vertexPositionXY.x(), vertexPositionXY.y(),
00082                               maxBound.z + 1.0);
00083         // Vertical ray/triangle intersection
00084         Eigen::Vector3f intersectionPoint;
00085         if (rayTriangleIntersect(point, ray, triangleVertexMatrix, intersectionPoint)) {
00086           // If data already present in this cell, taking the max, else setting the data
00087           if (gridMap.isValid(index, layer)) {
00088             gridMap.at(layer, index) = std::max(gridMap.at(layer, index), intersectionPoint.z());
00089           } else {
00090             gridMap.at(layer, index) = intersectionPoint.z();
00091           }
00092         }
00093       }
00094     }
00095   }
00096   // Success
00097   return true;
00098 }
00099 
00100 bool GridMapPclConverter::rayTriangleIntersect(
00101     const Eigen::Vector3f& point, const Eigen::Vector3f& ray,
00102     const Eigen::Matrix3f& triangleVertexMatrix,
00103     Eigen::Vector3f& intersectionPoint) {
00104   // Algorithm here is adapted from:
00105   // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
00106   //
00107   // Original copyright notice:
00108   // Copyright 2001, softSurfer (www.softsurfer.com)
00109   // This code may be freely used and modified for any purpose
00110   // providing that this copyright notice is included with it.
00111 
00112   const Eigen::Vector3f a = triangleVertexMatrix.row(0);
00113   const Eigen::Vector3f b = triangleVertexMatrix.row(1);
00114   const Eigen::Vector3f c = triangleVertexMatrix.row(2);
00115   const Eigen::Vector3f u = b - a;
00116   const Eigen::Vector3f v = c - a;
00117   const Eigen::Vector3f n = u.cross(v);
00118   const float n_dot_ray = n.dot(ray);
00119 
00120   if (std::fabs(n_dot_ray) < 1e-9) return false;
00121 
00122   const float r = n.dot(a - point) / n_dot_ray;
00123 
00124   if (r < 0) return false;
00125 
00126   // Note(alexmillane): The addition of this comparison delta (not in the
00127   // original algorithm) means that rays intersecting the edge of triangles are
00128   // treated at hits.
00129   constexpr float delta = 1e-5;
00130 
00131   const Eigen::Vector3f w = point + r * ray - a;
00132   const float denominator = u.dot(v) * u.dot(v) - u.dot(u) * v.dot(v);
00133   const float s_numerator = u.dot(v) * w.dot(v) - v.dot(v) * w.dot(u);
00134   const float s = s_numerator / denominator;
00135   if (s < (0 - delta) || s > (1 + delta)) return false;
00136 
00137   const float t_numerator = u.dot(v) * w.dot(u) - u.dot(u) * w.dot(v);
00138   const float t = t_numerator / denominator;
00139   if (t < (0 - delta) || s + t > (1 + delta)) return false;
00140 
00141   intersectionPoint = a + s * u + t * v;
00142 
00143   return true;
00144 }
00145 
00146 } /* namespace */


grid_map_pcl
Author(s): Dominic Jud
autogenerated on Tue Jul 9 2019 05:06:29