Pack polyvision inside p3iv

p3iv/package.xml

@@ -15,6 +15,7 @@
   <exec_depend>p3iv_types</exec_depend>
   <exec_depend>p3iv_modules</exec_depend>
   <exec_depend>p3iv_utils</exec_depend>
+  <exec_depend>p3iv_utils_polyvision</exec_depend>
   <exec_depend>p3iv_visualization</exec_depend>
 
   <export>

p3iv_utils_polyvision/CMakeLists.txt

@@ -0,0 +1,69 @@
+set(MRT_PKG_VERSION 4.0.0)
+# Modify only if you know what you are doing!
+cmake_minimum_required(VERSION 3.5.1)
+project(p3iv_utils_polyvision)
+
+###################
+## Find packages ##
+###################
+find_package(mrt_cmake_modules REQUIRED)
+include(UseMrtStdCompilerFlags)
+include(GatherDeps)
+
+# You can add a custom.cmake in order to add special handling for this package. E.g. you can do:
+# list(REMOVE_ITEM DEPENDEND_PACKAGES <package name 1> <package name 2> ...)
+# To remove libs which cannot be found automatically. You can also "find_package" other, custom dependencies there.
+# You can also set PROJECT_INSTALL_FILES to install files that are not installed by default.
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake")
+    include("${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake")
+endif()
+
+find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES})
+
+mrt_parse_package_xml()
+
+########################
+## Add python modules ##
+########################
+# This adds a python module if located under src/{PROJECT_NAME)
+mrt_python_module_setup()
+
+mrt_glob_files(PROJECT_PYTHON_SOURCE_FILES_SRC "python_api/*.cpp")
+if (PROJECT_PYTHON_SOURCE_FILES_SRC)
+    # Add a cpp-python api library. Make sure there are no name collisions with python modules in this project
+    mrt_add_python_api( ${PROJECT_NAME}_pyapi
+        FILES ${PROJECT_PYTHON_SOURCE_FILES_SRC}
+        )
+endif()
+
+############################
+## Read source code files ##
+############################
+mrt_glob_files_recurse(PROJECT_HEADER_FILES_INC "include/*.h" "include/*.hpp" "include/*.cuh")
+mrt_glob_files(PROJECT_SOURCE_FILES_INC "src/*.h" "src/*.hpp" "src/*.cuh")
+mrt_glob_files(PROJECT_SOURCE_FILES_SRC "src/*.cpp" "src/*.cu")
+
+###########
+## Build ##
+###########
+# Declare a cpp library
+mrt_add_library(${PROJECT_NAME}
+    INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC}
+    SOURCES ${PROJECT_SOURCE_FILES_SRC}
+    )
+
+#############
+## Install ##
+#############
+# Install all targets, headers by default and scripts and other files if specified (folders or files).
+# This command also exports libraries and config files for dependent packages and this supersedes catkin_package.
+mrt_install(PROGRAMS scripts FILES res data ${PROJECT_INSTALL_FILES})
+
+#############
+## Testing ##
+#############
+# Add test targets for cpp and python tests
+if (CATKIN_ENABLE_TESTING)
+    mrt_add_tests(test)
+    mrt_add_nosetests(test)
+endif()

p3iv_utils_polyvision/CODEOWNERS

@@ -0,0 +1 @@
+* [email protected]

p3iv_utils_polyvision/README.md

@@ -0,0 +1,87 @@
+# Polyvision
+
+Polygon intersection and offsetting operations for visible area calculations
+
+### [Doxygen documentation](/doxygen/index.html)
+### [Coverage report](/coverage/index.html)
+
+## Usage of Polyvision C++ Package in Python
+
+The Polyvision package uses internally the CGAL library for polygon intersection and clipping. It calculates the visible area as list of 2D polygons given a field of view (list of 2D polygons) and some obstacles (also list of 2D polygons).  
+Internally a VisibleArea object stores the fieldOfView polygons and the obstacle polygons in lists. The maximum precision for the coordinates of the polygon edges is 6 decimals.
+
+This is how to use this package:
+
+* The visible areas as well as the opaque polygons are passed to the constructor of the VisibleArea class as list of numpy arrays of shape (nx2), where n is the number of points of the polygon
+* The origin is passed to the constructor as numpy array of shape (1x2)
+* After the visible area object has been created, the function *calculateVisibleArea(bool mergeFieldsOfView = true, bool printEachStep = false)* must be called. 
+  * if mergeFieldsOfView = true, there will be only one contiguous visible area returned, otherwise the respective visible areas as a list
+  * if printEachStep = true, the results will be printed on the console
+* Next, the results can be accessed with the getter functions:
+  * py::list getVisibleAreas() const;
+  * py::list getNonVisibleAreas() const;
+  * py::list getCentralProjectedOpaquePolygons() const;
+  * py::list getOpaquePolygons() const;
+  * py::list getFieldsOfView() const;
+  * py::array_t<double> getOrigin() const;
+  * double getRange() const;
+
+Limitations:
+  * The origin must never be inside an opaque polygon.
+
+## Installation
+
+Polyvision requires `CGAL > 5.0`, which is a header only library. The older versions are not header only. 
+
+Assuming you use Ubuntu, you can install its [debian package](https://packages.ubuntu.com/focal/libcgal-dev). Make sure that you have installed its dependecies as well:
+```
+libboost-dev
+libboost-program-options-dev
+libboost-system-dev
+libboost-thread-dev
+libgmp-dev
+libmpfr-dev
+zlib1g-dev
+libmpfi-dev
+libntl-dev
+libtbb-dev
+```
+
+### Example
+In Python:
+
+    # origin
+    _position = np.array([0, 0])
+    # set up field of view
+    fov2 = np.array([[0, 0],
+                     [-5, 6],
+                     [5, 6]])
+    fov5 = np.array([[0, 0],
+                     [3, -2],
+                     [-3, -2]])
+    _fieldOfView = [fov2, fov5]
+    # set up polygons
+    p1 = np.array([[-2, 2],
+                   [-1, 2],
+                   [0, 3]])
+    p2 = np.array([[0, -1.5],
+                   [-0.5, -1],
+                   [1.3, -1]])
+    p3 = np.array([[-2, 4],
+                   [3, 4], [1, 5]])
+    p4 = np.array([[2, 3],
+                   [1, 2],
+                   [1, 0], [2, 2]])
+    perceptedPolygons = [p3, p4, p1, p2]
+
+    # create visibleArea object for calculations
+    visA = VisibleArea(_position, _fieldOfView, perceptedPolygons)
+    # calculate visible area
+    visA.calculateVisibleArea()
+
+    results = [visA.getFieldsOfView(), visA.getOpaquePolygons(),
+                   visA.getVisibleAreas(), visA.getNonVisibleAreas(), visA.getCentralProjectedOpaquePolygons()]
+
+Visible area returned in blue and the non-visible area in grey (not the result from example above):
+
+![alt text](res/Polyvision&#32;Demo1.png)

p3iv_utils_polyvision/include/p3iv_utils_polyvision/cgal_debug_utils..hpp

@@ -0,0 +1,158 @@
+#include <iostream>
+#include <list>
+#include <CGAL/Arr_segment_traits_2.h>
+#include <CGAL/Arrangement_2.h>
+#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Polygon_2.h>
+#include <CGAL/Polygon_set_2.h>
+#include <CGAL/Polygon_with_holes_2.h>
+#include "gtest/gtest.h"
+#include "p3iv_utils_polyvision/polyvision_cgal.hpp"
+
+namespace polyvision {
+
+class CGALDebugUtils {
+
+public:
+    /**
+     * @brief pretty-print a CGAL polygon.
+     */
+    template <class Kernel, class Container>
+    void printPolygon_2(const CGAL::Polygon_2<Kernel, Container>& P) {
+        typename CGAL::Polygon_2<Kernel, Container>::Vertex_const_iterator vit;
+
+        std::cout << "[ " << P.size() << " vertices:";
+        for (vit = P.vertices_begin(); vit != P.vertices_end(); ++vit)
+            std::cout << " (" << *vit << ')';
+        std::cout << " ]" << std::endl;
+
+        return;
+    }
+
+    /**
+     * @brief pretty-print a polygon with holes.
+     */
+    template <class Kernel, class Container>
+    void printPolygon_with_holes2(const CGAL::Polygon_with_holes_2<Kernel, Container>& pwh) {
+        if (!pwh.is_unbounded()) {
+            std::cout << "{ Outer boundary = ";
+            printPolygon_2(pwh.outer_boundary());
+        } else
+            std::cout << "{ Unbounded polygon." << std::endl;
+
+        typename CGAL::Polygon_with_holes_2<Kernel, Container>::Hole_const_iterator hit;
+        unsigned int k = 1;
+
+        std::cout << "  " << pwh.number_of_holes() << " holes:" << std::endl;
+        for (hit = pwh.holes_begin(); hit != pwh.holes_end(); ++hit, ++k) {
+            std::cout << "    Hole #" << k << " = ";
+            printPolygon_2(*hit);
+        }
+        std::cout << " }" << std::endl;
+        return;
+    }
+
+    /**
+     * @brief prints all polygons in a Polygon_set_2.
+     */
+    void printPolygon_set_2(const Polygon_set_2& polyset) {
+        std::list<Polygon_with_holes_2>
+            polysInPolyset; // the list, where all Polygons_with_holes from polyset will be inserted
+        std::list<Polygon_with_holes_2>::const_iterator it;
+        polyset.polygons_with_holes(
+            std::back_inserter(polysInPolyset)); // insert all polygons from polyset in polysInPolyset
+        std::cout << "-------------------print Polygon_set_2" << std::endl;
+        // iterate over polysInPolyset
+        int polyNum = 0;
+        for (it = polysInPolyset.begin(); it != polysInPolyset.end(); it++) {
+            // print vertices of boundary of polygon
+            Polygon_2 poly = it->outer_boundary();
+            int vertexNum = 0;
+            // iterate over vertices in polygon
+            for (Polygon_2::Vertex_const_iterator itv = poly.vertices_begin(); itv != poly.vertices_end(); itv++) {
+                std::cout << "Poly:" << polyNum << " V:" << vertexNum << " = (" << *itv << ")" << std::endl;
+                vertexNum++;
+            }
+            // print holes of polygon
+            if (it->has_holes()) {
+                int holeNum = 0;
+                for (Polygon_with_holes_2::Hole_const_iterator hit = it->holes_begin(); hit != it->holes_end(); hit++) {
+                    // print vertices of hole
+                    vertexNum = 0;
+                    for (Polygon_2::Vertex_const_iterator itv = hit->vertices_begin(); itv != hit->vertices_end();
+                         itv++) {
+                        std::cout << "Poly:" << polyNum << " -- Hole:" << holeNum << " V" << vertexNum << " = (" << *itv
+                                  << ")" << std::endl;
+                        vertexNum++;
+                    }
+                    holeNum++;
+                }
+            } else {
+                std::cout << "Poly:" << polyNum << " - has no holes" << std::endl;
+            }
+            polyNum++;
+        }
+        std::cout << "-------------------" << std::endl;
+    }
+
+    /**
+     * @brief print faces.
+     */
+    void print_face(const Arrangement_2::Face_const_handle fh) {
+        if (fh->is_unbounded()) {
+            std::cout << "Unbounded face! with holes: " << fh->number_of_holes() << std::endl;
+            Arrangement_2::Hole_const_iterator holit;
+            holit = fh->holes_begin();
+            Arrangement_2::Ccb_halfedge_const_circulator curr = *holit;
+            std::cout << "(" << curr->source()->point() << ")";
+            do {
+                Arrangement_2::Halfedge_const_handle he;
+                he = curr;
+                std::cout << " [" << he->curve() << "] " << std::endl;
+                std::cout << "(" << he->target()->point() << ")";
+            } while (++curr != *holit);
+            std::cout << "\n\n\n";
+        } else {
+            std::cout << "Outer boundary:";
+            Arrangement_2::Ccb_halfedge_const_circulator circ;
+            circ = fh->outer_ccb();
+            Arrangement_2::Ccb_halfedge_const_circulator curr = circ;
+            std::cout << "(" << curr->source()->point() << ")";
+            do {
+                Arrangement_2::Halfedge_const_handle he;
+                he = curr;
+                std::cout << " [" << he->curve() << "] " << std::endl;
+                std::cout << "(" << he->target()->point() << ")";
+            } while (++curr != circ);
+
+            std::cout << "\n\n\n";
+            std::cout << "Number of Holes: " << fh->number_of_holes() << std::endl;
+            Arrangement_2::Hole_const_iterator holit;
+            for (holit = fh->holes_begin(); holit != fh->holes_end(); holit++) {
+                Arrangement_2::Ccb_halfedge_const_circulator curr = *holit;
+                std::cout << "(" << curr->source()->point() << ")";
+                do {
+                    Arrangement_2::Halfedge_const_handle he;
+                    he = curr;
+                    std::cout << " [" << he->curve() << "] " << std::endl;
+                    std::cout << "(" << he->target()->point() << ")";
+                } while (++curr != *holit);
+                std::cout << std::endl;
+            }
+        }
+    }
+
+    void printArrangement_2Faces(const Arrangement_2& arr) {
+        std::cout << "-------------------print Arrangement_2 faces" << std::endl;
+        Arrangement_2::Face_const_iterator fit;
+        int k = 0;
+        for (fit = arr.faces_begin(); fit != arr.faces_end(); fit++) {
+            std::cout << k << " - ";
+            print_face(fit);
+            k++;
+        }
+        std::cout << "-------------------" << std::endl;
+    }
+};
+
+} // namespace polyvision

p3iv_utils_polyvision/include/p3iv_utils_polyvision/cgal_utils.hpp

@@ -0,0 +1,21 @@
+#include <iostream>
+#include <list>
+#include <CGAL/Arr_segment_traits_2.h>
+#include <CGAL/Arrangement_2.h>
+#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Polygon_2.h>
+#include <CGAL/Polygon_set_2.h>
+#include <CGAL/Polygon_with_holes_2.h>
+#include "polyvision_cgal.hpp"
+#include "gtest/gtest.h"
+
+namespace polyvision {
+
+class CGALUtils {
+
+public:
+    std::list<Polygon_with_holes_2> convertPolygonset2PolygonList(const Polygon_set_2& polygonset);
+    bool segmentIntersectsSegment(const Segment_2& segment_a, const Segment_2& segment_b, Point_2& point);
+    bool segmentIntersectsPolygon(const Polygon_set_2& polyset, const Segment_2& line, Point_2& point);
+};
+} // namespace polyvision

p3iv_utils_polyvision/include/p3iv_utils_polyvision/check_inside.hpp

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <polyvision_cgal.hpp>
+
+namespace polyvision {
+
+bool checkInside(Point_2 pt, std::vector<Point_2> points) {
+
+    switch (CGAL::bounded_side_2(&points[0], &points[0] + points.size(), pt, Kernel())) {
+    case CGAL::ON_BOUNDED_SIDE:
+        return true;
+    case CGAL::ON_BOUNDARY:
+        return true;
+    case CGAL::ON_UNBOUNDED_SIDE:
+        return false;
+    }
+    return false; // todo: hack!
+}
+} // namespace polyvision

p3iv_utils_polyvision/include/p3iv_utils_polyvision/polyvision_cgal.hpp

@@ -0,0 +1,25 @@
+#pragma once
+// All CGAL includes and typdefs here
+
+// CGAL includes
+#include <CGAL/Arr_segment_traits_2.h>
+#include <CGAL/Arrangement_2.h>
+#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Polygon_2.h>
+#include <CGAL/Polygon_set_2.h>
+
+// CGAL typdefs
+namespace polyvision {
+typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;
+typedef Kernel::Point_2 Point_2;
+typedef Kernel::Segment_2 Segment_2;
+typedef Kernel::Line_2 Line_2;
+typedef Kernel::Intersect_2 Intersect_2;
+typedef CGAL::Polygon_2<Kernel> Polygon_2;
+typedef CGAL::Polygon_with_holes_2<Kernel> Polygon_with_holes_2;
+typedef CGAL::Polygon_set_2<Kernel> Polygon_set_2;
+typedef CGAL::Arr_segment_traits_2<Kernel> Traits_2;
+typedef CGAL::Arrangement_2<Traits_2> Arrangement_2;
+typedef Arrangement_2::Face_handle Face_handle;
+typedef Arrangement_2::Face_const_handle Face_const_handle;
+} // namespace polyvision