feat: add plane_segmentation (ROS 2) packages

Author: Idate96

plane_segmentation/convex_plane_decomposition/CMakeLists.txt

@@ -0,0 +1,101 @@
+cmake_minimum_required(VERSION 3.10)
+project(convex_plane_decomposition)
+
+find_package(ament_cmake REQUIRED)
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
+find_package(OpenCV REQUIRED)
+
+find_package(CGAL REQUIRED)
+find_package(GMP REQUIRED)
+find_package(MPFR REQUIRED)
+
+find_package(grid_map_core REQUIRED)
+find_package(grid_map_cv REQUIRED)
+find_package(grid_map_filters_rsl REQUIRED)
+
+###########
+## Build ##
+###########
+
+add_library(${PROJECT_NAME}
+  src/contour_extraction/ContourExtraction.cpp
+  src/contour_extraction/Upsampling.cpp
+  src/ransac/RansacPlaneExtractor.cpp
+  src/sliding_window_plane_extraction/SlidingWindowPlaneExtractor.cpp
+  src/ConvexRegionGrowing.cpp
+  src/Draw.cpp
+  src/GridMapPreprocessing.cpp
+  src/LoadGridmapFromImage.cpp
+  src/PlanarRegion.cpp
+  src/PlaneDecompositionPipeline.cpp
+  src/Postprocessing.cpp
+  src/SegmentedPlaneProjection.cpp
+)
+
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+target_compile_definitions(${PROJECT_NAME} PUBLIC CGAL_HAS_THREADS)
+
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  grid_map_core
+  grid_map_cv
+  grid_map_filters_rsl
+)
+target_link_libraries(${PROJECT_NAME}
+  Eigen3::Eigen
+  ${OpenCV_LIBRARIES}
+  CGAL::CGAL
+  gmp
+  mpfr
+)
+
+#############
+## Install ##
+#############
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include/${PROJECT_NAME}
+)
+install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
+
+ament_export_include_directories("include/${PROJECT_NAME}")
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  CGAL
+  Eigen3
+  grid_map_core
+  grid_map_cv
+  grid_map_filters_rsl
+)
+ament_package()
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(Boost REQUIRED COMPONENTS filesystem system)
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/testConvexApproximation.cpp
+    test/testPipeline.cpp
+    test/testPlanarRegion.cpp
+    test/testRegionGrowing.cpp
+    test/testUpsampling.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}
+    Boost::filesystem
+    Boost::system
+  )
+endif()

plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/ConvexRegionGrowing.h

@@ -0,0 +1,20 @@
+//
+// Created by rgrandia on 09.06.20.
+//
+
+#pragma once
+
+#include "PolygonTypes.h"
+
+namespace convex_plane_decomposition {
+
+CgalPolygon2d createRegularPolygon(const CgalPoint2d& center, double radius, int numberOfVertices);
+
+CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygon2d& parentShape, CgalPoint2d center, int numberOfVertices, double growthFactor);
+
+CgalPolygon2d growConvexPolygonInsideShape(const CgalPolygonWithHoles2d& parentShape, CgalPoint2d center, int numberOfVertices,
+                                           double growthFactor);
+
+void updateMean(CgalPoint2d& mean, const CgalPoint2d& oldValue, const CgalPoint2d& updatedValue, int N);
+
+}  // namespace convex_plane_decomposition

plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/Draw.h

@@ -0,0 +1,27 @@
+//
+// Created by rgrandia on 09.06.20.
+//
+
+#pragma once
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include "opencv2/imgproc/imgproc.hpp"
+
+#include "PolygonTypes.h"
+
+namespace convex_plane_decomposition {
+
+cv::Vec3b randomColor();
+
+std::vector<cv::Point> toCv(const CgalPolygon2d& polygon);
+
+void drawContour(cv::Mat& img, const CgalPoint2d& point, double radius = 1, const cv::Vec3b* color = nullptr);
+void drawContour(cv::Mat& img, const CgalPolygon2d& polygon, const cv::Vec3b* color = nullptr);
+void drawContour(cv::Mat& img, const CgalPolygonWithHoles2d& polygonWithHoles2d, const cv::Vec3b* color = nullptr);
+
+CgalPolygon2d scaleShape(const CgalPolygon2d& polygon, double scale);
+CgalPolygonWithHoles2d scaleShape(const CgalPolygonWithHoles2d& polygonWithHoles, double scale);
+
+}  // namespace convex_plane_decomposition

plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GeometryUtils.h

@@ -0,0 +1,140 @@
+//
+// Created by rgrandia on 09.06.20.
+//
+
+#pragma once
+
+#include "PlanarRegion.h"
+#include "PolygonTypes.h"
+
+namespace convex_plane_decomposition {
+
+inline bool doEdgesIntersect(const CgalSegment2d& line, const CgalPolygon2d& contour) {
+  for (auto edgeIt = contour.edges_begin(); edgeIt != contour.edges_end(); ++edgeIt) {
+    if (CGAL::do_intersect(line, *edgeIt)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+inline bool doEdgesIntersect(const CgalSegment2d& line, const CgalPolygonWithHoles2d& parentShape) {
+  if (doEdgesIntersect(line, parentShape.outer_boundary())) {
+    return true;
+  } else {
+    for (const auto& hole : parentShape.holes()) {
+      if (doEdgesIntersect(line, hole)) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+inline double squaredDistance(const CgalPoint2d& point, const CgalPolygon2d& polygon) {
+  double minDistSquared = std::numeric_limits<double>::max();
+  for (auto edgeIt = polygon.edges_begin(); edgeIt != polygon.edges_end(); ++edgeIt) {
+    double distSquare = CGAL::squared_distance(point, *edgeIt);
+    minDistSquared = std::min(distSquare, minDistSquared);
+  }
+  return minDistSquared;
+}
+
+inline double squaredDistance(const CgalPoint2d& point, const CgalPolygonWithHoles2d& parentShape) {
+  double minDistSquared = squaredDistance(point, parentShape.outer_boundary());
+  for (const auto& hole : parentShape.holes()) {
+    double distSquare = squaredDistance(point, hole);
+    minDistSquared = std::min(distSquare, minDistSquared);
+  }
+  return minDistSquared;
+}
+
+inline double squaredDistance(const CgalPoint2d& point, const CgalCircle2d& circle) {
+  auto dx = (point.x() - circle.center().x());
+  auto dy = (point.y() - circle.center().y());
+  return dx * dx + dy * dy;
+}
+
+template <typename T>
+double distance(const CgalPoint2d& point, const T& shape) {
+  double distanceSquared = squaredDistance(point, shape);
+  return (distanceSquared > 0.0) ? std::sqrt(distanceSquared) : 0.0;
+}
+
+inline bool isInside(const CgalPoint2d& point, const CgalCircle2d& circle) {
+  return squaredDistance(point, circle) <= circle.squared_radius();
+}
+
+inline bool isInside(const CgalPoint2d& point, const CgalPolygon2d& polygon) {
+  const auto boundedSide = CGAL::bounded_side_2(polygon.begin(), polygon.end(), point);
+  return boundedSide == CGAL::ON_BOUNDED_SIDE || boundedSide == CGAL::ON_BOUNDARY;
+}
+
+inline bool isInside(const CgalPoint2d& point, const CgalPolygonWithHoles2d& polygonWithHoles) {
+  if (isInside(point, polygonWithHoles.outer_boundary())) {
+    // Inside the outer contour -> return false if the point is inside any of the holes
+    for (const auto& hole : polygonWithHoles.holes()) {
+      const auto boundedSide = CGAL::bounded_side_2(hole.begin(), hole.end(), point);
+      if (boundedSide == CGAL::ON_BOUNDED_SIDE) {  // The edge of the hole is considered part of the polygon
+        return false;
+      }
+    }
+    return true;
+  } else {
+    return false;
+  }
+}
+
+inline CgalPoint2d getPointOnLine(const CgalPoint2d& start, const CgalPoint2d& end, double factor) {
+  return {factor * (end.x() - start.x()) + start.x(), factor * (end.y() - start.y()) + start.y()};
+}
+
+inline CgalPoint2d projectToClosestPoint(const CgalPoint2d& point, const CgalSegment2d& segment) {
+  // The segment as a vector, with the source being the origin
+  const Eigen::Vector2d sourceToTarget{segment.target().x() - segment.source().x(), segment.target().y() - segment.source().y()};
+  const double sourceToTargetDistance = sourceToTarget.norm();
+  const Eigen::Vector2d n = sourceToTarget / sourceToTargetDistance;
+
+  // Vector from source to the query point
+  const Eigen::Vector2d sourceToPoint{point.x() - segment.source().x(), point.y() - segment.source().y()};
+
+  // Projection to the line, clamped to be between source and target points
+  const double coeff = std::min(std::max(0.0, n.dot(sourceToPoint)), sourceToTargetDistance);
+
+  return {coeff * n.x() + segment.source().x(), coeff * n.y() + segment.source().y()};
+}
+
+inline CgalPoint2d projectToClosestPoint(const CgalPoint2d& point, const CgalPolygon2d& polygon) {
+  double minDistSquared = CGAL::squared_distance(point, *polygon.edges_begin());
+  auto closestEdge = polygon.edges_begin();
+  for (auto edgeIt = std::next(polygon.edges_begin()); edgeIt != polygon.edges_end(); ++edgeIt) {
+    double distSquare = CGAL::squared_distance(point, *edgeIt);
+    if (distSquare < minDistSquared) {
+      closestEdge = edgeIt;
+      minDistSquared = distSquare;
+    }
+  }
+  return projectToClosestPoint(point, *closestEdge);
+}
+
+inline void transformInPlace(CgalPolygon2d& polygon, const std::function<void(CgalPoint2d&)>& f) {
+  for (auto& point : polygon) {
+    f(point);
+  }
+}
+
+inline void transformInPlace(CgalPolygonWithHoles2d& polygonWithHoles, const std::function<void(CgalPoint2d&)>& f) {
+  transformInPlace(polygonWithHoles.outer_boundary(), f);
+  for (auto& hole : polygonWithHoles.holes()) {
+    transformInPlace(hole, f);
+  }
+}
+
+inline void transformInPlace(BoundaryWithInset& boundaryWithInset, const std::function<void(CgalPoint2d&)>& f) {
+  transformInPlace(boundaryWithInset.boundary, f);
+  for (auto& inset : boundaryWithInset.insets) {
+    transformInPlace(inset, f);
+  }
+}
+
+}  // namespace convex_plane_decomposition

plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/GridMapPreprocessing.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <grid_map_core/GridMap.hpp>
+
+namespace convex_plane_decomposition {
+
+struct PreprocessingParameters {
+  /// Resample to this resolution, set to negative values to skip
+  double resolution = 0.04;
+  /// Kernel size of the median filter, either 3 or 5
+  int kernelSize = 3;
+  /// Number of times the image is filtered
+  int numberOfRepeats = 2;
+};
+
+class GridMapPreprocessing {
+ public:
+  GridMapPreprocessing(const PreprocessingParameters& parameters);
+
+  void preprocess(grid_map::GridMap& gridMap, const std::string& layer) const;
+
+ private:
+  void denoise(grid_map::GridMap& gridMap, const std::string& layer) const;
+  void changeResolution(grid_map::GridMap& gridMap, const std::string& layer) const;
+  void inpaint(grid_map::GridMap& gridMap, const std::string& layer) const;
+
+  PreprocessingParameters parameters_;
+};
+
+/**
+ * @return true if any of the elements in the map are finite
+ */
+bool containsFiniteValue(const grid_map::Matrix& map);
+
+}  // namespace convex_plane_decomposition

plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/LoadGridmapFromImage.h

@@ -0,0 +1,26 @@
+//
+// Created by rgrandia on 16.03.22.
+//
+
+#pragma once
+
+#include <string>
+
+#include <grid_map_core/GridMap.hpp>
+
+namespace convex_plane_decomposition {
+
+/**
+ * Load an elevation map from a grey scale image.
+ *
+ * @param filePath : absolute path to file
+ * @param elevationLayer : name of the elevation layer
+ * @param frameId : frame assigned to loaded map
+ * @param resolution : map resolution [m/pixel]
+ * @param scale : distance [m] between lowest and highest point in the map
+ * @return Gridmap with the loaded image as elevation layer.
+ */
+grid_map::GridMap loadGridmapFromImage(const std::string& filePath, const std::string& elevationLayer, const std::string& frameId,
+                                       double resolution, double scale);
+
+}  // namespace convex_plane_decomposition