From c45c300ce3930312dcb4deadc811ccb4eaeb4bd0 Mon Sep 17 00:00:00 2001
From: Jaeyoung Lim <jalim@ethz.ch>
Date: Mon, 12 Sep 2022 14:27:08 +0200
Subject: [PATCH] Add method for loading a color layer from a textured mesh

This commit extends the gird map pcl coverter for loading a color layer from a textured mesh
---
 .../grid_map_pcl/GridMapPclConverter.hpp      | 10 ++++
 grid_map_pcl/src/GridMapPclConverter.cpp      | 60 +++++++++++++++++++
 2 files changed, 70 insertions(+)

diff --git a/grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp b/grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp
index 828b12442..9dd2e2a11 100644
--- a/grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp
+++ b/grid_map_pcl/include/grid_map_pcl/GridMapPclConverter.hpp
@@ -60,6 +60,16 @@ class GridMapPclConverter {
    */
   static bool addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, const std::string& layer, grid_map::GridMap& gridMap);
 
+  /*!
+   * Adds a color 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 addColorLayerFromPolygonMesh(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);
diff --git a/grid_map_pcl/src/GridMapPclConverter.cpp b/grid_map_pcl/src/GridMapPclConverter.cpp
index eb55b3778..1dbd78233 100644
--- a/grid_map_pcl/src/GridMapPclConverter.cpp
+++ b/grid_map_pcl/src/GridMapPclConverter.cpp
@@ -80,6 +80,66 @@ bool GridMapPclConverter::addLayerFromPolygonMesh(const pcl::PolygonMesh& mesh,
   return true;
 }
 
+bool GridMapPclConverter::addColorLayerFromPolygonMesh(const pcl::PolygonMesh& mesh, const std::string& layer, grid_map::GridMap& gridMap) {
+  // Adding a layer to the grid map to put data into
+  gridMap.add(layer);
+  // Converting out of binary cloud data
+  pcl::PointCloud<pcl::PointXYZRGB> cloud;
+  pcl::fromPCLPointCloud2(mesh.cloud, cloud);
+  // Direction and max height for projection ray
+  const Eigen::Vector3f ray = -Eigen::Vector3f::UnitZ();
+  pcl::PointXYZRGB minBound;
+  pcl::PointXYZRGB maxBound;
+  pcl::getMinMax3D(cloud, minBound, maxBound);
+
+  // Iterating over the triangles in the mesh
+  for (const pcl::Vertices& polygon : mesh.polygons) {
+    // Testing this is a triangle
+    assert(polygon.vertices.size() == 3);
+    // Getting the vertices of the triangle (as a single matrix)
+    Eigen::Matrix3f triangleVertexMatrix;
+    triangleVertexMatrix.row(0) = cloud[polygon.vertices[0]].getVector3fMap();
+    triangleVertexMatrix.row(1) = cloud[polygon.vertices[1]].getVector3fMap();
+    triangleVertexMatrix.row(2) = cloud[polygon.vertices[2]].getVector3fMap();
+
+    Eigen::Matrix3i triangleColorMatrix;
+    triangleColorMatrix.row(0) = cloud[polygon.vertices[0]].getRGBVector3i();
+    triangleColorMatrix.row(1) = cloud[polygon.vertices[1]].getRGBVector3i();
+    triangleColorMatrix.row(2) = cloud[polygon.vertices[2]].getRGBVector3i();
+
+    Eigen::Vector3i color_vector = triangleColorMatrix.colwise().mean();
+    // Getting the bounds in the XY plane (down projection)
+    float maxX = triangleVertexMatrix.col(0).maxCoeff();
+    float minX = triangleVertexMatrix.col(0).minCoeff();
+    float maxY = triangleVertexMatrix.col(1).maxCoeff();
+    float minY = triangleVertexMatrix.col(1).minCoeff();
+    // Iterating over the grid cells in the a submap below the triangle
+    grid_map::Length length(maxX - minX, maxY - minY);
+    grid_map::Position position((maxX + minX) / 2.0, (maxY + minY) / 2.0);
+    bool isSuccess;
+    SubmapGeometry submap(gridMap, position, length, isSuccess);
+    if (isSuccess) {
+      for (grid_map::SubmapIterator iterator(submap); !iterator.isPastEnd(); ++iterator) {
+        // Cell position
+        const Index index(*iterator);
+        grid_map::Position vertexPositionXY;
+        gridMap.getPosition(index, vertexPositionXY);
+        // Ray origin
+        Eigen::Vector3f point(vertexPositionXY.x(), vertexPositionXY.y(), maxBound.z + 1.0);
+        // Vertical ray/triangle intersection
+        Eigen::Vector3f intersectionPoint;
+        if (rayTriangleIntersect(point, ray, triangleVertexMatrix, intersectionPoint)) {
+          float color_value;
+          grid_map::colorVectorToValue(color_vector, color_value);
+          gridMap.at(layer, index) = color_value;
+        }
+      }
+    }
+  }
+  // Success
+  return true;
+}
+
 bool GridMapPclConverter::rayTriangleIntersect(const Eigen::Vector3f& point, const Eigen::Vector3f& ray,
                                                const Eigen::Matrix3f& triangleVertexMatrix, Eigen::Vector3f& intersectionPoint) {
   // Algorithm here is adapted from: