Merge branch 'calderpg:master' into master

CMakeLists.txt.ros1

@@ -96,11 +96,13 @@ set(CMAKE_SHARED_LINKER_FLAGS
 add_library(${PROJECT_NAME}
             include/${PROJECT_NAME}/collision_map.hpp
             include/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.hpp
+            include/${PROJECT_NAME}/mesh_rasterizer.hpp
             include/${PROJECT_NAME}/signed_distance_field.hpp
             include/${PROJECT_NAME}/signed_distance_field_generation.hpp
             include/${PROJECT_NAME}/tagged_object_collision_map.hpp
             include/${PROJECT_NAME}/topology_computation.hpp
             src/${PROJECT_NAME}/collision_map.cpp
+            src/${PROJECT_NAME}/mesh_rasterizer.cpp
             src/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.cpp
             src/${PROJECT_NAME}/tagged_object_collision_map.cpp)
 add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
@@ -223,6 +225,10 @@ if(CATKIN_ENABLE_TESTING)
         ${PROJECT_NAME}_pointcloud_voxelization)
     target_link_libraries(pointcloud_voxelization_test
         ${PROJECT_NAME}_pointcloud_voxelization)
+
+    catkin_add_gtest(mesh_rasterization_test test/mesh_rasterization_test.cpp)
+    add_dependencies(mesh_rasterization_test ${PROJECT_NAME})
+    target_link_libraries(mesh_rasterization_test ${PROJECT_NAME})
 endif()
 
 #############

CMakeLists.txt.ros2

@@ -70,11 +70,13 @@ set(CMAKE_SHARED_LINKER_FLAGS
 add_library(${PROJECT_NAME}
             include/${PROJECT_NAME}/collision_map.hpp
             include/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.hpp
+            include/${PROJECT_NAME}/mesh_rasterizer.hpp
             include/${PROJECT_NAME}/signed_distance_field.hpp
             include/${PROJECT_NAME}/signed_distance_field_generation.hpp
             include/${PROJECT_NAME}/tagged_object_collision_map.hpp
             include/${PROJECT_NAME}/topology_computation.hpp
             src/${PROJECT_NAME}/collision_map.cpp
+            src/${PROJECT_NAME}/mesh_rasterizer.cpp
             src/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.cpp
             src/${PROJECT_NAME}/tagged_object_collision_map.cpp)
 ament_target_dependencies(${PROJECT_NAME} common_robotics_utilities)
@@ -209,6 +211,9 @@ if(BUILD_TESTING)
         test/pointcloud_voxelization_test.cpp)
     target_link_libraries(pointcloud_voxelization_test
         ${PROJECT_NAME}_pointcloud_voxelization)
+
+    ament_add_gtest(mesh_rasterization_test test/mesh_rasterization_test.cpp)
+    target_link_libraries(mesh_rasterization_test ${PROJECT_NAME})
 endif()
 
 #############

include/voxelized_geometry_tools/mesh_rasterizer.hpp

@@ -0,0 +1,45 @@
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Geometry>
+#include <common_robotics_utilities/math.hpp>
+#include <common_robotics_utilities/voxel_grid.hpp>
+#include <voxelized_geometry_tools/collision_map.hpp>
+
+// TODO(calderpg) Factor this out to support different voxel grid types.
+namespace voxelized_geometry_tools
+{
+/// This sets all voxels intersected by the provided triangle to filled. Note
+/// that it sets voxels filled regardless of whether or not the voxel cell
+/// center is "inside" or "outside" the triangle.
+/// Note: this uses a conservative approximation of triangle-box collision that
+/// will identify some extraneous voxels as colliding. For its use so far, this
+/// has better than adding too few, since we generally want a watertight layer
+/// of surface voxels.
+void RasterizeTriangle(
+    const std::vector<Eigen::Vector3d>& vertices,
+    const std::vector<Eigen::Vector3i>& triangles,
+    const size_t triangle_index,
+    voxelized_geometry_tools::CollisionMap& collision_map,
+    const bool enforce_collision_map_contains_triangle = true);
+
+/// This sets all voxels intersected by the provided mesh to filled. Note
+/// that it sets voxels filled regardless of whether or not the voxel cell
+/// center is "inside" or "outside" the intersecting triangle(s) of the mesh.
+void RasterizeMesh(
+    const std::vector<Eigen::Vector3d>& vertices,
+    const std::vector<Eigen::Vector3i>& triangles,
+    voxelized_geometry_tools::CollisionMap& collision_map,
+    const bool enforce_collision_map_contains_mesh = true);
+
+/// This sets all voxels intersected by the provided mesh to filled. Note
+/// that it sets voxels filled regardless of whether or not the voxel cell
+/// center is "inside" or "outside" the intersecting triangle(s) of the mesh.
+/// The generated CollisionMap will fully contain the provided mesh.
+voxelized_geometry_tools::CollisionMap RasterizeMeshIntoCollisionMap(
+    const std::vector<Eigen::Vector3d>& vertices,
+    const std::vector<Eigen::Vector3i>& triangles,
+    const double resolution);
+}  // namespace voxelized_geometry_tools
+

