add plane projection and convex approximation to the demo

Author: rubengrandia

README.md

@@ -3,8 +3,10 @@
 ## Overview
 
 This is a ROS package for elevation mapping on GPU. The elevation mapping code is written in python and uses cupy for GPU computation. The
-plane segmentation is done independently and runs on CPU.
+plane segmentation is done independently and runs on CPU. When the plane segmentation is generated, local convex approximations of the
+terrain can be efficiently generated.
 ![screenshot](doc/main_repo.png)
+![gif](doc/convex_approximation.gif)
 
 ## Citing
 

doc/convex_approximation.gif

@@ -87,6 +87,15 @@ target_link_libraries(${PROJECT_NAME}_save_elevationmap
 	${OpenCV_LIBRARIES}
 	)
 
+add_executable(${PROJECT_NAME}_approximation_demo_node
+	src/ConvexApproximationDemoNode.cpp
+	)
+target_link_libraries(${PROJECT_NAME}_approximation_demo_node
+	${catkin_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PROJECT_NAME}
+	)
+
 #############
 ## Install ##
 #############

plane_segmentation/convex_plane_decomposition_ros/CMakeLists.txt

@@ -1,11 +1,19 @@
 #pragma once
 
+#include <geometry_msgs/PolygonStamped.h>
+
 #include <jsk_recognition_msgs/PolygonArray.h>
 
 #include <convex_plane_decomposition/PlanarRegion.h>
 
 namespace convex_plane_decomposition {
 
+geometry_msgs::PolygonStamped to3dRosPolygon(const CgalPolygon2d& polygon, const Eigen::Isometry3d& transformPlaneToWorld,
+                                   const std_msgs::Header& header);
+
+std::vector<geometry_msgs::PolygonStamped> to3dRosPolygon(const CgalPolygonWithHoles2d& polygonWithHoles,
+                                                const Eigen::Isometry3d& transformPlaneToWorld, const std_msgs::Header& header);
+
 jsk_recognition_msgs::PolygonArray convertBoundariesToRosPolygons(const std::vector<PlanarRegion>& planarRegions,
                                                                   const std::string& frameId, grid_map::Time time);
 

plane_segmentation/convex_plane_decomposition_ros/include/convex_plane_decomposition_ros/RosVisualizations.h

@@ -38,6 +38,10 @@
     <arg name="node_parameter_file"    value="$(find convex_plane_decomposition_ros)/config/demo_node.yaml"/>
   </include>
 
+  <!-- Launch the approximation demo. -->
+  <node pkg="convex_plane_decomposition_ros" type="convex_plane_decomposition_ros_approximation_demo_node" name="convex_plane_decomposition_ros_approximation_demo_node"
+      output="screen" launch-prefix=""/>
+
   <!-- Launch rviz. -->
   <node type="rviz" name="rviz" respawn="true" pkg="rviz" args="-d $(find convex_plane_decomposition_ros)/rviz/config_demo.rviz" />
 

plane_segmentation/convex_plane_decomposition_ros/launch/demo.launch

@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
-      Splitter Ratio: 0.6307384967803955
+      Splitter Ratio: 0.45271122455596924
     Tree Height: 1658
   - Class: rviz/Selection
     Name: Selection
@@ -107,6 +107,37 @@ Visualization Manager:
       Unreliable: false
       Use Rainbow: false
       Value: false
+    - Alpha: 1
+      Class: rviz/PointStamped
+      Color: 204; 41; 204
+      Enabled: true
+      History Length: 1
+      Name: Query
+      Queue Size: 10
+      Radius: 0.10000000149011612
+      Topic: /convex_plane_decomposition_ros_approximation_demo_node/queryPosition
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/PointStamped
+      Color: 245; 121; 0
+      Enabled: true
+      History Length: 1
+      Name: Projection
+      Queue Size: 10
+      Radius: 0.10000000149011612
+      Topic: /convex_plane_decomposition_ros_approximation_demo_node/projectedQueryPosition
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 245; 121; 0
+      Enabled: true
+      Name: ConvexApproximation
+      Queue Size: 10
+      Topic: /convex_plane_decomposition_ros_approximation_demo_node/convex_terrain
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 255; 255; 255
@@ -135,33 +166,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 12.910699844360352
+      Distance: 7.742486953735352
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 0.23195862770080566
-        Y: 6.398481369018555
-        Z: 3.2901763916015625e-05
+        X: -3.113455057144165
+        Y: 1.4824542999267578
+        Z: 3.337860107421875e-05
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5807958841323853
+      Pitch: 0.3057960569858551
       Target Frame: <Fixed Frame>
-      Yaw: 1.4850016832351685
+      Yaw: 2.410000801086426
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 2032
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f7000006e8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033300000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006e80000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003750000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7000000060fc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000c6d000006e800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002b8000006e8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000033300000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006e80000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003750000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7000000060fc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000bac000006e800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -171,5 +202,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 3696
-  X: 3984
+  X: 144
   Y: 54

plane_segmentation/convex_plane_decomposition_ros/rviz/config_demo.rviz

@@ -0,0 +1,100 @@
+//
+// Created by rgrandia on 24.06.20.
+//
+
+#include <mutex>
+
+#include <ros/ros.h>
+
+#include <geometry_msgs/PointStamped.h>
+
+#include <convex_plane_decomposition/ConvexRegionGrowing.h>
+#include <convex_plane_decomposition/GeometryUtils.h>
+#include <convex_plane_decomposition/SegmentedPlaneProjection.h>
+
+#include <convex_plane_decomposition_msgs/PlanarTerrain.h>
+
+#include <convex_plane_decomposition_ros/MessageConversion.h>
+#include <convex_plane_decomposition_ros/RosVisualizations.h>
+
+const std::string frameId = "odom";
+std::mutex terrainMutex;
+std::unique_ptr<convex_plane_decomposition::PlanarTerrain> planarTerrainPtr;
+
+void callback(const convex_plane_decomposition_msgs::PlanarTerrain::ConstPtr& msg) {
+  std::unique_ptr<convex_plane_decomposition::PlanarTerrain> newTerrain(
+      new convex_plane_decomposition::PlanarTerrain(convex_plane_decomposition::fromMessage(*msg)));
+
+  std::lock_guard<std::mutex> lock(terrainMutex);
+  planarTerrainPtr.swap(newTerrain);
+}
+
+geometry_msgs::PointStamped toMarker(const Eigen::Vector3d& position, const std_msgs::Header& header) {
+  geometry_msgs::PointStamped sphere;
+  sphere.header = header;
+  sphere.point.x = position.x();
+  sphere.point.y = position.y();
+  sphere.point.z = position.z();
+  return sphere;
+}
+
+float randomFloat(float a, float b) {
+  float random = ((float)rand()) / (float)RAND_MAX;
+  float diff = b - a;
+  float r = random * diff;
+  return a + r;
+}
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "convex_approximation_demo_node");
+  ros::NodeHandle nodeHandle("~");
+
+  // Publishers for visualization
+  auto positionPublisher = nodeHandle.advertise<geometry_msgs::PointStamped>("queryPosition", 1);
+  auto projectionPublisher = nodeHandle.advertise<geometry_msgs::PointStamped>("projectedQueryPosition", 1);
+  auto convexTerrainPublisher = nodeHandle.advertise<geometry_msgs::PolygonStamped>("convex_terrain", 1);
+  auto terrainSubscriber = nodeHandle.subscribe("/convex_plane_decomposition_ros/planar_terrain", 1, &callback);
+
+  // Node loop
+  ros::Rate rate(ros::Duration(1.0));
+  while (ros::ok()) {
+    {
+      std::lock_guard<std::mutex> lock(terrainMutex);
+      if (planarTerrainPtr) {
+        const auto& map = planarTerrainPtr->gridMap;
+
+        // Find edges.
+        double maxX = map.getPosition().x() + map.getLength().x() * 0.5;
+        double minX = map.getPosition().x() - map.getLength().x() * 0.5;
+        double maxY = map.getPosition().y() + map.getLength().y() * 0.5;
+        double minY = map.getPosition().y() - map.getLength().y() * 0.5;
+
+        Eigen::Vector3d query{randomFloat(minX, maxX), randomFloat(minY, maxY), randomFloat(0.0, 1.0)};
+        auto penaltyFunction = [](const Eigen::Vector3d& projectedPoint) { return 0.0; };
+
+        const auto projection = getBestPlanarRegionAtPositionInWorld(query, planarTerrainPtr->planarRegions, penaltyFunction);
+
+        int numberOfVertices = 16;
+        double growthFactor = 1.05;
+        const auto convexRegion = convex_plane_decomposition::growConvexPolygonInsideShape(
+            projection.regionPtr->boundaryWithInset.boundary, projection.positionInTerrainFrame, numberOfVertices, growthFactor);
+
+        std_msgs::Header header;
+        header.stamp.fromNSec(planarTerrainPtr->gridMap.getTimestamp());
+        header.frame_id = frameId;
+
+        auto convexRegionMsg =
+            convex_plane_decomposition::to3dRosPolygon(convexRegion, projection.regionPtr->transformPlaneToWorld, header);
+
+        convexTerrainPublisher.publish(convexRegionMsg);
+        positionPublisher.publish(toMarker(query, header));
+        projectionPublisher.publish(toMarker(projection.positionInWorld, header));
+      }
+    }
+
+    ros::spinOnce();
+    rate.sleep();
+  }
+
+  return 0;
+}