src/voxelized_geometry_tools/mesh_rasterizer.cpp

@@ -0,0 +1,261 @@
+#include <voxelized_geometry_tools/mesh_rasterizer.hpp>
+
+#include <algorithm>
+#include <limits>
+#include <stdexcept>
+#include <vector>
+
+#include <Eigen/Geometry>
+#include <common_robotics_utilities/math.hpp>
+#include <common_robotics_utilities/voxel_grid.hpp>
+#if defined(_OPENMP)
+#include <omp.h>
+#endif
+#include <voxelized_geometry_tools/collision_map.hpp>
+
+namespace voxelized_geometry_tools
+{
+namespace
+{
+bool PointProjectsInsideTriangle(
+    const Eigen::Vector3d& p_Mv1, const Eigen::Vector3d& p_Mv2,
+    const Eigen::Vector3d& p_Mv3, const Eigen::Vector3d& p_MQ)
+{
+  const auto same_side = [] (
+      const Eigen::Vector3d& p_MA, const Eigen::Vector3d& p_MB,
+      const Eigen::Vector3d& p_Mp1, const Eigen::Vector3d& p_Mp2) -> bool
+  {
+    const Eigen::Vector3d v_AB = p_MB - p_MA;
+    const Eigen::Vector3d cross1 = v_AB.cross(p_Mp1 - p_MA);
+    const Eigen::Vector3d cross2 = v_AB.cross(p_Mp2 - p_MA);
+    return (cross1.dot(cross2) >= 0.0);
+  };
+
+  return (same_side(p_Mv1, p_Mv2, p_Mv3, p_MQ) &&
+          same_side(p_Mv2, p_Mv3, p_Mv1, p_MQ) &&
+          same_side(p_Mv3, p_Mv1, p_Mv2, p_MQ));
+}
+
+Eigen::Vector3d ClosestPointOnLineSegment(
+    const Eigen::Vector3d& p_MA, const Eigen::Vector3d& p_MB,
+    const Eigen::Vector3d& p_MQ)
+{
+  const Eigen::Vector3d v_AB = p_MB - p_MA;
+  const Eigen::Vector3d v_AQ = p_MQ - p_MA;
+
+  const double ratio = v_AB.dot(v_AQ) / v_AB.squaredNorm();
+  const double clamped =
+      common_robotics_utilities::utility::ClampValue(ratio, 0.0, 1.0);
+
+  return p_MA + (v_AB * clamped);
+}
+
+Eigen::Vector3d CalcClosestPointOnTriangle(
+    const Eigen::Vector3d& p_Mv1, const Eigen::Vector3d& p_Mv2,
+    const Eigen::Vector3d& p_Mv3, const Eigen::Vector3d& normal,
+    const Eigen::Vector3d& p_MQ)
+{
+  Eigen::Vector3d p_MQclosest;
+
+  if (PointProjectsInsideTriangle(p_Mv1, p_Mv2, p_Mv3, p_MQ))
+  {
+    const Eigen::Vector3d v_v1Q = p_MQ - p_Mv1;
+    // Project query point to triangle plane.
+    const Eigen::Vector3d p_MQprojected =
+        p_Mv1 + common_robotics_utilities::math::VectorRejection(normal, v_v1Q);
+    p_MQclosest = p_MQprojected;
+  } 
+  else
+  {
+    const Eigen::Vector3d p_MQclosest12 =
+        ClosestPointOnLineSegment(p_Mv1, p_Mv2, p_MQ);
+    const Eigen::Vector3d p_MQclosest23 =
+        ClosestPointOnLineSegment(p_Mv2, p_Mv3, p_MQ);
+    const Eigen::Vector3d p_MQclosest31 =
+        ClosestPointOnLineSegment(p_Mv3, p_Mv1, p_MQ);
+    const double d_closest12_squared = p_MQclosest12.squaredNorm();
+    const double d_closest23_squared = p_MQclosest23.squaredNorm();
+    const double d_closest31_squared = p_MQclosest31.squaredNorm();
+    if (d_closest12_squared <= d_closest23_squared &&
+        d_closest12_squared <= d_closest31_squared)
+    {
+      p_MQclosest = p_MQclosest12;
+    }
+    else if (d_closest23_squared <= d_closest12_squared &&
+             d_closest23_squared <= d_closest31_squared)
+    {
+      p_MQclosest = p_MQclosest23;
+    }
+    else
+    {
+      p_MQclosest = p_MQclosest31;
+    }
+  }
+
+  return p_MQclosest;
+}
+}  // namespace
+
+void RasterizeTriangle(
+    const std::vector<Eigen::Vector3d>& vertices,
+    const std::vector<Eigen::Vector3i>& triangles,
+    const size_t triangle_index,
+    voxelized_geometry_tools::CollisionMap& collision_map,
+    const bool enforce_collision_map_contains_triangle)
+{
+  if (!collision_map.IsInitialized())
+  {
+    throw std::invalid_argument("collision_map must be initialized");
+  }
+
+  const double min_check_radius = collision_map.GetResolution() * 0.5;
+  const double max_check_radius = min_check_radius * std::sqrt(3.0);
+  const double max_check_radius_squared = std::pow(max_check_radius, 2.0);
+
+  const Eigen::Vector3i& triangle = triangles.at(triangle_index);
+  const Eigen::Vector3d& p_Mv1 = vertices.at(triangle(0));
+  const Eigen::Vector3d& p_Mv2 = vertices.at(triangle(1));
+  const Eigen::Vector3d& p_Mv3 = vertices.at(triangle(2));
+
+  const Eigen::Vector3d v1v2 = p_Mv2 - p_Mv1;
+  const Eigen::Vector3d v1v3 = p_Mv3 - p_Mv1;
+
+  // This assumes that the triangle is well-conditioned. Long and skinny
+  // triangles may lead to problems.
+  const Eigen::Vector3d normal = v1v2.cross(v1v3);
+
+  const double x_min = std::min({p_Mv1.x(), p_Mv2.x(), p_Mv3.x()});
+  const double y_min = std::min({p_Mv1.y(), p_Mv2.y(), p_Mv3.y()});
+  const double z_min = std::min({p_Mv1.z(), p_Mv2.z(), p_Mv3.z()});
+
+  const double x_max = std::max({p_Mv1.x(), p_Mv2.x(), p_Mv3.x()});
+  const double y_max = std::max({p_Mv1.y(), p_Mv2.y(), p_Mv3.y()});
+  const double z_max = std::max({p_Mv1.z(), p_Mv2.z(), p_Mv3.z()});
+
+  const auto min_index =
+      collision_map.LocationToGridIndex(x_min, y_min, z_min);
+  const auto max_index =
+      collision_map.LocationToGridIndex(x_max, y_max, z_max);
+
+  for (int64_t x_index = min_index.X(); x_index <= max_index.X();
+       x_index++)
+  {
+    for (int64_t y_index = min_index.Y(); y_index <= max_index.Y();
+         y_index++)
+    {
+      for (int64_t z_index = min_index.Z(); z_index <= max_index.Z();
+           z_index++)
+      {
+        const common_robotics_utilities::voxel_grid::GridIndex
+            current_index(x_index, y_index, z_index);
+        const Eigen::Vector3d p_MQ =
+            collision_map.GridIndexToLocation(current_index).head<3>();
+
+        const Eigen::Vector3d p_MQclosest =
+            CalcClosestPointOnTriangle(p_Mv1, p_Mv2, p_Mv3, normal, p_MQ);
+
+        const double distance_squared = (p_MQclosest - p_MQ).squaredNorm();
+
+        /// TODO(calderpg) Improve this with more a accurate triangle-box check.
+        /// This is a coarse approximation of true triangle-box collision.
+        /// The right solution is something along the lines of:
+        ///
+        /// if distance_squared > max_check_radius_squared:
+        ///   triangle_intersects = false
+        /// else if distance_squared < min_check_radius_squared:
+        ///   triangle_intersects = true
+        /// else:
+        ///   check_index = collision_map.LocationToGridIndex3d(p_MQclosest)
+        ///   if check_index == current_index:
+        ///     triangle_intersects = true;
+        ///   else:
+        ///     Fall back to a better triangle-box check
+        ///
+        /// For now the following approximation is enough.
+        const bool triangle_intersects =
+            (distance_squared <= max_check_radius_squared);
+
+        if (triangle_intersects)
+        {
+          auto query = collision_map.GetMutable(current_index);
+          if (query)
+          {
+            query.Value().Occupancy() = 1.0f;
+          }
+          else if (enforce_collision_map_contains_triangle)
+          {
+            throw std::runtime_error(
+                "Triangle is not contained by collision map");
+          }
+        }
+      }
+    }
+  }
+}
+
+void RasterizeMesh(
+    const std::vector<Eigen::Vector3d>& vertices,
+    const std::vector<Eigen::Vector3i>& triangles,
+    voxelized_geometry_tools::CollisionMap& collision_map,
+    const bool enforce_collision_map_contains_mesh)
+{
+  if (!collision_map.IsInitialized())
+  {
+    throw std::invalid_argument("collision_map must be initialized");
+  }
+
+#if defined(_OPENMP)
+#pragma omp parallel for
+#endif
+  for (size_t idx = 0; idx < triangles.size(); idx++)
+  {
+    RasterizeTriangle(
+        vertices, triangles, idx, collision_map,
+        enforce_collision_map_contains_mesh);
+  }
+}
+
+voxelized_geometry_tools::CollisionMap RasterizeMeshIntoCollisionMap(
+    const std::vector<Eigen::Vector3d>& vertices,
+    const std::vector<Eigen::Vector3i>& triangles,
+    const double resolution)
+{
+  if (resolution <= 0.0)
+  {
+    throw std::invalid_argument("resolution must be greater than zero");
+  }
+
+  Eigen::Vector3d lower_corner =
+      Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
+  Eigen::Vector3d upper_corner =
+      Eigen::Vector3d::Constant(-std::numeric_limits<double>::infinity());
+
+  for (size_t idx = 0; idx < vertices.size(); idx++)
+  {
+    const Eigen::Vector3d& vertex = vertices.at(idx);
+    lower_corner = lower_corner.cwiseMin(vertex);
+    upper_corner = upper_corner.cwiseMax(vertex);
+  }
+
+  const Eigen::Vector3d object_size = upper_corner - lower_corner;
+
+  const double buffer_size = resolution * 2.0;
+  const common_robotics_utilities::voxel_grid::GridSizes filter_grid_sizes(
+      resolution, object_size.x() + buffer_size, object_size.y() + buffer_size,
+      object_size.z() + buffer_size);
+
+  const Eigen::Isometry3d X_OG(Eigen::Translation3d(
+      lower_corner.x() - resolution,
+      lower_corner.y() - resolution,
+      lower_corner.z() - resolution));
+
+  const voxelized_geometry_tools::CollisionCell empty_cell(0.0f);
+  voxelized_geometry_tools::CollisionMap collision_map(
+      X_OG, "mesh", filter_grid_sizes, empty_cell);
+
+  RasterizeMesh(vertices, triangles, collision_map);
+
+  return collision_map;
+}
+}  // namespace voxelized_geometry_tools
+