plane_segmentation/convex_plane_decomposition_ros/src/ConvexApproximationDemoNode.cpp

@@ -1,12 +1,11 @@
 #include "convex_plane_decomposition_ros/RosVisualizations.h"
 
 #include <geometry_msgs/Point32.h>
-#include <geometry_msgs/PolygonStamped.h>
 
 namespace convex_plane_decomposition {
 
-inline geometry_msgs::PolygonStamped to3d(const CgalPolygon2d& polygon, const Eigen::Isometry3d& transformPlaneToWorld,
-                                          const std_msgs::Header& header) {
+geometry_msgs::PolygonStamped to3dRosPolygon(const CgalPolygon2d& polygon, const Eigen::Isometry3d& transformPlaneToWorld,
+                                             const std_msgs::Header& header) {
   geometry_msgs::PolygonStamped polygon3d;
   polygon3d.header = header;
   polygon3d.polygon.points.reserve(polygon.size());
@@ -21,15 +20,15 @@ inline geometry_msgs::PolygonStamped to3d(const CgalPolygon2d& polygon, const Ei
   return polygon3d;
 }
 
-inline std::vector<geometry_msgs::PolygonStamped> to3d(const CgalPolygonWithHoles2d& polygonWithHoles, const Eigen::Isometry3d& transformPlaneToWorld,
-                                                       const std_msgs::Header& header) {
+std::vector<geometry_msgs::PolygonStamped> to3dRosPolygon(const CgalPolygonWithHoles2d& polygonWithHoles,
+                                                          const Eigen::Isometry3d& transformPlaneToWorld, const std_msgs::Header& header) {
   std::vector<geometry_msgs::PolygonStamped> polygons;
 
   polygons.reserve(polygonWithHoles.number_of_holes() + 1);
-  polygons.emplace_back(to3d(polygonWithHoles.outer_boundary(), transformPlaneToWorld, header));
+  polygons.emplace_back(to3dRosPolygon(polygonWithHoles.outer_boundary(), transformPlaneToWorld, header));
 
   for (const auto& hole : polygonWithHoles.holes()) {
-    polygons.emplace_back(to3d(hole, transformPlaneToWorld, header));
+    polygons.emplace_back(to3dRosPolygon(hole, transformPlaneToWorld, header));
   }
   return polygons;
 }
@@ -46,7 +45,7 @@ jsk_recognition_msgs::PolygonArray convertBoundariesToRosPolygons(const std::vec
   polygon_buffer.labels.reserve(planarRegions.size());
   uint32_t label = 0;
   for (const auto& planarRegion : planarRegions) {
-    auto boundaries = to3d(planarRegion.boundaryWithInset.boundary, planarRegion.transformPlaneToWorld, header);
+    auto boundaries = to3dRosPolygon(planarRegion.boundaryWithInset.boundary, planarRegion.transformPlaneToWorld, header);
     std::move(boundaries.begin(), boundaries.end(), std::back_inserter(polygon_buffer.polygons));
     for (size_t i = 0; i < boundaries.size(); ++i) {
       polygon_buffer.labels.push_back(label);
@@ -70,7 +69,7 @@ jsk_recognition_msgs::PolygonArray convertInsetsToRosPolygons(const std::vector<
   uint32_t label = 0;
   for (const auto& planarRegion : planarRegions) {
     for (const auto& inset : planarRegion.boundaryWithInset.insets) {
-      auto insets = to3d(inset, planarRegion.transformPlaneToWorld, header);
+      auto insets = to3dRosPolygon(inset, planarRegion.transformPlaneToWorld, header);
       std::move(insets.begin(), insets.end(), std::back_inserter(polygon_buffer.polygons));
       for (size_t i = 0; i < insets.size(); ++i) {
         polygon_buffer.labels.push_back(label);