[
  {
    "path": ".gitlab-ci.yml",
    "content": "build-kinetic:\n  variables:\n    ROSDISTRO: \"kinetic\"\n    CATKIN_WS: \"/root/catkin_ws\"\n  stage: build\n  image: osrf/ros:$ROSDISTRO-desktop-full\n  before_script: \n    - apt-get -qq update\n    - apt-get -qq install git-core python-catkin-tools curl\n    - mkdir -p $CATKIN_WS/src\n    - cp -a . $CATKIN_WS/src/\n    - cd $CATKIN_WS\n    - rosdep update\n    - rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false\n  script: \n    - source /ros_entrypoint.sh\n    - cd $CATKIN_WS\n    - catkin build -i -s --no-status\n  only:\n    - master\n  tags:\n    - ubuntu\n    - docker"
  },
  {
    "path": ".travis.yml",
    "content": "# Generic .travis.yml file for running continuous integration on Travis-CI with\n# any ROS package.\n#\n# This installs ROS on a clean Travis-CI virtual machine, creates a ROS\n# workspace, resolves all listed dependencies, and sets environment variables\n# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are\n# no compilation errors), and runs all the tests. If any of the compilation/test\n# phases fail, the build is marked as a failure.\n#\n# We handle two types of package dependencies:\n#   - packages (ros and otherwise) available through apt-get. These are installed\n#     using rosdep, based on the information in the ROS package.xml.\n#   - dependencies that must be checked out from source. These are handled by\n#     'wstool', and should be listed in a file named dependencies.rosinstall.\n#\n# There are two variables you may want to change:\n#   - ROS_DISTRO (default is indigo). Note that packages must be available for\n#     ubuntu 14.04 trusty.\n#   - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo\n#     root). This should list all necessary repositories in wstool format (see\n#     the ros wiki). If the file does not exists then nothing happens.\n#\n# See the README.md for more information.\n#\n# Author: Felix Duvallet <felixd@gmail.com>\n\n# NOTE: The build lifecycle on Travis.ci is something like this:\n#    before_install\n#    install\n#    before_script\n#    script\n#    after_success or after_failure\n#    after_script\n#    OPTIONAL before_deploy\n#    OPTIONAL deploy\n#    OPTIONAL after_deploy\n\n################################################################################\n\n# Use ubuntu trusty (14.04) with sudo privileges.\ndist: trusty\nsudo: required\nlanguage:\n  - generic\ncache:\n  - apt\n\n# Configuration variables. All variables are global now, but this can be used to\n# trigger a build matrix for different ROS distributions if desired.\nenv:\n  global:\n    - ROS_CI_DESKTOP=\"`lsb_release -cs`\"  # e.g. [precise|trusty|...]\n    - CI_SOURCE_PATH=$(pwd)\n    - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall\n    - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options\n    - ROS_PARALLEL_JOBS='-j8 -l6'\n  matrix:\n    - ROS_DISTRO=indigo\n    - ROS_DISTRO=jade\n\n\n################################################################################\n\n# Install system dependencies, namely a very barebones ROS setup.\nbefore_install:\n  - sudo sh -c \"echo \\\"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\\\" > /etc/apt/sources.list.d/ros-latest.list\"\n  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -\n  - sudo apt-get update -qq\n  - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin\n  - source /opt/ros/$ROS_DISTRO/setup.bash\n  # Prepare rosdep to install dependencies.\n  - sudo rosdep init\n  - rosdep update\n\n# Create a catkin workspace with the package under integration.\ninstall:\n  - mkdir -p ~/catkin_ws/src\n  - cd ~/catkin_ws/src\n  - catkin_init_workspace\n  # Create the devel/setup.bash (run catkin_make with an empty workspace) and\n  # source it to set the path variables.\n  - cd ~/catkin_ws\n  - catkin_make\n  - source devel/setup.bash\n  # Add the package under integration to the workspace using a symlink.\n  - cd ~/catkin_ws/src\n  - ln -s $CI_SOURCE_PATH .\n\n# Install all dependencies, using wstool and rosdep.\n# wstool looks for a ROSINSTALL_FILE defined in the environment variables.\nbefore_script:\n  # source dependencies: install using wstool.\n  - cd ~/catkin_ws/src\n  - wstool init\n  - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi\n  - wstool up\n  # package depdencies: install using rosdep.\n  - cd ~/catkin_ws\n  - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO\n\n# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to\n# catkin_make.\nscript:\n  - cd ~/catkin_ws\n  - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS )\n  # Testing: Use both run_tests (to see the output) and test (to error out).\n  - catkin_make run_tests  # This always returns 0, but looks pretty.\n  - catkin_make test  # This will return non-zero if a test fails.\n\n"
  },
  {
    "path": "CHANGELOG.rst",
    "content": "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\nChangelog for package costmap_converter\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n\n0.0.13 (2020-05-13)\n-------------------\n* Changed minimum CMake version to 3.1\n* Fixed wrong return type of method pointToNeighborCells\n* OpenCV 4 compatibility fix (Thanks to daviddudas)\n* Contributors: Christoph Rösmann, daviddudas\n\n0.0.12 (2019-12-02)\n-------------------\n* CostmapToPolygons: Simplification of the polygon by Douglas-Peucker algorithm (reduces the density of points in the polygon).\n* Bugfixes\n* Contributors: Rainer Kümmerle\n\n0.0.11 (2019-10-26)\n-------------------\n* rostest integration to avoid running a roscore separately for unit testing\n* Contributors: Christoph Rösmann\n\n0.0.10 (2019-10-26)\n-------------------\n* Runtime improvements for CostmapToPolygonsDBSMCCH (`#12 <https://github.com/rst-tu-dortmund/costmap_converter/issues/12>`_)\n  * Grid lookup for regionQuery\n  * use a grid structure for looking up nearest neighbors\n  * parameters in a struct\n  * guard the parameters by drawing a copy from dynamic reconfigure\n  * Adding some test cases for regionQuery and dbScan\n  * Avoid computing sqrt at the end of convexHull2\n  * Add doxygen comments for the neighbor lookup\n  * Change the param read to one liners\n  * Add test on empty map for dbScan\n* Contributors: Rainer Kümmerle\n\n0.0.9 (2018-05-28)\n------------------\n* Moved plugin loader for static costmap conversion to BaseCostmapToDynamicObstacles.\n  The corresponding ROS parameter `static_converter_plugin` is now defined in the CostmapToDynamicObstacles namespace. \n* Contributors: Christoph Rösmann\n\n0.0.8 (2018-05-17)\n------------------\n* Standalone converter subscribes now to costmap updates. Fixes `#1 <https://github.com/rst-tu-dortmund/costmap_converter/issues/1>`_\n* Adds radius field for circular obstacles to ObstacleMsg\n* Stacked costmap conversion (`#7 <https://github.com/rst-tu-dortmund/costmap_converter/issues/7>`_).\n  E.g., it is now possible combine a dynamic obstacle and static obstacle converter plugin.\n* Contributors: Christoph Rösmann, Franz Albers\n\n0.0.7 (2017-09-20)\n------------------\n* Fixed some compilation issues (C++11 compiler flags and opencv2 on indigo/jade).\n* Dynamic obstacle plugin: obstacle velocity is now published for both x and y coordinates rather than the absolute value\n\n0.0.6 (2017-09-18)\n------------------\n* This pull request adds the costmap to dynamic obstacles plugin (written by Franz Albers).\n  It detects the dynamic foreground of the costmap (based on the temporal evolution of the costmap)\n  including blobs representing the obstacles. Furthermore, Kalman-based tracking is applied to estimate\n  the current velocity for each obstacle.\n  **Note, this plugin is still experimental.**\n* New message types are introduced: costmap\\_converter::ObstacleMsg and costmap\\_converter::ObstacleArrayMsg.\n  These types extend the previous polygon representation by additional velocity, orientation and id information.\n* The API has been extended to provide obstacles via the new ObstacleArrayMsg type instead of vector of polygons.\n* Contributors: Franz Albers, Christoph Rösmann\n\n0.0.5 (2016-02-01)\n------------------\n* Major changes regarding the line detection based on the convex hull\n  (it should be much more robust now).\n* Concave hull plugin added.\n* The cluster size can now be limited from above using a specific parameter.\n  This implicitly avoids large clusters forming a 'L' or 'U'.\n* All parameters can now be adjusted using dynamic_reconfigure (rqt_reconfigure).\n* Some parameter names changed.\n* Line plugin based on ransac: line inliers must now be placed inbetween the start and end of a line.\n\n0.0.4 (2016-01-11)\n------------------\n* Fixed conversion from map to world coordinates if the costmap is not quadratic.\n\n0.0.3 (2015-12-23)\n------------------\n* The argument list of the initialize method requires a nodehandle from now on. This facilitates the handling of parameter namespaces for multiple instantiations of the plugin.\n* This change is pushed immediately as a single release to avoid API breaks (since version 0.0.2 is not on the official repos up to now).\n\n0.0.2 (2015-12-22)\n------------------\n* Added a plugin for converting the costmap to lines using ransac\n\n0.0.1 (2015-12-21)\n------------------\n* First release of the package including a pluginlib interface, two plugins (costmap to polygons and costmap to lines) and a standalone conversion node.\n\n"
  },
  {
    "path": "CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 3.1)\nproject(costmap_converter)\n\n# Set to Release in order to speed up the program significantly\nset(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\nfind_package(catkin REQUIRED COMPONENTS\n  geometry_msgs\n  roscpp\n  std_msgs\n  message_generation\n  costmap_2d\n  dynamic_reconfigure\n  pluginlib\n  cv_bridge\n)\n\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\nfind_package(OpenCV REQUIRED)\n\n###set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR})\n###find_package(Eigen3 REQUIRED)\n###set(EXTERNAL_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS})\n\nif (CATKIN_ENABLE_TESTING)\n  find_package(rostest REQUIRED)\nendif()\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n## C++11 support\nIF(NOT MSVC)\n  include(CheckCXXCompilerFlag)\n  CHECK_CXX_COMPILER_FLAG(\"-std=c++11\" COMPILER_SUPPORTS_CXX11)\n  CHECK_CXX_COMPILER_FLAG(\"-std=c++0x\" COMPILER_SUPPORTS_CXX0X)\nif(COMPILER_SUPPORTS_CXX11)\n  set(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++11\")\nelseif(COMPILER_SUPPORTS_CXX0X)\n  set(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++0x\")\nelse()\n  message(STATUS \"The compiler ${CMAKE_CXX_COMPILER} has no C++11 support which is required \n  by linked third party packages starting from ROS Jade. Ignore this message for ROS Indigo.\")\nendif()\nendif()\n\n################################################\n## Declare ROS messages, services and actions ##\n################################################\n\n## To declare and build messages, services or actions from within this\n## package, follow these steps:\n## * Let MSG_DEP_SET be the set of packages whose message types you use in\n##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).\n## * In the file package.xml:\n##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET\n##   * If MSG_DEP_SET isn't empty the following dependencies might have been\n##     pulled in transitively but can be declared for certainty nonetheless:\n##     * add a build_depend tag for \"message_generation\"\n##     * add a run_depend tag for \"message_runtime\"\n## * In this file (CMakeLists.txt):\n##   * add \"message_generation\" and every package in MSG_DEP_SET to\n##     find_package(catkin REQUIRED COMPONENTS ...)\n##   * add \"message_runtime\" and every package in MSG_DEP_SET to\n##     catkin_package(CATKIN_DEPENDS ...)\n##   * uncomment the add_*_files sections below as needed\n##     and list every .msg/.srv/.action file to be processed\n##   * uncomment the generate_messages entry below\n##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)\n\n## Generate messages in the 'msg' folder\nadd_message_files(\n  FILES\n  ObstacleMsg.msg\n  ObstacleArrayMsg.msg\n)\n\n## Generate services in the 'srv' folder\n# add_service_files(\n#   FILES\n#   Service1.srv\n#   Service2.srv\n# )\n\n## Generate actions in the 'action' folder\n# add_action_files(\n#   FILES\n#   Action1.action\n#   Action2.action\n# )\n\n## Generate added messages and services with any dependencies listed here\ngenerate_messages(\n   DEPENDENCIES\n   geometry_msgs std_msgs\n)\n\n#add dynamic reconfigure api\n#find_package(catkin REQUIRED dynamic_reconfigure)\ngenerate_dynamic_reconfigure_options(\n  cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg\n  cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg\n  cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg\n  cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg\n  cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg\n)\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## INCLUDE_DIRS: uncomment this if you package contains header files\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\n  INCLUDE_DIRS include\n  LIBRARIES costmap_converter\n  CATKIN_DEPENDS \n    geometry_msgs\n    pluginlib\n    roscpp \n    std_msgs\n    message_runtime \n    dynamic_reconfigure\n    costmap_2d\n  #DEPENDS \n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\n# include_directories(include)\ninclude_directories(\n  include\n  ${catkin_INCLUDE_DIRS}\n  ${OpenCV_INCLUDE_DIRS}\n)\n\n## Declare a cpp library\nadd_library(costmap_converter   src/costmap_to_polygons.cpp \n                                src/costmap_to_polygons_concave.cpp\n                                src/costmap_to_lines_convex_hull.cpp\n                                src/costmap_to_lines_ransac.cpp\n                                src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp\n                                src/costmap_to_dynamic_obstacles/background_subtractor.cpp\n                                src/costmap_to_dynamic_obstacles/blob_detector.cpp\n                                src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp\n                                src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp\n                                src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp\n)\ntarget_link_libraries(costmap_converter ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})\n# target_compile_features(costmap_converter PUBLIC cxx_nullptr cxx_range_for)\n\n# Dynamic reconfigure: make sure configure headers raare built before any node using them\nadd_dependencies(costmap_converter ${PROJECT_NAME}_gencfg)\n# Generate messages before compiling the lib\nadd_dependencies(costmap_converter ${PROJECT_NAME}_generate_messages_cpp)\n\n## Declare a cpp executable\nadd_executable(standalone_converter src/costmap_converter_node.cpp)\ntarget_link_libraries(standalone_converter ${catkin_LIBRARIES} )\nadd_dependencies(standalone_converter costmap_converter)\n  \n# (un)set: cmake -DCVV_DEBUG_MODE=OFF ..\noption(CVV_DEBUG_MODE \"cvvisual-debug-mode\" ON)\nif(CVV_DEBUG_MODE MATCHES ON)\n  set(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -DCVVISUAL_DEBUGMODE\")\nendif()\n\n\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# install(PROGRAMS\n#   scripts/my_python_script\n#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables and/or libraries for installation\n\n## Mark executables and/or libraries for installation\ninstall(TARGETS costmap_converter\n   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n)\ninstall(TARGETS standalone_converter\n   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n)\n\n## Mark cpp header files for installation\ninstall(DIRECTORY include/${PROJECT_NAME}/\n   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n   #FILES_MATCHING PATTERN \"*.h\"\n   PATTERN \".svn\" EXCLUDE\n)\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\ninstall(FILES\n  plugins.xml\n  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n)\n\ninstall(DIRECTORY\n  cfg\n  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n  PATTERN \".svn\" EXCLUDE\n)\n\n\n#############\n## Testing ##\n#############\n\nif (CATKIN_ENABLE_TESTING)\n  add_rostest_gtest(test_costmap_polygons test/costmap_polygons.test test/costmap_polygons.cpp)\n  if(TARGET test_costmap_polygons)\n     target_link_libraries(test_costmap_polygons costmap_converter)  \n  endif()\nendif()\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/test_drawing.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n"
  },
  {
    "path": "README.md",
    "content": "costmap_converter ROS Package\n=============================\n\nA ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types\n\nBuild status of the *master* branch:\n- ROS Buildfarm Noetic: [![Noetic Build Status](http://build.ros.org/buildStatus/icon?job=Ndev__costmap_converter__ubuntu_focal_amd64)](http://build.ros.org/job/Ndev__costmap_converter__ubuntu_focal_amd64/)\n- ROS Buildfarm Melodic: [![Melodic Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__costmap_converter__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__costmap_converter__ubuntu_bionic_amd64/)\n\n\n### Contributors\n\n- Christoph Rösmann\n- Franz Albers (*CostmapToDynamicObstacles* plugin)\n- Otniel Rinaldo\n\n\n### License\n\nThe *costmap_converter* package is licensed under the BSD license.\nIt depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed.\n\nSome third-party dependencies are included that are licensed under different terms:\n - *MultitargetTracker*, GNU GPLv3, https://github.com/Smorodov/Multitarget-tracker\n   (partially required for the *CostmapToDynamicObstacles* plugin)\n\nAll packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details.\n\n\n\n"
  },
  {
    "path": "cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg",
    "content": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For integers and doubles:\n#       Name                    Type      Reconfiguration level\n#       Description\n#       Default  Min  Max\n\n##################################################################\n###################### Foreground detection ######################\ngen.add(\"alpha_slow\", double_t, 0,\n\t\"Foreground detection: Learning rate of the slow filter\",\n\t0.3, 0.0, 1.0)\n\ngen.add(\"alpha_fast\", double_t, 0,\n\t\"Foreground detection: Learning rate of the fast filter\",\n\t0.85, 0.0, 1.0)\n\ngen.add(\"beta\", double_t, 0,\n\t\"Foreground detection: Weighting coefficient between a pixels value and the mean of its nearest neighbors\",\n\t0.85, 0.0, 1.0)\n\ngen.add(\"min_sep_between_slow_and_fast_filter\", int_t, 0,\n\t\"Foreground detection: Minimal difference between the fast and the slow filter to recognize a obstacle as dynamic\",\n\t80, 0, 255)\n\ngen.add(\"min_occupancy_probability\", int_t, 0,\n\t\"Foreground detection: Minimal value of the fast filter to recognize a obstacle as dynamic\",\n\t180, 0, 255)\n\ngen.add(\"max_occupancy_neighbors\", int_t, 0,\n\t\"Foreground detection: Maximal mean value of the nearest neighbors of a pixel in the slow filter\",\n\t80, 0, 255)\n\ngen.add(\"morph_size\", int_t, 0,\n\t\"Foreground detection: Size of the structuring element for the closing operation\",\n\t1, 0, 10)\n\ngen.add(\"publish_static_obstacles\", bool_t, 0,\n        \"Include static obstacles as single-point polygons\",\n        True)\n\n############################################################\n###################### Blob detection ######################\n\n# These parameters are commented out, because the input image for the blob detection is already binary -> irrelevant\n#gen.add(\"threshold_step\", double_t, 0,\n#\t\"Blob detection: Distance between neighboring thresholds\",\n#\t256.0, 0.0, 256.0)\n#\n#gen.add(\"min_threshold\", double_t, 0,\n#\t\"Blob detection: Convert the source image to binary images by applying several thresholds, starting at min_threshold\",\n#\t1, 0, 255)\n#\n#gen.add(\"max_threshold\", double_t, 0,\n#\t\"Blob detection: Convert the source image to binary images by applying several thresholds, ending at max_threshold\",\n#\t255, 0, 255)\n#\n#gen.add(\"min_repeatability\", int_t, 0,\n#\t\"Blob detection: Minimal number of detections of a blob in the several thresholds to be considered as real blob\",\n#\t1, 1, 10)\n#\ngen.add(\"min_distance_between_blobs\", double_t, 0,\n\t\"Blob detection: Minimal distance between centers of two blobs to be considered as seperate blobs\",\n\t10, 0.0, 300.0)\n\ngen.add(\"filter_by_area\", bool_t, 0,\n\t\"Blob detection: Filter blobs based on number of pixels\",\n\tTrue)\n\ngen.add(\"min_area\", int_t, 0,\n\t\"Blob detection: Minimal number of pixels a blob consists of\",\n\t3, 0, 300)\n\ngen.add(\"max_area\", int_t, 0,\n\t\"Blob detection: Maximal number of pixels a blob consists of\",\n\t300, 0, 300)\n\ngen.add(\"filter_by_circularity\", bool_t, 0,\n\t\"Blob detection: Filter blobs based on their circularity\",\n\tTrue)\n\ngen.add(\"min_circularity\", double_t, 0,\n\t\"Blob detection: Minimal circularity value (0 in case of a line)\",\n\t0.2, 0.0, 1.0)\n\ngen.add(\"max_circularity\", double_t, 0,\n\t\"Blob detection: Maximal circularity value (1 in case of a circle)\",\n\t1.0, 0.0, 1.0)\n\ngen.add(\"filter_by_inertia\", bool_t, 0,\n\t\"Blob detection: Filter blobs based on their inertia ratio\",\n\tTrue)\n\ngen.add(\"min_inertia_ratio\", double_t, 0,\n\t\"Blob detection: Minimal inertia ratio\",\n\t0.2, 0.0, 1.0)\n\ngen.add(\"max_inertia_ratio\", double_t, 0,\n\t\"Blob detection: Maximal inertia ratio\",\n\t1.0, 0.0, 1.0)\n\ngen.add(\"filter_by_convexity\", bool_t, 0,\n\t\"Blob detection: Filter blobs based on their convexity (Blob area / area of its convex hull)\",\n\tFalse)\n\ngen.add(\"min_convexity\", double_t, 0,\n\t\"Blob detection: Minimum convexity ratio\",\n\t0.0, 0.0, 1.0)\n\ngen.add(\"max_convexity\", double_t, 0,\n\t\"Blob detection: Maximal convexity ratio\",\n\t1.0, 0.0, 1.0)\n\n################################################################\n#################### Tracking ##################################\ngen.add(\"dt\", double_t, 0,\n\t\"Tracking: Time for one timestep of the kalman filter\",\n\t0.2, 0.1, 3.0)\n\ngen.add(\"dist_thresh\", double_t, 0,\n\t\"Tracking: Maximum distance between two points to be considered in the assignment problem\",\n\t20.0, 0.0, 150.0)\n\ngen.add(\"max_allowed_skipped_frames\", int_t, 0,\n\t\"Tracking: Maximum number of frames a object is tracked while it is not seen\",\n\t3, 0, 10)\n\ngen.add(\"max_trace_length\", int_t, 0,\n\t\"Tracking: Maximum number of Points in a objects trace\",\n\t10, 1, 100)\n\nexit(gen.generate(\"costmap_converter\", \"standalone_converter\", \"CostmapToDynamicObstacles\"))\n"
  },
  {
    "path": "cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg",
    "content": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For integers and doubles:\n#       Name                    Type      Reconfiguration level\n#       Description\n#       Default  Min  Max\n\n\ngen.add(\"cluster_max_distance\", double_t, 0, \n\t\"Parameter for DB_Scan, maximum distance to neighbors [m]\",\n\t0.4, 0.0, 10.0)\n\ngen.add(\"cluster_min_pts\", int_t, 0, \n\t\"Parameter for DB_Scan: minimum number of points that define a cluster\",\n\t2, 1, 20)\n\ngen.add(\"cluster_max_pts\", int_t, 0, \n  \"Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)\",\n  30, 2, 200)\n\ngen.add(\"convex_hull_min_pt_separation\",   double_t,   0,\n\t\"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)\",\n\t0.1, 0.0, 10.0)\t\n\ngen.add(\"support_pts_max_dist\",   double_t,   0,\n\t\"Minimum distance from a point to the line to be counted as support point\",\n\t0.3, 0.0, 10.0)\n\ngen.add(\"support_pts_max_dist_inbetween\",   double_t,   0,\n  \"A line is only defined, if the distance between two consecutive support points is less than this treshold. Set to 0 in order to deactivate this check.\",\n  1.0, 0.0, 10.0)\n\ngen.add(\"min_support_pts\", int_t, 0, \n\t\"Minimum number of support points to represent a line\",\n\t2, 0, 50)\t\n\nexit(gen.generate(\"costmap_converter\", \"standalone_converter\", \"CostmapToLinesDBSMCCH\"))\n"
  },
  {
    "path": "cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg",
    "content": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For integers and doubles:\n#       Name                    Type      Reconfiguration level\n#       Description\n#       Default  Min  Max\n\n\ngen.add(\"cluster_max_distance\", double_t, 0, \n\t\"Parameter for DB_Scan, maximum distance to neighbors [m]\",\n\t0.4, 0.0, 10.0)\n\ngen.add(\"cluster_min_pts\", int_t, 0, \n\t\"Parameter for DB_Scan: minimum number of points that define a cluster\",\n\t2, 1, 20)\n\ngen.add(\"cluster_max_pts\", int_t, 0, \n  \"Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)\",\n  30, 2, 200)\n\ngen.add(\"ransac_inlier_distance\", double_t, 0, \n\t\"Maximum distance to the line segment for inliers\",\n\t0.2, 0.0, 10.0)\n\ngen.add(\"ransac_min_inliers\", int_t, 0, \n\t\"Minimum numer of inliers required to form a line\",\n\t10, 0, 100)\n\ngen.add(\"ransac_no_iterations\", int_t, 0, \n\t\"Number of ransac iterations\",\n\t2000, 1, 10000)\n\ngen.add(\"ransac_remainig_outliers\", int_t, 0, \n\t\"Repeat ransac until the number of outliers is as specified here\",\n\t3, 0, 50)\n\ngen.add(\"ransac_convert_outlier_pts\",   bool_t,   0, \n\t\"Convert remaining outliers to single points.\",\n\tTrue)\n\ngen.add(\"ransac_filter_remaining_outlier_pts\",   bool_t,   0, \n\t\"Filter the interior of remaining outliers and keep only keypoints of their convex hull\",\n\tFalse)\n\ngen.add(\"convex_hull_min_pt_separation\",   double_t,   0,\n\t\"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)\",\n\t0.1, 0.0, 10.0)\t\n\n\nexit(gen.generate(\"costmap_converter\", \"standalone_converter\", \"CostmapToLinesDBSRANSAC\"))\n"
  },
  {
    "path": "cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg",
    "content": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For integers and doubles:\n#       Name                    Type      Reconfiguration level\n#       Description\n#       Default  Min  Max\n\n\ngen.add(\"cluster_max_distance\", double_t, 0, \n\t\"Parameter for DB_Scan, maximum distance to neighbors [m]\",\n\t0.4, 0.0, 10.0)\n\ngen.add(\"cluster_min_pts\", int_t, 0, \n\t\"Parameter for DB_Scan: minimum number of points that define a cluster\",\n\t2, 1, 20)\n\ngen.add(\"cluster_max_pts\", int_t, 0, \n  \"Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)\",\n  30, 2, 200)\n\ngen.add(\"convex_hull_min_pt_separation\",   double_t,   0,\n\t\"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)\",\n\t0.1, 0.0, 10.0)\t\n\ngen.add(\"concave_hull_depth\",   double_t,   0,\n  \"Smaller depth: sharper surface, depth -> high value: convex hull\",\n  2.0, 0.0, 100.0) \n\nexit(gen.generate(\"costmap_converter\", \"standalone_converter\", \"CostmapToPolygonsDBSConcaveHull\"))\n"
  },
  {
    "path": "cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg",
    "content": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For integers and doubles:\n#       Name                    Type      Reconfiguration level\n#       Description\n#       Default  Min  Max\n\n\ngen.add(\"cluster_max_distance\", double_t, 0, \n\t\"Parameter for DB_Scan, maximum distance to neighbors [m]\",\n\t0.4, 0.0, 10.0)\n\ngen.add(\"cluster_min_pts\", int_t, 0, \n\t\"Parameter for DB_Scan: minimum number of points that define a cluster\",\n\t2, 1, 20)\n\ngen.add(\"cluster_max_pts\", int_t, 0, \n  \"Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)\",\n  30, 2, 200)\n\ngen.add(\"convex_hull_min_pt_separation\",   double_t,   0,\n\t\"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)\",\n\t0.1, 0.0, 10.0)\t\n\nexit(gen.generate(\"costmap_converter\", \"standalone_converter\", \"CostmapToPolygonsDBSMCCH\"))\n"
  },
  {
    "path": "include/costmap_converter/costmap_converter_interface.h",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann, Otniel Rinaldo\n *********************************************************************/\n\n#ifndef COSTMAP_CONVERTER_INTERFACE_H_\n#define COSTMAP_CONVERTER_INTERFACE_H_\n\n//#include <costmap_2d/costmap_2d_ros.h>\n#include <ros/ros.h>\n#include <ros/callback_queue.h>\n#include <boost/thread.hpp>\n#include <costmap_2d/costmap_2d.h>\n#include <costmap_2d/costmap_2d_ros.h>\n#include <geometry_msgs/Polygon.h>\n#include <costmap_converter/ObstacleArrayMsg.h>\n\n\nnamespace costmap_converter\n{\n//! Typedef for a shared dynamic obstacle container\ntypedef boost::shared_ptr<ObstacleArrayMsg> ObstacleArrayPtr;\n//! Typedef for a shared dynamic obstacle container (read-only access)\ntypedef boost::shared_ptr< const ObstacleArrayMsg > ObstacleArrayConstPtr;\n\n//! Typedef for a shared polygon container \ntypedef boost::shared_ptr< std::vector<geometry_msgs::Polygon> > PolygonContainerPtr;\n//! Typedef for a shared polygon container (read-only access)\ntypedef boost::shared_ptr< const std::vector<geometry_msgs::Polygon> > PolygonContainerConstPtr;\n  \n\n/**\n * @class BaseCostmapToPolygons\n * @brief This abstract class defines the interface for plugins that convert the costmap into polygon types\n * \n * Plugins must accept a costmap_2d::Costmap2D datatype as information source.\n * The interface provides two different use cases:\n * 1. Manual call to conversion routines: setCostmap2D(), compute() and getPolygons() \n *    (in subsequent calls setCostmap2D() can be substituted by updateCostmap2D())\n * 2. Repeatedly process costmap with a specific rate (startWorker() and stopWorker()):\n *    Make sure that the costmap is valid while the worker is active (you can specify your own spinner or activate a threaded spinner).\n *    Costmaps can be obtained by invoking getPolygons().\n */\nclass BaseCostmapToPolygons\n{\npublic: \n  \n    /**\n     * @brief Initialize the plugin\n     * @param nh Nodehandle that defines the namespace for parameters\n     */\n    virtual void initialize(ros::NodeHandle nh) = 0;\n    \n    /**\n     * @brief Destructor\n     */\n    virtual ~BaseCostmapToPolygons() \n    {\n      stopWorker();\n    }\n\n    \n    /**\n     * @brief Pass a pointer to the costap to the plugin.\n     * @warning The plugin should handle the costmap's mutex locking.\n     * @sa updateCostmap2D\n     * @param costmap Pointer to the costmap2d source\n     */\n    virtual void setCostmap2D(costmap_2d::Costmap2D* costmap) = 0;\n    \n    /**\n     * @brief Get updated data from the previously set Costmap2D\n     * @warning The plugin should handle the costmap's mutex locking.\n     * @sa setCostmap2D\n     */\n    virtual void updateCostmap2D() = 0;\n    \n     /**\n     * @brief This method performs the actual work (conversion of the costmap to polygons)\n     */\n    virtual void compute() = 0;\n    \n    /**\n     * @brief Get a shared instance of the current polygon container\n     *\n     * If this method is not implemented by any subclass (plugin) the returned shared\n     * pointer is empty.\n     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!\n     * @warning The underlying plugin must ensure that this method is thread safe.\n     * @return Shared instance of the current polygon container\n     */\n    virtual PolygonContainerConstPtr getPolygons(){return PolygonContainerConstPtr();}\n\n  /**\n   * @brief Get a shared instance of the current obstacle container\n   * If this method is not overwritten by the underlying plugin, the obstacle container just imports getPolygons().\n   * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!\n   * @warning The underlying plugin must ensure that this method is thread safe.\n   * @return Shared instance of the current obstacle container\n   * @sa getPolygons\n   */\n    virtual ObstacleArrayConstPtr getObstacles()\n    {\n      ObstacleArrayPtr obstacles = boost::make_shared<ObstacleArrayMsg>();\n      PolygonContainerConstPtr polygons = getPolygons();\n      if (polygons)\n      {\n        for (const geometry_msgs::Polygon& polygon : *polygons)\n        {\n          obstacles->obstacles.emplace_back();\n          obstacles->obstacles.back().polygon = polygon;\n        }\n      }\n      return obstacles;\n    }\n\n    /**\n     * @brief Set name of robot's odometry topic\n     *\n     * Some plugins might require odometry information\n     * to compensate the robot's ego motion\n     * @param odom_topic topic name\n     */\n    virtual void setOdomTopic(const std::string& odom_topic) {}\n\n    /**\n     * @brief Determines whether an additional plugin for subsequent costmap conversion is specified\n     *\n     * @return false, since all plugins for static costmap conversion are independent\n     */\n    virtual bool stackedCostmapConversion() {return false;}\n\n     /**\n      * @brief Instantiate a worker that repeatedly coverts the most recent costmap to polygons.\n      * The worker is implemented as a timer event that is invoked at a specific \\c rate.\n      * The passed \\c costmap pointer must be valid at the complete time and must be lockable.\n      * By specifying the argument \\c spin_thread the timer event is invoked in a separate\n      * thread and callback queue or otherwise it is called from the global callback queue (of the\n      * node in which the plugin is used).\n      * @param rate The rate that specifies how often the costmap should be updated\n      * @param costmap Pointer to the underlying costmap (must be valid and lockable as long as the worker is active\n      * @param spin_thread if \\c true,the timer is invoked in a separate thread, otherwise in the default callback queue)\n     */\n    void startWorker(ros::Rate rate, costmap_2d::Costmap2D* costmap, bool spin_thread = false)\n    {\n      setCostmap2D(costmap);\n      \n      if (spin_thread_)\n      {\n        {\n          boost::mutex::scoped_lock terminate_lock(terminate_mutex_);\n          need_to_terminate_ = true;\n        }\n        spin_thread_->join();\n        delete spin_thread_;\n      }\n      \n      if (spin_thread)\n      {\n        ROS_DEBUG_NAMED(\"costmap_converter\", \"Spinning up a thread for the CostmapToPolygons plugin\");\n        need_to_terminate_ = false;\n        spin_thread_ = new boost::thread(boost::bind(&BaseCostmapToPolygons::spinThread, this));\n        nh_.setCallbackQueue(&callback_queue_);\n      }\n      else\n      {\n        spin_thread_ = NULL;\n        nh_.setCallbackQueue(ros::getGlobalCallbackQueue());\n      }\n      \n      worker_timer_ = nh_.createTimer(rate, &BaseCostmapToPolygons::workerCallback, this);\n    }\n    \n    /**\n     * @brief Stop the worker that repeatedly converts the costmap to polygons\n     */\n    void stopWorker()\n    {\n      worker_timer_.stop();\n      if (spin_thread_)\n      {\n        {\n          boost::mutex::scoped_lock terminate_lock(terminate_mutex_);\n          need_to_terminate_ = true;\n        }\n        spin_thread_->join();\n        delete spin_thread_;\n      }\n    }\n\nprotected:\n  \n    /**\n     * @brief Protected constructor that should be called by subclasses\n     */\n    BaseCostmapToPolygons() : nh_(\"~costmap_to_polygons\"), spin_thread_(NULL), need_to_terminate_(false) {}\n    \n    /**\n     * @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) \n     */\n    void spinThread()\n    {\n      while (nh_.ok())\n      {\n        {\n          boost::mutex::scoped_lock terminate_lock(terminate_mutex_);\n          if (need_to_terminate_)\n            break;\n        }\n        callback_queue_.callAvailable(ros::WallDuration(0.1f));\n      }\n    }\n    \n    /**\n     * @brief The callback of the worker that performs the actual work (updating the costmap and converting it to polygons)\n     */\n    void workerCallback(const ros::TimerEvent&)\n    {\n      updateCostmap2D();\n      compute();\n    }\n    \nprivate:\n  ros::Timer worker_timer_;\n  ros::NodeHandle nh_;\n  boost::thread* spin_thread_;\n  ros::CallbackQueue callback_queue_;\n  boost::mutex terminate_mutex_;\n  bool need_to_terminate_;\n};    \n\n\n/**\n * @class BaseCostmapToDynamicPolygons\n * @brief This class extends the BaseCostmapToPolygongs class for dynamic costmap conversion plugins in order to incorporate a subsequent costmap converter plugin for static obstacles\n *\n * This class should not be instantiated.\n */\nclass BaseCostmapToDynamicObstacles : public BaseCostmapToPolygons\n{\npublic:\n\n  /**\n   * @brief Load underlying static costmap conversion plugin via plugin-loader\n   * @param plugin_name Exact class name of the plugin to be loaded, e.g.\n   *                    costmap_converter::CostmapToPolygonsDBSMCCH\n   * @param nh_parent   NodeHandle which is extended by the namespace of the static conversion plugin\n   */\n  void loadStaticCostmapConverterPlugin(const std::string& plugin_name, ros::NodeHandle nh_parent)\n  {\n    try\n    {\n      static_costmap_converter_ = static_converter_loader_.createInstance(plugin_name);\n\n      if(boost::dynamic_pointer_cast<BaseCostmapToDynamicObstacles>(static_costmap_converter_))\n      {\n        throw pluginlib::PluginlibException(\"The specified plugin for static costmap conversion is a dynamic plugin. Specify a static plugin.\");\n      }\n      std::string raw_plugin_name = static_converter_loader_.getName(plugin_name);\n      static_costmap_converter_->initialize(ros::NodeHandle(nh_parent, raw_plugin_name));\n      setStaticCostmapConverterPlugin(static_costmap_converter_);\n      ROS_INFO_STREAM(\"CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles \" << plugin_name << \" loaded.\");\n    }\n    catch(const pluginlib::PluginlibException& ex)\n    {\n      ROS_WARN(\"CostmapToDynamicObstacles: The specified costmap converter plugin cannot be loaded. Continuing without subsequent conversion of static obstacles. Error message: %s\", ex.what());\n      static_costmap_converter_.reset();\n    }\n  }\n\n  /**\n   * @brief Set the underlying plugin for subsequent costmap conversion of the static background of the costmap\n   * @param static_costmap_converter shared pointer to the static costmap conversion plugin\n   */\n  void setStaticCostmapConverterPlugin(boost::shared_ptr<BaseCostmapToPolygons> static_costmap_converter)\n  {\n    static_costmap_converter_ = static_costmap_converter;\n  }\n\n  /**\n   * @brief Set the costmap for the underlying plugin\n   * @param static_costmap Costmap2D, which contains the static part of the original costmap\n   */\n  void setStaticCostmap(boost::shared_ptr<costmap_2d::Costmap2D> static_costmap)\n  {\n    static_costmap_converter_->setCostmap2D(static_costmap.get());\n  }\n\n  /**\n   * @brief Apply the underlying plugin for static costmap conversion\n   */\n  void convertStaticObstacles()\n  {\n    static_costmap_converter_->compute();\n  }\n\n  /**\n   * @brief Get the converted polygons from the underlying plugin\n   * @return Shared instance of the underlying plugins polygon container\n   */\n  PolygonContainerConstPtr getStaticPolygons()\n  {\n    return static_costmap_converter_->getPolygons();\n  }\n\n  /**\n   * @brief Determines whether an additional plugin for subsequent costmap conversion is specified\n   *\n   * @return true, if a plugin for subsequent costmap conversion is specified\n   */\n  bool stackedCostmapConversion()\n  {\n    if(static_costmap_converter_)\n      return true;\n    else\n      return false;\n  }\n\nprotected:\n  /**\n   * @brief Protected constructor that should be called by subclasses\n   */\n  BaseCostmapToDynamicObstacles() : static_converter_loader_(\"costmap_converter\", \"costmap_converter::BaseCostmapToPolygons\"), static_costmap_converter_() {}\n\nprivate:\n  pluginlib::ClassLoader<BaseCostmapToPolygons> static_converter_loader_;\n  boost::shared_ptr<BaseCostmapToPolygons> static_costmap_converter_;\n};\n\n\n}\n\n\n\n#endif // end COSTMAP_CONVERTER_INTERFACE_H_\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2017\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Notes:\n * The following code makes use of the OpenCV library.\n * OpenCV is licensed under the terms of the 3-clause BSD License.\n *\n * Authors: Franz Albers, Christoph Rösmann\n *********************************************************************/\n\n#ifndef BACKGROUNDSUBTRACTOR_H_\n#define BACKGROUNDSUBTRACTOR_H_\n\n#include <cv_bridge/cv_bridge.h>\n\n/**\n * @class BackgroundSubtractor\n * @brief Perform background subtraction to extract the \"moving\" foreground\n *\n * This class is based on OpenCV's background subtraction class cv::BackgroundSubtractor.\n * It has been modified in order to incorporate a specialized bandpass filter.\n *\n * See http://docs.opencv.org/3.2.0/d7/df6/classcv_1_1BackgroundSubtractor.html for the original class.\n */\nclass BackgroundSubtractor\n{\npublic:\n  struct Params\n  {\n    double alpha_slow; //!< Filter constant (learning rate) of the slow filter part\n    double alpha_fast; //!< Filter constant (learning rate) of the fast filter part\n    double beta;\n    double min_sep_between_fast_and_slow_filter;\n    double min_occupancy_probability;\n    double max_occupancy_neighbors;\n    int morph_size;\n  };\n\n  //! Constructor that accepts a specific parameter configuration\n  BackgroundSubtractor(const Params& parameters);\n\n  /**\n   * @brief Computes a foreground mask\n   * @param[in]  image    Next video frame\n   * @param[out] fg_mask  Foreground mask as an 8-bit binary image\n   * @param[in]  shift_x  Translation along the x axis between the current and previous image\n   * @param[in]  shift_y  Translation along the y axis between the current and previous image\n   */\n  void apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x = 0, int shift_y = 0);\n\n  /**\n   * @brief OpenCV Visualization\n   * @param name  Id/name of the opencv window\n   * @param image Image to be visualized\n   */\n  void visualize(const std::string& name, const cv::Mat& image);\n\n  /**\n   * @brief Export vector of matrices to yaml file\n   * @remarks This method is intended for debugging purposes\n   * @param filename   Desired filename including path and excluding file suffix\n   * @param mat_vec    Vector of cv::Mat to be exported\n   */\n  void writeMatToYAML(const std::string& filename, const std::vector<cv::Mat>& mat_vec);\n\n  //! Update internal parameters\n  void updateParameters(const Params& parameters);\n\nprivate:\n  //! Transform/shift all internal matrices/grids according to a given translation vector\n  void transformToCurrentFrame(int shift_x, int shift_y);\n\n  cv::Mat occupancy_grid_fast_;\n  cv::Mat occupancy_grid_slow_;\n  cv::Mat current_frame_;\n\n  int previous_shift_x_;\n  int previous_shift_y_;\n\n  Params params_;\n};\n\n#endif // BACKGROUNDSUBTRACTOR_H_\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2017\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Notes:\n * The following code makes use of the OpenCV library.\n * OpenCV is licensed under the terms of the 3-clause BSD License.\n *\n * Authors: Franz Albers, Christoph Rösmann\n *********************************************************************/\n\n#ifndef BLOBDETECTOR_H_\n#define BLOBDETECTOR_H_\n\n// Basically the OpenCV SimpleBlobDetector, extended with getContours()\n\n#include <opencv2/features2d/features2d.hpp>\n\n/**\n * @class BlobDetector\n * @brief Detect blobs in image (specialized for dynamic obstacles in the costmap)\n *\n * This class is based on OpenCV's blob detector cv::SimpleBlobDetector.\n * It has been modified and specialized for dynamic obstacle tracking in the costmap:\n * -> The modified version also returns contours of the blob.\n *\n * See http://docs.opencv.org/trunk/d0/d7a/classcv_1_1SimpleBlobDetector.html for the original class.\n */\nclass BlobDetector : public cv::SimpleBlobDetector\n{\npublic:\n  //! Default constructor which optionally accepts custom parameters\n  BlobDetector(const cv::SimpleBlobDetector::Params& parameters = cv::SimpleBlobDetector::Params());\n\n  //! Create shared instance of the blob detector with given parameters\n  static cv::Ptr<BlobDetector> create(const BlobDetector::Params& params);\n\n  /**\n   * @brief Detects keypoints in an image and extracts contours\n   *\n   * In contrast to the original detect method, this extended version\n   * also extracts contours. Contours can be accessed by getContours()\n   * after invoking this method.\n   *\n   * @todo The mask option is currently not implemented.\n   *\n   * @param image     image\n   * @param keypoints The detected keypoints.\n   * @param mask      Mask specifying where to look for keypoints (optional). It must be a 8-bit integer\n   *                  matrix with non-zero values in the region of interest.\n   */\n  virtual void detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,\n                      const cv::Mat& mask = cv::Mat());\n\n  /**\n   * @brief Access contours extracted during detection stage\n   * @return Read-only reference to the contours set of the previous detect() run\n   */\n  const std::vector<std::vector<cv::Point>>& getContours() { return contours_; }\n\n  //! Update internal parameters\n  void updateParameters(const cv::SimpleBlobDetector::Params& parameters);\n\nprotected:\n  struct Center\n  {\n    cv::Point2d location;\n    double radius;\n    double confidence;\n  };\n\n  virtual void findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,\n                         std::vector<std::vector<cv::Point>>& cur_contours) const;\n\n  std::vector<std::vector<cv::Point>> contours_;\n\n  Params params_;\n};\n\n#endif // BLOBDETECTOR_H_\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2017\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Notes:\n * The following code makes use of the OpenCV library.\n * OpenCV is licensed under the terms of the 3-clause BSD License.\n *\n * Authors: Franz Albers, Christoph Rösmann\n *********************************************************************/\n\n#ifndef COSTMAP_TO_DYNAMIC_OBSTACLES_H_\n#define COSTMAP_TO_DYNAMIC_OBSTACLES_H_\n\n// ROS\n#include <costmap_converter/costmap_converter_interface.h>\n#include <nav_msgs/Odometry.h>\n#include <pluginlib/class_loader.h>\n#include <ros/ros.h>\n\n// OpenCV\n#include <cv_bridge/cv_bridge.h>\n#include <opencv2/features2d/features2d.hpp>\n#include <opencv2/video/tracking.hpp>\n\n// dynamic reconfigure\n#include <costmap_converter/CostmapToDynamicObstaclesConfig.h>\n#include <dynamic_reconfigure/server.h>\n\n// Own includes\n#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>\n#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>\n#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>\n\n// STL\n#include <memory>\n\nnamespace costmap_converter {\n\n/**\n * @class CostmapToDynamicObstacles\n * @brief This class converts the costmap_2d into dynamic obstacles.\n *\n * Static obstacles are treated as point obstacles.\n */\nclass CostmapToDynamicObstacles : public BaseCostmapToDynamicObstacles\n{\npublic:\n  /**\n   * @brief Constructor\n   */\n  CostmapToDynamicObstacles();\n\n  /**\n   * @brief Destructor\n   */\n  virtual ~CostmapToDynamicObstacles();\n\n  /**\n   * @brief Initialize the plugin\n   * @param nh Nodehandle that defines the namespace for parameters\n   */\n  virtual void initialize(ros::NodeHandle nh);\n\n  /**\n   * @brief This method performs the actual work (conversion of the costmap to\n   * obstacles)\n   */\n  virtual void compute();\n\n  /**\n   * @brief Pass a pointer to the costmap to the plugin.\n   * @sa updateCostmap2D\n   * @param costmap Pointer to the costmap2d source\n   */\n  virtual void setCostmap2D(costmap_2d::Costmap2D* costmap);\n\n  /**\n   * @brief Get updated data from the previously set Costmap2D\n   * @sa setCostmap2D\n   */\n  virtual void updateCostmap2D();\n\n  /**\n   * @brief Get a shared instance of the current obstacle container\n   * @remarks If compute() or startWorker() has not been called before, this\n   * method returns an empty instance!\n   * @return Shared instance of the current obstacle container\n   */\n  ObstacleArrayConstPtr getObstacles();\n\n  /**\n   * @brief Set name of robot's odometry topic\n   *\n   * @warning The method must be called before initialize()\n   *\n   * Some plugins might require odometry information\n   * to compensate the robot's ego motion\n   * @param odom_topic topic name\n   */\n  virtual void setOdomTopic(const std::string& odom_topic)\n  {\n    odom_topic_ = odom_topic;\n  }\n\n  /**\n   * @brief OpenCV Visualization\n   * @param name  Id/name of the opencv window\n   * @param image Image to be visualized\n   */\n  void visualize(const std::string& name, const cv::Mat& image);\n\nprotected:\n  /**\n   * @brief Converts the estimated velocity of a tracked object to m/s and\n   * returns it\n   * @param idx Index of the tracked object in the tracker\n   * @return Point_t, which represents velocity in [m/s] of object(idx) in x,y,z\n   * coordinates\n   */\n  Point_t getEstimatedVelocityOfObject(unsigned int idx);\n\n  /**\n   * @brief Gets the last observed contour of a object and converts it from [px]\n   * to [m]\n   * @param[in]  idx Index of the tracked object in the tracker\n   * @param[out] contour vector of Point_t, which represents the last observed contour in [m]\n   *             in x,y,z coordinates\n   */\n  void getContour(unsigned int idx, std::vector<Point_t>& contour);\n\n  /**\n   * @brief Thread-safe update of the internal obstacle container (that is\n   * shared using getObstacles() from outside this\n   * class)\n   * @param obstacles Updated obstacle container\n   */\n  void updateObstacleContainer(ObstacleArrayPtr obstacles);\n\nprivate:\n  boost::mutex mutex_;\n  costmap_2d::Costmap2D* costmap_;\n  cv::Mat costmap_mat_;\n  ObstacleArrayPtr obstacles_;\n  cv::Mat fg_mask_;\n  std::unique_ptr<BackgroundSubtractor> bg_sub_;\n  cv::Ptr<BlobDetector> blob_det_;\n  std::vector<cv::KeyPoint> keypoints_;\n  std::unique_ptr<CTracker> tracker_;\n  ros::Subscriber odom_sub_;\n  Point_t ego_vel_;\n\n  std::string odom_topic_ = \"/odom\";\n  bool publish_static_obstacles_ = true;\n\n  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>*\n      dynamic_recfg_; //!< Dynamic reconfigure server to allow config\n                       //! modifications at runtime\n\n  /**\n   * @brief Callback for the odometry messages of the observing robot.\n   *\n   * Used to convert the velocity of obstacles to the /map frame.\n   * @param msg The Pointer to the nav_msgs::Odometry of the observing robot\n   */\n  void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);\n\n  /**\n   * @brief Callback for the dynamic_reconfigure node.\n   *\n   * This callback allows to modify parameters dynamically at runtime without\n   * restarting the node\n   * @param config Reference to the dynamic reconfigure config\n   * @param level Dynamic reconfigure level\n   */\n  void reconfigureCB(CostmapToDynamicObstaclesConfig &config, uint32_t level);\n};\n\n} // end namespace costmap_converter\n\n#endif /* COSTMAP_TO_DYNAMIC_OBSTACLES_H_ */\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h",
    "content": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this directory.\n\n#pragma once\n#include \"Kalman.h\"\n#include \"HungarianAlg.h\"\n#include \"defines.h\"\n#include <iostream>\n#include <vector>\n#include <memory>\n#include <array>\n\n// --------------------------------------------------------------------------\nclass CTrack\n{\npublic:\n  CTrack(const Point_t& p, const std::vector<cv::Point>& contour, track_t dt, size_t trackID)\n      : track_id(trackID),\n        skipped_frames(0),\n        prediction(p),\n        lastContour(contour),\n        KF(p, dt)\n  {\n  }\n\n  track_t CalcDist(const Point_t& p)\n  {\n    Point_t diff = prediction - p;\n    return std::sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);\n  }\n\n  void Update(const Point_t& p, const std::vector<cv::Point>& contour, bool dataCorrect, size_t max_trace_length)\n  {\n    KF.Prediction();\n    prediction = KF.Update(p, dataCorrect);\n\n    if (dataCorrect)\n    {\n      lastContour = contour;\n    }\n\n    if (trace.size() > max_trace_length)\n    {\n      trace.erase(trace.begin(), trace.end() - max_trace_length);\n    }\n\n    trace.push_back(prediction);\n  }\n\n  // Returns contour in [px], not in [m]!\n  std::vector<cv::Point> getLastContour() const\n  {\n    return lastContour;\n  }\n\n  // Returns velocity in [px/s], not in [m/s]!\n  Point_t getEstimatedVelocity() const\n  {\n    return KF.LastVelocity;\n  }\n\n  std::vector<Point_t> trace;\n  size_t track_id;\n  size_t skipped_frames;\n\nprivate:\n  Point_t prediction;\n  std::vector<cv::Point> lastContour; // Contour liegt immer auf ganzen Pixeln -> Integer Punkt\n  TKalmanFilter KF;\n};\n\n// --------------------------------------------------------------------------\nclass CTracker\n{\npublic:\n  struct Params{\n    track_t dt; // time for one step of the filter\n    track_t dist_thresh;// distance threshold. Pairs of points with higher distance are not considered in the assignment problem\n    int max_allowed_skipped_frames; // Maximum number of frames the track is maintained without seeing the object in the measurement data\n    int max_trace_length; // Maximum trace length\n  };\n\n  CTracker(const Params& parameters);\n  ~CTracker(void);\n\n  std::vector<std::unique_ptr<CTrack>> tracks;\n  void Update(const std::vector<Point_t>& detectedCentroid, const std::vector<std::vector<cv::Point> > &contour);\n\n  void updateParameters(const Params &parameters);\n\nprivate:\n  // Aggregated parameters for the tracker object\n  Params params;\n  // ID for the upcoming CTrack object\n  size_t NextTrackID;\n};\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h",
    "content": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this directory.\n\n#include <vector>\n#include <iostream>\n#include <limits>\n#include <time.h>\n#include \"defines.h\"\n\n// http://community.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm\n\ntypedef std::vector<int> assignments_t;\ntypedef std::vector<track_t> distMatrix_t;\n\nclass AssignmentProblemSolver\n{\nprivate:\n  // --------------------------------------------------------------------------\n  // Computes the optimal assignment (minimum overall costs) using Munkres algorithm.\n  // --------------------------------------------------------------------------\n  void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,\n                         size_t nOfColumns);\n  void buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows, size_t nOfColumns);\n  void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn,\n                             size_t nOfRows);\n  void step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,\n              bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);\n  void step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,\n              bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);\n  void step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,\n             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);\n  void step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,\n             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row,\n             size_t col);\n  void step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,\n             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);\n  // --------------------------------------------------------------------------\n  // Computes a suboptimal solution. Good for cases with many forbidden assignments.\n  // --------------------------------------------------------------------------\n  void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,\n                             size_t nOfColumns);\n  // --------------------------------------------------------------------------\n  // Computes a suboptimal solution. Good for cases with many forbidden assignments.\n  // --------------------------------------------------------------------------\n  void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,\n                             size_t nOfColumns);\n\npublic:\n  enum TMethod\n  {\n    optimal,\n    many_forbidden_assignments,\n    without_forbidden_assignments\n  };\n\n  AssignmentProblemSolver();\n  ~AssignmentProblemSolver();\n  track_t Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns, assignments_t& assignment,\n                TMethod Method = optimal);\n};\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h",
    "content": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this directory.\n\n#pragma once\n#include \"defines.h\"\n#include <opencv2/opencv.hpp>\n\n// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/\nclass TKalmanFilter\n{\npublic:\n  TKalmanFilter(Point_t p, track_t deltatime = 0.2);\n  ~TKalmanFilter();\n  void Prediction();\n  Point_t Update(Point_t p, bool DataCorrect);\n  cv::KalmanFilter* kalman;\n  track_t dt;\n  Point_t LastPosition; // contour in [px]\n  Point_t LastVelocity; // velocity in [px/s]\n};\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md",
    "content": "Multitarget Tracker\n===================\n\nThis code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].\nIt is utilized for the *CostmapToDynamicObstacles* plugin.\n\nThe *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].\nThe package itself depends on other third party packages with different licenses (refer to the package repository).\n\nTODO: Include the whole package as external CMake project.\n\n\n\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h",
    "content": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this directory.\n\n#pragma once\n#include <opencv2/opencv.hpp>\n\ntypedef float track_t;\ntypedef cv::Point3_<track_t> Point_t;\n#define Mat_t CV_32FC\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_lines_convex_hull.h",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann, Otniel Rinaldo\n *********************************************************************/\n\n#ifndef COSTMAP_TO_LINES_CONVEX_HULL_H_\n#define COSTMAP_TO_LINES_CONVEX_HULL_H_\n\n#include <costmap_converter/costmap_converter_interface.h>\n#include <costmap_converter/costmap_to_polygons.h>\n\n// dynamic reconfigure\n#include <costmap_converter/CostmapToLinesDBSMCCHConfig.h>\n\nnamespace costmap_converter\n{\n  \n/**\n * @class CostmapToLinesDBSMCCH\n * @brief This class converts the costmap_2d into a set of lines (and points)\n * \n * The conversion is performed in three stages:\n * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)\n *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,\n *               A density-based algorithm for discovering clusters in large spatial databases with noise. \n *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. \n *    \n * 2. In the subsequent stage, the convex hull of each cluster is determined using the monotone chain algorithm aka Andrew's algorithm:\n *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )\n *    Reference:  A. M. Andrew, \"Another Efficient Algorithm for Convex Hulls in Two Dimensions\", Info. Proc. Letters 9, 216-219 (1979).\n * \n * 3. In the third step extract lines from each polygon (convex hull) if there exist at least a predefined number of support points.\n * \n * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)\n */\n  class CostmapToLinesDBSMCCH : public CostmapToPolygonsDBSMCCH\n  {\n  public:\n   \n    /**\n     * @brief Constructor\n     */\n    CostmapToLinesDBSMCCH();\n       \n    /**\n     * @brief Destructor\n     */\n    virtual ~CostmapToLinesDBSMCCH();\n    \n    /**\n     * @brief Initialize the plugin\n     * @param nh Nodehandle that defines the namespace for parameters\n     */\n    virtual void initialize(ros::NodeHandle nh);\n    \n    /**\n     * @brief This method performs the actual work (conversion of the costmap to polygons)\n     */\n    virtual void compute();   \n    \n    \n  protected:\n    \n    /**\n     * @brief Extract points and lines from a given cluster by checking all keypoint pairs of the convex hull for a minimum number of support points\n     * @param cluster list of points in the cluster\n     * @param polygon convex hull of the cluster \\c cluster\n     * @param[out] lines back_inserter object to a sequence of polygon msgs (new lines will be pushed back)\n     */\n    void extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::Polygon& polygon, std::back_insert_iterator< std::vector<geometry_msgs::Polygon> > lines);\n\n    \n    \n  protected:\n      \n    double support_pts_max_dist_inbetween_;\n    double support_pts_max_dist_;\n    int min_support_pts_;\n   \n  private:\n    \n    /**\n     * @brief Callback for the dynamic_reconfigure node.\n     * \n     * This callback allows to modify parameters dynamically at runtime without restarting the node\n     * @param config Reference to the dynamic reconfigure config\n     * @param level Dynamic reconfigure level\n     */\n    void reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level);\n    \n    dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime   \n \n  };  \n  \n  \n  \n  \n\n\n  \n  \n} //end namespace teb_local_planner\n\n#endif /* COSTMAP_TO_LINES_CONVEX_HULL_H_ */\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_lines_ransac.h",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann\n *********************************************************************/\n\n#ifndef COSTMAP_TO_LINES_RANSAC_H_\n#define COSTMAP_TO_LINES_RANSAC_H_\n\n#include <costmap_converter/costmap_converter_interface.h>\n#include <costmap_converter/costmap_to_polygons.h>\n#include <costmap_converter/misc.h>\n#include <boost/random.hpp>\n\n#include <costmap_converter/CostmapToLinesDBSRANSACConfig.h>\n\nnamespace costmap_converter\n{\n  \n/**\n * @class CostmapToLinesDBSRANSAC\n * @brief This class converts the costmap_2d into a set of lines (and points)\n * \n * The conversion is performed in two stages:\n * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)\n *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,\n *               A density-based algorithm for discovering clusters in large spatial databases with noise. \n *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. \n *    \n * 2. The RANSAC algorithm is used to find straight line segment models (https://en.wikipedia.org/wiki/RANSAC)\n *    RANSAC is called repeatedly to find multiple lines per cluster until the number of inliners is below a specific threshold.\n *    In that case the remaining outliers are used as points or keypoints of their convex hull are used as points (depending on a paramter).\n *    The latter one applies as a filter. The convex assumption is not strict in practice, since the remaining regions/cluster (line inliers are removed)\n *    should be dense and small. For details about the convex hull algorithm, refer to costmap_converter::CostmapToPolygonsDBSMCCH.\n *    Resulting lines of RANSAC are currently not refined by linear regression.\n * \n * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)\n */\n  class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH\n  {\n  public:\n   \n    /**\n     * @brief Constructor\n     */\n    CostmapToLinesDBSRANSAC();\n       \n    /**\n     * @brief Destructor\n     */\n    virtual ~CostmapToLinesDBSRANSAC();\n    \n    /**\n     * @brief Initialize the plugin\n     * @param nh Nodehandle that defines the namespace for parameters\n     */\n    virtual void initialize(ros::NodeHandle nh);\n    \n    /**\n     * @brief This method performs the actual work (conversion of the costmap to polygons)\n     */\n    virtual void compute();   \n        \n     \n    /**\n     * @brief Check if the candidate point is an inlier.\n     * @param point generic 2D point type defining the reference point\n     * @param line_start generic 2D point type defining the start of the line\n     * @param line_end generic 2D point type defining the end of the line\n     * @param min_distance minimum distance allowed\n     * @tparam Point generic point type that should provide (writable) x and y member fiels.\n     * @tparam LinePoint generic point type that should provide (writable) x and y member fiels.\n     * @return \\c true if inlier, \\c false otherwise\n     */\n    template <typename Point, typename LinePoint>\n    static bool isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance);\n    \n  protected:\n    \n    double ransac_inlier_distance_; //!< Maximum distance to the line segment for inliers\n    int ransac_min_inliers_; //!< Minimum numer of inliers required to form a line\n    int ransac_no_iterations_; //!< Number of ransac iterations\n    int ransac_remainig_outliers_; //!< Repeat ransac until the number of outliers is as specified here\n    bool ransac_convert_outlier_pts_; //!< If \\c true, convert remaining outliers to single points.\n    bool ransac_filter_remaining_outlier_pts_; //!< If \\c true, filter the interior of remaining outliers and keep only keypoints of their convex hull\n   \n   \n   boost::random::mt19937 rnd_generator_; //!< Random number generator for ransac with default seed\n    \n    \n    /**\n     * @brief Find a straight line segment in a point cloud with RANSAC (without linear regression).\n     * @param data set of 2D data points\n     * @param inlier_distance maximum distance that inliers must satisfy\n     * @param no_iterations number of RANSAC iterations\n     * @param min_inliers minimum number of inliers to return true\n     * @param[out] best_model start and end of the best straight line segment \n     * @param[out] inliers inlier keypoints are written to this container [optional]\n     * @param[out] outliers outlier keypoints are written to this container [optional]\n     * @return \\c false, if \\c min_inliers is not satisfied, \\c true otherwise\n     */\n    bool lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations, int min_inliers,\n                    std::pair<KeyPoint, KeyPoint>& best_model, std::vector<KeyPoint>* inliers = NULL,\n                     std::vector<KeyPoint>* outliers = NULL);\n    \n    /**\n     * @brief Perform a simple linear regression in order to fit a straight line 'y = slope*x + intercept'\n     * @param data set of 2D data points\n     * @param[out] slope The slope of the fitted line \n     * @param[out] intercept The intercept / offset of the line\n     * @param[out] mean_x_out mean of the x-values of the data [optional]\n     * @param[out] mean_y_out mean of the y-values of the data [optional]\n     * @return \\c true, if a valid line has been fitted, \\c false otherwise.\n     */\n    bool linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept,\n                          double* mean_x_out = NULL, double* mean_y_out = NULL);\n    \n    \n    \n//     void adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2, \n//                           KeyPoint& line_start, KeyPoint& line_end);\n    \n    private:\n    \n    /**\n     * @brief Callback for the dynamic_reconfigure node.\n     * \n     * This callback allows to modify parameters dynamically at runtime without restarting the node\n     * @param config Reference to the dynamic reconfigure config\n     * @param level Dynamic reconfigure level\n     */\n    void reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level);\n    \n    dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime   \n    \n  };  \n  \n  \n\ntemplate <typename Point, typename LinePoint>\nbool CostmapToLinesDBSRANSAC::isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance)\n{\n  bool is_inbetween = false;\n  double distance = computeDistanceToLineSegment(point, line_start, line_end, &is_inbetween);\n  if (!is_inbetween)\n    return false;\n  if (distance <= min_distance)\n    return true;\n  return false;\n}\n     \n  \n} //end namespace teb_local_planner\n\n#endif /* COSTMAP_TO_LINES_RANSAC_H_ */\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_polygons.h",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann, Otniel Rinaldo\n *********************************************************************/\n\n#ifndef COSTMAP_TO_POLYGONS_H_\n#define COSTMAP_TO_POLYGONS_H_\n\n#include <ros/ros.h>\n#include <costmap_converter/costmap_converter_interface.h>\n#include <nav_msgs/OccupancyGrid.h>\n#include <visualization_msgs/Marker.h>\n#include <geometry_msgs/Point.h>\n#include <geometry_msgs/Polygon.h>\n#include <vector>\n#include <algorithm>\n#include <Eigen/Core>\n#include <Eigen/StdVector>\n#include <boost/thread/mutex.hpp>\n#include <boost/thread/thread.hpp>\n\n// dynamic reconfigure\n#include <costmap_converter/CostmapToPolygonsDBSMCCHConfig.h>\n#include <dynamic_reconfigure/server.h>\n\n\nnamespace costmap_converter\n{\n  \n/**\n * @class CostmapToPolygonsDBSMCCH\n * @brief This class converts the costmap_2d into a set of convex polygons (and points)\n * \n * The conversion is performed in two stages:\n * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)\n *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,\n *               A density-based algorithm for discovering clusters in large spatial databases with noise. \n *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. \n *    \n * 2. In the subsequent stage, clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:\n *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )\n *    Reference:  A. M. Andrew, \"Another Efficient Algorithm for Convex Hulls in Two Dimensions\", Info. Proc. Letters 9, 216-219 (1979).\n *  \n * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)\n */\nclass CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons\n{\n  public:\n    \n    /**\n     * @struct KeyPoint\n     * @brief Defines a keypoint in metric coordinates of the map \n     */\n    struct KeyPoint\n    {\n      //! Default constructor\n      KeyPoint() {}\n      //! Constructor with point initialization\n      KeyPoint(double x_, double y_) : x(x_), y(y_) {}\n      double x; //!< x coordinate [m]\n      double y; //!< y coordinate [m]\n      \n      //! Convert keypoint to geometry_msgs::Point message type\n      void toPointMsg(geometry_msgs::Point& point) const {point.x=x; point.y=y; point.z=0;}\n      //! Convert keypoint to geometry_msgs::Point32 message type\n      void toPointMsg(geometry_msgs::Point32& point) const {point.x=x; point.y=y; point.z=0;}\n    };\n\n    /**\n     * @struct Parameters\n     * @brief Defines the parameters of the algorithm\n     */\n    struct Parameters\n    {\n      Parameters() : max_distance_(0.4), min_pts_(2), max_pts_(30), min_keypoint_separation_(0.1) {}\n      // DBSCAN parameters\n      double max_distance_; //!< Parameter for DB_Scan, maximum distance to neighbors [m]\n      int min_pts_; //!< Parameter for DB_Scan: minimum number of points that define a cluster\n      int max_pts_; //!< Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes)\n      \n      // convex hull parameters\n      double min_keypoint_separation_; //!< Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)\n    };\n    \n    /**\n     * @brief Constructor\n     */\n    CostmapToPolygonsDBSMCCH();\n    \n    /**\n     * @brief Destructor\n     */\n    virtual ~CostmapToPolygonsDBSMCCH();\n    \n    /**\n     * @brief Initialize the plugin\n     * @param nh Nodehandle that defines the namespace for parameters\n     */\n    virtual void initialize(ros::NodeHandle nh);\n    \n    /**\n     * @brief This method performs the actual work (conversion of the costmap to polygons)\n     */\n    virtual void compute();\n        \n    /**\n     * @brief Pass a pointer to the costap to the plugin.\n     * @sa updateCostmap2D\n     * @param costmap Pointer to the costmap2d source\n     */\n    virtual void setCostmap2D(costmap_2d::Costmap2D* costmap);\n    \n    /**\n     * @brief Get updated data from the previously set Costmap2D\n     * @sa setCostmap2D\n     */\n    virtual void updateCostmap2D();\n    \n    \n    /**\n     * @brief Convert a generi point type to a geometry_msgs::Polygon\n     * @param point generic 2D point type\n     * @param[out] polygon already instantiated polygon that will be filled with a single point\n     * @tparam Point generic point type that should provide (writable) x and y member fiels.\n     */\n    template< typename Point>\n    static void convertPointToPolygon(const Point& point, geometry_msgs::Polygon& polygon)\n    {\n      polygon.points.resize(1);\n      polygon.points.front().x = point.x;\n      polygon.points.front().y = point.y;\n      polygon.points.front().z = 0;\n    }\n    \n    /**\n     * @brief Get a shared instance of the current polygon container\n     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!\n     * @return Shared instance of the current polygon container\n     */\n    PolygonContainerConstPtr getPolygons();\n\n    \n    \n  protected:\n    \n    /**\n     * @brief DBSCAN algorithm for clustering \n     * \n     * Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)\n     *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,\n     *               A density-based algorithm for discovering clusters in large spatial databases with noise. \n     *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. \n     * \n     * @param[out] clusters clusters will added to this output-argument (a sequence of keypoints for each cluster)\n     *                      the first cluster (clusters[0]) will contain all noise points (that does not fulfil the min_pts condition \n     */\n    void dbScan(std::vector< std::vector<KeyPoint> >& clusters);\n    \n    /**\n     * @brief Helper function for dbScan to search for neighboring points \n     * \n     * @param curr_index index to the current item in \\c occupied_cells\n     * @param[out] neighbor_indices list of neighbors (indices of \\c occupied cells)\n     */\n    void regionQuery(int curr_index, std::vector<int>& neighbor_indices);\n\n    /**\n     * @brief helper function for adding a point to the lookup data structures\n     */\n    void addPoint(double x, double y)\n    {\n      int idx = occupied_cells_.size();\n      occupied_cells_.emplace_back(x, y);\n      int cx, cy;\n      pointToNeighborCells(occupied_cells_.back(), cx, cy);\n      int nidx = neighborCellsToIndex(cx, cy);\n      if (nidx >= 0)\n        neighbor_lookup_[nidx].push_back(idx);\n    }\n    \n    /**\n     * @brief Compute the convex hull for a single cluster (monotone chain algorithm)\n     * \n     * Clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:\n     *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )\n     *    Reference:  A. M. Andrew, \"Another Efficient Algorithm for Convex Hulls in Two Dimensions\", Info. Proc. Letters 9, 216-219 (1979).\n     * @remarks The algorithm seems to cut edges, thus we give a try to convexHull2().\n     * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)\n     * @remarks The last point is the same as the first one\n     * @param cluster list of keypoints that should be converted into a polygon\n     * @param[out] polygon the resulting convex polygon\n     */  \n    void convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon);\n    \n    /**\n     * @brief Compute the convex hull for a single cluster\n     * \n     * Clusters are converted into convex polgons using an algorithm provided here:\n     *  https://bitbucket.org/vostreltsov/concave-hull/overview\n     * The convex hull algorithm is part of the concave hull algorithm.\n     * The license is WTFPL 2.0 and permits any usage.\n     * \n     * @remarks The last point is the same as the first one\n     * @param cluster list of keypoints that should be converted into a polygon\n     * @param[out] polygon the resulting convex polygon\n     * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)\n     */  \n    void convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon);\n\n    /**\n     * @brief Simplify a polygon by removing points.\n     * \n     * We apply the Douglas-Peucker Algorithm to simplify the edges of the polygon.\n     * Internally, the parameter min_keypoint_separation is used for deciding whether\n     * a point is considered close to an edge and to be merged into the line.\n     */\n    void simplifyPolygon(geometry_msgs::Polygon& polygon);\n    \n   /**\n    * @brief 2D Cross product of two vectors defined by two points and a common origin\n    * @param O Origin\n    * @param A First point\n    * @param B Second point\n    * @tparam P1 2D Point type with x and y member fields\n    * @tparam P2 2D Point type with x and y member fields\n    * @tparam P3 2D Point type with x and y member fields\n    */    \n    template <typename P1, typename P2, typename P3>\n    long double cross(const P1& O, const P2& A, const P3& B)\n    { \n        return (long double)(A.x - O.x) * (long double)(B.y - O.y) - (long double)(A.y - O.y) * (long double)(B.x - O.x);\n    }\n    \n      \n   /**\n    * @brief Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class)\n    * @param polygons Updated polygon container\n    */\n   void updatePolygonContainer(PolygonContainerPtr polygons);   \n          \n   \n   std::vector<KeyPoint> occupied_cells_; //!< List of occupied cells in the current map (updated by updateCostmap2D())\n   \n    std::vector<std::vector<int> > neighbor_lookup_; //! array of cells for neighbor lookup\n    int neighbor_size_x_; //! size of the neighbour lookup in x (number of cells)\n    int neighbor_size_y_; //! size of the neighbour lookup in y (number of cells)\n    double offset_x_;     //! offset [meters] in x for the lookup grid\n    double offset_y_;     //! offset [meters] in y for the lookup grid\n\n    /**\n      * @brief convert a 2d cell coordinate into the 1D index of the array\n      * @param cx the x index of the cell\n      * @param cy the y index of the cell\n      */\n    int neighborCellsToIndex(int cx, int cy)\n    {\n      if (cx < 0 || cx >= neighbor_size_x_ || cy < 0 || cy >= neighbor_size_y_)\n        return -1;\n      return cy * neighbor_size_x_ + cx;\n    }\n\n    /**\n      * @brief compute the cell indices of a keypoint\n      * @param kp key point given in world coordinates [m, m]\n      * @param cx output cell index in x direction\n      * @param cy output cell index in y direction\n      */\n    void pointToNeighborCells(const KeyPoint& kp, int& cx, int& cy)\n    {\n      cx = int((kp.x - offset_x_) / parameter_.max_distance_);\n      cy = int((kp.y - offset_y_) / parameter_.max_distance_);\n    }\n\n\n    Parameters parameter_;          //< active parameters throughout computation\n    Parameters parameter_buffered_; //< the buffered parameters that are offered to dynamic reconfigure\n    boost::mutex parameter_mutex_;  //!< Mutex that keeps track about the ownership of the shared polygon instance\n   \n  private:\n       \n    /**\n     * @brief Callback for the dynamic_reconfigure node.\n     * \n     * This callback allows to modify parameters dynamically at runtime without restarting the node\n     * @param config Reference to the dynamic reconfigure config\n     * @param level Dynamic reconfigure level\n     */\n    void reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level);\n    \n    \n    PolygonContainerPtr polygons_; //!< Current shared container of polygons\n    boost::mutex mutex_; //!< Mutex that keeps track about the ownership of the shared polygon instance\n    \n    dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime    \n   \n    costmap_2d::Costmap2D *costmap_; //!< Pointer to the costmap2d\n   \n}; \n\n  \n} //end namespace teb_local_planner\n\n#endif /* COSTMAP_TO_POLYGONS_H_ */\n"
  },
  {
    "path": "include/costmap_converter/costmap_to_polygons_concave.h",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann\n *********************************************************************/\n\n#ifndef COSTMAP_TO_POLYGONS_CONCAVE_H_\n#define COSTMAP_TO_POLYGONS_CONCAVE_H_\n\n#include <costmap_converter/costmap_to_polygons.h>\n#include <costmap_converter/misc.h>\n\n// dynamic reconfigure\n#include <costmap_converter/CostmapToPolygonsDBSConcaveHullConfig.h>\n#include <dynamic_reconfigure/server.h>\n\n\nnamespace costmap_converter\n{\n  \n/**\n * @class CostmapToPolygonsDBSConcaveHull\n * @brief This class converts the costmap_2d into a set of non-convex polygons (and points)\n *  \n * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)\n * @todo change the class hierarchy to a clearer and more generic one\n */\nclass CostmapToPolygonsDBSConcaveHull : public CostmapToPolygonsDBSMCCH\n{\n  public:\n    \n    \n    \n    /**\n     * @brief Constructor\n     */\n    CostmapToPolygonsDBSConcaveHull();\n    \n    /**\n     * @brief Destructor\n     */\n    virtual ~CostmapToPolygonsDBSConcaveHull();\n    \n    /**\n     * @brief Initialize the plugin\n     * @param nh Nodehandle that defines the namespace for parameters\n     */\n    virtual void initialize(ros::NodeHandle nh);\n   \n    \n    /**\n     * @brief This method performs the actual work (conversion of the costmap to polygons)\n     */\n    virtual void compute();\n    \n  protected:\n \n   \n    /**\n     * @brief Compute the concave hull for a single cluster\n     * \n     * @remarks The last point is the same as the first one\n     * @param cluster list of keypoints that should be converted into a polygon\n     * @param depth Smaller depth: sharper surface, depth -> high value: convex hull\n     * @param[out] polygon the resulting convex polygon\n     */  \n    void concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon);\n        \n    void concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon);\n    \n    template <typename PointLine, typename PointCluster, typename PointHull>\n    std::size_t findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found);\n        \n    \n    template <typename Point1, typename Point2, typename Point3, typename Point4>\n    bool checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end);\n    \n    template <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>\n    bool checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start, \n                               const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end);\n    \n    double concave_hull_depth_;\n\n  private:\n       \n    /**\n     * @brief Callback for the dynamic_reconfigure node.\n     * \n     * This callback allows to modify parameters dynamically at runtime without restarting the node\n     * @param config Reference to the dynamic reconfigure config\n     * @param level Dynamic reconfigure level\n     */\n    void reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level);\n    \n    dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime    \n   \n   \n}; \n\n\ntemplate <typename PointLine, typename PointCluster, typename PointHull>\nstd::size_t CostmapToPolygonsDBSConcaveHull::findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found)\n{\n    std::size_t nearsest_idx = 0;\n    double distance = 0;\n    *found = false;\n\n    for (std::size_t i = 0; i < cluster.size(); ++i)\n    {\n        // Skip points that are already in the hull\n        if (std::find_if( hull.begin(), hull.end(), boost::bind(isApprox2d<PointHull, PointCluster>, _1, boost::cref(cluster[i]), 1e-5) ) != hull.end() )\n            continue;\n\n        double dist = computeDistanceToLineSegment(cluster[i], line_start, line_end);\n        bool skip = false;\n        for (int j = 0; !skip && j < (int)hull.size() - 1; ++j) \n        {\n            double dist_temp = computeDistanceToLineSegment(cluster[i], hull[j], hull[j + 1]);\n            skip |= dist_temp < dist;\n        }\n        if (skip) \n            continue;\n\n        if (!(*found) || dist < distance) \n        {\n            nearsest_idx = i;\n            distance = dist;\n            *found = true;\n        }\n    }\n    return nearsest_idx;\n}\n\n\ntemplate <typename Point1, typename Point2, typename Point3, typename Point4>\nbool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end)\n{\n    double dx1 = line1_end.x - line1_start.x;\n    double dy1 = line1_end.y - line1_start.y;\n    double dx2 = line2_end.x - line2_start.x;\n    double dy2 = line2_end.y - line2_start.y;\n    double s = (-dy1 * (line1_start.x - line2_start.x) + dx1 * (line1_start.y - line2_start.y)) / (-dx2 * dy1 + dx1 * dy2);\n    double t = ( dx2 * (line1_start.y - line2_start.y) - dy2 * (line1_start.x - line2_start.x)) / (-dx2 * dy1 + dx1 * dy2);\n    return (s > 0 && s < 1 && t > 0 && t < 1);\n}\n\n\ntemplate <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>\nbool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start, \n                                                            const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end)\n{\n    for (int i = 0; i < (int)hull.size() - 2; i++) \n    {\n        if (isApprox2d(current_line_start, hull[i], 1e-5) && isApprox2d(current_line_end, hull[i+1], 1e-5))\n        {\n            continue;\n        }\n\n        if (checkLineIntersection(test_line_start, test_line_end, hull[i], hull[i+1])) \n        {\n            return true;\n        }\n    }\n    return false;\n} \n  \n  \n} //end namespace teb_local_planner\n\n#endif /* COSTMAP_TO_POLYGONS_CONCAVE_H_ */\n"
  },
  {
    "path": "include/costmap_converter/misc.h",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann\n *********************************************************************/\n\n#ifndef MISC_H_\n#define MISC_H_\n\n#include <algorithm>\n#include <cmath>\n\nnamespace costmap_converter\n{\n\n/**\n  * @brief Calculate the distance between a point and a straight line (with infinite length)\n  * @param point generic 2D point type defining the reference point\n  * @param line_pt1 generic 2D point as part of the line\n  * @param line_pt2 generic 2D point as part of the line\n  * @tparam Point generic point type that should provide x and y member fiels.\n  * @tparam LinePoint generic point type that should provide x and y member fiels.\n  * @return (minimum) euclidean distance to the line segment\n  */\ntemplate <typename Point, typename LinePoint>\ninline double computeDistanceToLine(const Point& point, const LinePoint& line_pt1, const LinePoint& line_pt2)\n{\n    double dx = line_pt2.x - line_pt1.x;\n    double dy = line_pt2.y - line_pt1.y;\n    \n    double length = std::sqrt(dx*dx + dy*dy);\n    \n    if (length>0)\n      return std::abs(dy * point.x - dx * point.y + line_pt2.x * line_pt1.y - line_pt2.y * line_pt1.x) / length;\n  \n    return std::sqrt(std::pow(point.x - line_pt1.x, 2) + std::pow(point.y - line_pt1.y, 2));  \n}\n\n\n/**\n  * @brief Calculate the squared distance between a point and a straight line segment\n  * @param point generic 2D point type defining the reference point\n  * @param line_start generic 2D point type defining the start of the line\n  * @param line_end generic 2D point type defining the end of the line\n  * @param[out] is_inbetween write \\c true, if the point is placed inbetween start and end [optional]\n  * @tparam Point generic point type that should provide x and y member fiels.\n  * @tparam LinePoint generic point type that should provide x and y member fiels.\n  * @return (minimum) squared euclidean distance to the line segment\n  */\ntemplate <typename Point, typename LinePoint>\ninline double computeSquaredDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)\n{\n    double dx = line_end.x - line_start.x;\n    double dy = line_end.y - line_start.y;\n    \n    double length_sqr = dx*dx + dy*dy;\n\n    double u = 0;\n\n    if (length_sqr > 0)\n      u = ((point.x - line_start.x) * dx + (point.y - line_start.y) * dy) / length_sqr;\n\n    if (is_inbetween)\n      *is_inbetween = (u>=0 && u<=1);\n  \n    if (u <= 0)\n      return std::pow(point.x-line_start.x,2) + std::pow(point.y-line_start.y,2);\n    \n    if (u >= 1)\n      return std::pow(point.x-line_end.x,2) + std::pow(point.y-line_end.y,2);\n    \n    return std::pow(point.x - (line_start.x+u*dx) ,2) + std::pow(point.y - (line_start.y+u*dy),2);\n}\n  \n/**\n  * @brief Calculate the distance between a point and a straight line segment\n  * @param point generic 2D point type defining the reference point\n  * @param line_start generic 2D point type defining the start of the line\n  * @param line_end generic 2D point type defining the end of the line\n  * @param[out] is_inbetween write \\c true, if the point is placed inbetween start and end [optional]\n  * @tparam Point generic point type that should provide x and y member fiels.\n  * @tparam LinePoint generic point type that should provide x and y member fiels.\n  * @return (minimum) euclidean distance to the line segment\n  */\ntemplate <typename Point, typename LinePoint>\ninline double computeDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)\n{\n  return std::sqrt(computeSquaredDistanceToLineSegment(point, line_start, line_end, is_inbetween));\n}\n  \n\n/**\n  * @brief Calculate the distance between two 2d points\n  * @param pt1 generic 2D point\n  * @param pt2 generic 2D point\n  * @tparam Point1 generic point type that should provide x and y member fiels.\n  * @tparam Point2 generic point type that should provide x and y member fiels.\n  * @return euclidean distance to the line segment\n  */\ntemplate <typename Point1, typename Point2>  \ninline double norm2d(const Point1& pt1, const Point2& pt2)\n{\n  return std::sqrt( std::pow(pt2.x - pt1.x, 2) + std::pow(pt2.y - pt1.y, 2)  );\n}\n\n/**\n  * @brief Check if two points are approximately defining the same one\n  * @param pt1 generic 2D point\n  * @param pt2 generic 2D point\n  * @param threshold define the minimum threshold |pt1.x-pt2.y| < tresh && |pt1.y-pt2.y| < tresh\n  * @tparam Point1 generic point type that should provide x and y member fiels.\n  * @tparam Point2 generic point type that should provide x and y member fiels.\n  * @return \\c true, if similar, \\c false otherwise\n  */\ntemplate <typename Point1, typename Point2>  \ninline bool isApprox2d(const Point1& pt1, const Point2& pt2, double threshold)\n{\n  return ( std::abs(pt2.x-pt1.x)<threshold && std::abs(pt2.y-pt1.y)<threshold );\n}\n\n\n  \n} //end namespace teb_local_planner\n\n#endif /* MISC_H_ */\n"
  },
  {
    "path": "msg/ObstacleArrayMsg.msg",
    "content": "# Message that contains a list of polygon shaped obstacles.\n# Special types:\n# Polygon with 1 vertex: Point obstacle\n# Polygon with 2 vertices: Line obstacle\n# Polygon with more than 2 vertices: First and last points are assumed to be connected\n\nstd_msgs/Header header\n\ncostmap_converter/ObstacleMsg[] obstacles\n\n"
  },
  {
    "path": "msg/ObstacleMsg.msg",
    "content": "# Special types:\n# Polygon with 1 vertex: Point obstacle (you might also specify a non-zero value for radius)\n# Polygon with 2 vertices: Line obstacle\n# Polygon with more than 2 vertices: First and last points are assumed to be connected\n\nstd_msgs/Header header\n\n# Obstacle footprint (polygon descriptions)\ngeometry_msgs/Polygon polygon\n\n# Specify the radius for circular/point obstacles\nfloat64 radius\n\n# Obstacle ID\n# Specify IDs in order to provide (temporal) relationships\n# between obstacles among multiple messages.\nint64 id\n\n# Individual orientation (centroid)\ngeometry_msgs/Quaternion orientation\n\n# Individual velocities (centroid)\ngeometry_msgs/TwistWithCovariance velocities\n\n"
  },
  {
    "path": "package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n  <name>costmap_converter</name>\n  <version>0.0.13</version>\n  <description>\n    A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types.\n  </description>\n\n  <maintainer email=\"christoph.roesmann@tu-dortmund.de\">Christoph Rösmann</maintainer>\n\n  <author email=\"christoph.roesmann@tu-dortmund.de\">Christoph Rösmann</author>\n  <author email=\"franz.albers@tu-dortmund.de\">Franz Albers</author>\n  <author email=\"otniel.rinaldo@tu-dortmund.de\">Otniel Rinaldo</author> \n\n  <license>BSD</license>\n\n  <url type=\"website\">http://wiki.ros.org/costmap_converter</url>\n  \n  <buildtool_depend>catkin</buildtool_depend>\n\n  <build_depend>message_generation</build_depend>\n\n  <build_export_depend>message_runtime</build_export_depend>\n  <exec_depend>message_runtime</exec_depend>\n\n  <test_depend>rostest</test_depend>\n\n  <depend>geometry_msgs</depend>\n  <depend>roscpp</depend>\n  <depend>std_msgs</depend>\n  <depend>costmap_2d</depend>\n  <depend>dynamic_reconfigure</depend>\n  <depend>pluginlib</depend>\n  <depend>cv_bridge</depend>\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n    <costmap_converter plugin=\"${prefix}/plugins.xml\" />\n  </export>\n</package>\n"
  },
  {
    "path": "plugins.xml",
    "content": "<library path=\"lib/libcostmap_converter\">\n  \n  <class type=\"costmap_converter::CostmapToPolygonsDBSMCCH\" base_class_type=\"costmap_converter::BaseCostmapToPolygons\">\n  <description>\n      This class converts costmap2d obstacles into a vector of convex polygons. \n      Clustering is performed with DBScan. Convex polygons are detected using the Monotone Chain Convex Hull algorithm.\n  </description>\n  </class>\n  \n  <class type=\"costmap_converter::CostmapToLinesDBSMCCH\" base_class_type=\"costmap_converter::BaseCostmapToPolygons\">\n  <description>\n      This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). \n      Clustering is performed with DBScan. Clusters are transformed into convex polygons using the Monotone Chain Convex Hull algorithm.\n      The resulting keypoints are used for possible line candidates.\n      A line is only defined if there exist a specified number of support points between each keypoint pair.\n  </description>\n  </class>\n  \n  <class type=\"costmap_converter::CostmapToLinesDBSRANSAC\" base_class_type=\"costmap_converter::BaseCostmapToPolygons\">\n  <description>\n      This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). \n      Clustering is performed with DBScan. For each cluster RANSAC is applied sequentally to fit multiple line models.\n  </description>\n  </class>\n  \n  <class type=\"costmap_converter::CostmapToPolygonsDBSConcaveHull\" base_class_type=\"costmap_converter::BaseCostmapToPolygons\">\n  <description>\n      This class converts costmap2d obstacles into a vector of non-convex (concave) polygons. \n  </description>\n  </class>\n\n  <class type=\"costmap_converter::CostmapToDynamicObstacles\" base_class_type=\"costmap_converter::BaseCostmapToPolygons\">\n  <description>\n      This class detects and tracks obstacles from a sequence of costmaps. \n  </description>\n  </class>\n  \n</library>\n"
  },
  {
    "path": "src/costmap_converter_node.cpp",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann, Otniel Rinaldo\n *********************************************************************/\n\n#include <ros/ros.h>\n\n#include <costmap_2d/costmap_2d.h>\n#include <nav_msgs/OccupancyGrid.h>\n#include <geometry_msgs/PolygonStamped.h>\n#include <visualization_msgs/Marker.h>\n\n#include <costmap_converter/costmap_converter_interface.h>\n#include <pluginlib/class_loader.h>\n\n\n\nclass CostmapStandaloneConversion\n{\npublic:\n  CostmapStandaloneConversion() : converter_loader_(\"costmap_converter\", \"costmap_converter::BaseCostmapToPolygons\"), n_(\"~\")\n  {\n      // load converter plugin from parameter server, otherwise set default\n      std::string converter_plugin = \"costmap_converter::CostmapToPolygonsDBSMCCH\";\n      n_.param(\"converter_plugin\", converter_plugin, converter_plugin);\n\n      try\n      {\n        converter_ = converter_loader_.createInstance(converter_plugin);\n      }\n      catch(const pluginlib::PluginlibException& ex)\n      {\n        ROS_ERROR(\"The plugin failed to load for some reason. Error: %s\", ex.what());\n        ros::shutdown();\n      }\n\n      ROS_INFO_STREAM(\"Standalone costmap converter:\" << converter_plugin << \" loaded.\");\n\n      std::string costmap_topic = \"/move_base/local_costmap/costmap\";\n      n_.param(\"costmap_topic\", costmap_topic, costmap_topic);\n\n      std::string costmap_update_topic = \"/move_base/local_costmap/costmap_updates\";\n      n_.param(\"costmap_update_topic\", costmap_update_topic, costmap_update_topic);\n\n      std::string obstacles_topic = \"costmap_obstacles\";\n      n_.param(\"obstacles_topic\", obstacles_topic, obstacles_topic);\n\n      std::string polygon_marker_topic = \"costmap_polygon_markers\";\n      n_.param(\"polygon_marker_topic\", polygon_marker_topic, polygon_marker_topic);\n\n      costmap_sub_ = n_.subscribe(costmap_topic, 1, &CostmapStandaloneConversion::costmapCallback, this);\n      costmap_update_sub_ = n_.subscribe(costmap_update_topic, 1, &CostmapStandaloneConversion::costmapUpdateCallback, this);\n      obstacle_pub_ = n_.advertise<costmap_converter::ObstacleArrayMsg>(obstacles_topic, 1000);\n      marker_pub_ = n_.advertise<visualization_msgs::Marker>(polygon_marker_topic, 10);\n\n      occupied_min_value_ = 100;\n      n_.param(\"occupied_min_value\", occupied_min_value_, occupied_min_value_);\n\n      std::string odom_topic = \"/odom\";\n      n_.param(\"odom_topic\", odom_topic, odom_topic);\n\n      if (converter_)\n      {\n        converter_->setOdomTopic(odom_topic);\n        converter_->initialize(n_);\n        converter_->setCostmap2D(&map_);\n        //converter_->startWorker(ros::Rate(5), &map, true);\n      }\n   }\n\n\n  void costmapCallback(const nav_msgs::OccupancyGridConstPtr& msg)\n  {\n      ROS_INFO_ONCE(\"Got first costmap callback. This message will be printed once\");\n\n      if (msg->info.width != map_.getSizeInCellsX() || msg->info.height != map_.getSizeInCellsY() || msg->info.resolution != map_.getResolution())\n      {\n        ROS_INFO(\"New map format, resizing and resetting map...\");\n        map_.resizeMap(msg->info.width, msg->info.height, msg->info.resolution, msg->info.origin.position.x, msg->info.origin.position.y);\n      }\n      else\n      {\n        map_.updateOrigin(msg->info.origin.position.x, msg->info.origin.position.y);\n      }\n\n\n      for (std::size_t i=0; i < msg->data.size(); ++i)\n      {\n        unsigned int mx, my;\n        map_.indexToCells((unsigned int)i, mx, my);\n        map_.setCost(mx, my, msg->data[i] >= occupied_min_value_ ? 255 : 0 );\n      }\n\n      // convert\n      converter_->updateCostmap2D();\n      converter_->compute();\n      costmap_converter::ObstacleArrayConstPtr obstacles = converter_->getObstacles();\n\n      if (!obstacles)\n        return;\n\n      obstacle_pub_.publish(obstacles);\n\n      publishAsMarker(msg->header.frame_id, *obstacles, marker_pub_);\n  }\n\n  void costmapUpdateCallback(const map_msgs::OccupancyGridUpdateConstPtr& update)\n  {\n    unsigned int di = 0;\n    for (unsigned int y = 0; y < update->height ; ++y)\n    {\n      for (unsigned int x = 0; x < update->width ; ++x)\n      {\n        map_.setCost(x, y, update->data[di++] >= occupied_min_value_ ? 255 : 0 );\n      }\n    }\n\n    // convert\n    // TODO(roesmann): currently, the converter updates the complete costmap and not the part which is updated in this callback\n    converter_->updateCostmap2D();\n    converter_->compute();\n    costmap_converter::ObstacleArrayConstPtr obstacles = converter_->getObstacles();\n\n    if (!obstacles)\n      return;\n\n    obstacle_pub_.publish(obstacles);\n\n    publishAsMarker(update->header.frame_id, *obstacles, marker_pub_);\n  }\n\n  void publishAsMarker(const std::string& frame_id, const std::vector<geometry_msgs::PolygonStamped>& polygonStamped, ros::Publisher& marker_pub)\n  {\n    visualization_msgs::Marker line_list;\n    line_list.header.frame_id = frame_id;\n    line_list.header.stamp = ros::Time::now();\n    line_list.ns = \"Polygons\";\n    line_list.action = visualization_msgs::Marker::ADD;\n    line_list.pose.orientation.w = 1.0;\n\n    line_list.id = 0;\n    line_list.type = visualization_msgs::Marker::LINE_LIST;\n\n    line_list.scale.x = 0.1;\n    line_list.color.g = 1.0;\n    line_list.color.a = 1.0;\n\n    for (std::size_t i=0; i<polygonStamped.size(); ++i)\n    {\n      for (int j=0; j< (int)polygonStamped[i].polygon.points.size()-1; ++j)\n      {\n        geometry_msgs::Point line_start;\n        line_start.x = polygonStamped[i].polygon.points[j].x;\n        line_start.y = polygonStamped[i].polygon.points[j].y;\n        line_list.points.push_back(line_start);\n        geometry_msgs::Point line_end;\n        line_end.x = polygonStamped[i].polygon.points[j+1].x;\n        line_end.y = polygonStamped[i].polygon.points[j+1].y;\n        line_list.points.push_back(line_end);\n      }\n      // close loop for current polygon\n      if (!polygonStamped[i].polygon.points.empty() && polygonStamped[i].polygon.points.size() != 2 )\n      {\n        geometry_msgs::Point line_start;\n        line_start.x = polygonStamped[i].polygon.points.back().x;\n        line_start.y = polygonStamped[i].polygon.points.back().y;\n        line_list.points.push_back(line_start);\n        if (line_list.points.size() % 2 != 0)\n        {\n          geometry_msgs::Point line_end;\n          line_end.x = polygonStamped[i].polygon.points.front().x;\n          line_end.y = polygonStamped[i].polygon.points.front().y;\n          line_list.points.push_back(line_end);\n        }\n      }\n\n\n    }\n    marker_pub.publish(line_list);\n  }\n\n  void publishAsMarker(const std::string& frame_id, const costmap_converter::ObstacleArrayMsg& obstacles, ros::Publisher& marker_pub)\n  {\n    visualization_msgs::Marker line_list;\n    line_list.header.frame_id = frame_id;\n    line_list.header.stamp = ros::Time::now();\n    line_list.ns = \"Polygons\";\n    line_list.action = visualization_msgs::Marker::ADD;\n    line_list.pose.orientation.w = 1.0;\n\n    line_list.id = 0;\n    line_list.type = visualization_msgs::Marker::LINE_LIST;\n\n    line_list.scale.x = 0.1;\n    line_list.color.g = 1.0;\n    line_list.color.a = 1.0;\n\n    for (const costmap_converter::ObstacleMsg& obstacle : obstacles.obstacles)\n    {\n      for (int j=0; j< (int)obstacle.polygon.points.size()-1; ++j)\n      {\n        geometry_msgs::Point line_start;\n        line_start.x = obstacle.polygon.points[j].x;\n        line_start.y = obstacle.polygon.points[j].y;\n        line_list.points.push_back(line_start);\n        geometry_msgs::Point line_end;\n        line_end.x = obstacle.polygon.points[j+1].x;\n        line_end.y = obstacle.polygon.points[j+1].y;\n        line_list.points.push_back(line_end);\n      }\n      // close loop for current polygon\n      if (!obstacle.polygon.points.empty() && obstacle.polygon.points.size() != 2 )\n      {\n        geometry_msgs::Point line_start;\n        line_start.x = obstacle.polygon.points.back().x;\n        line_start.y = obstacle.polygon.points.back().y;\n        line_list.points.push_back(line_start);\n        if (line_list.points.size() % 2 != 0)\n        {\n          geometry_msgs::Point line_end;\n          line_end.x = obstacle.polygon.points.front().x;\n          line_end.y = obstacle.polygon.points.front().y;\n          line_list.points.push_back(line_end);\n        }\n      }\n\n\n    }\n    marker_pub.publish(line_list);\n  }\n\nprivate:\n  pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> converter_loader_;\n  boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> converter_;\n\n  ros::NodeHandle n_;\n  ros::Subscriber costmap_sub_;\n  ros::Subscriber costmap_update_sub_;\n  ros::Publisher obstacle_pub_;\n  ros::Publisher marker_pub_;\n\n  int occupied_min_value_;\n\n  costmap_2d::Costmap2D map_;\n\n};\n\n\nint main(int argc, char** argv)\n{\n  ros::init(argc, argv, \"costmap_converter\");\n\n  CostmapStandaloneConversion convert_process;\n\n  ros::spin();\n\n  return 0;\n}\n\n\n"
  },
  {
    "path": "src/costmap_to_dynamic_obstacles/background_subtractor.cpp",
    "content": "#include <opencv2/highgui/highgui.hpp>\n//#include <opencv2/cvv/cvv.hpp>\n#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>\n\nBackgroundSubtractor::BackgroundSubtractor(const Params &parameters): params_(parameters)\n{\n}\n\nvoid BackgroundSubtractor::apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x, int shift_y)\n{\n  current_frame_ = image;\n\n  // occupancy grids are empty only once in the beginning -> initialize variables\n  if (occupancy_grid_fast_.empty() && occupancy_grid_slow_.empty())\n  {\n    occupancy_grid_fast_ = current_frame_;\n    occupancy_grid_slow_ = current_frame_;\n    previous_shift_x_ = shift_x;\n    previous_shift_y_ = shift_y;\n    return;\n  }\n\n  // Shift previous occupancy grid to new location (match current_frame_)\n  int shift_relative_to_previous_pos_x_ = shift_x - previous_shift_x_;\n  int shift_relative_to_previous_pos_y_ = shift_y - previous_shift_y_;\n  previous_shift_x_ = shift_x;\n  previous_shift_y_ = shift_y;\n\n  // if(shift_relative_to_previous_pos_x_ != 0 || shift_relative_to_previous_pos_y_ != 0)\n  transformToCurrentFrame(shift_relative_to_previous_pos_x_, shift_relative_to_previous_pos_y_);\n\n  // cvv::debugFilter(occupancy_grid_fast_, currentFrame_, CVVISUAL_LOCATION);\n\n  // Calculate normalized sum (mean) of nearest neighbors\n  int size = 3; // 3, 5, 7, ....\n  cv::Mat nearest_neighbor_mean_fast(occupancy_grid_fast_.size(), CV_8UC1);\n  cv::Mat nearest_neighbor_mean_slow(occupancy_grid_slow_.size(), CV_8UC1);\n  cv::boxFilter(occupancy_grid_fast_, nearest_neighbor_mean_fast, -1, cv::Size(size, size), cv::Point(-1, -1), true,\n                cv::BORDER_REPLICATE);\n  cv::boxFilter(occupancy_grid_slow_, nearest_neighbor_mean_slow, -1, cv::Size(size, size), cv::Point(-1, -1), true,\n                cv::BORDER_REPLICATE);\n  //  cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);\n  //  cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);\n\n  // compute time mean value for each pixel according to learningrate alpha\n  // occupancy_grid_fast_ = beta*(alpha_fast*current_frame_ + (1.0-alpha_fast)*occupancy_grid_fast_) + (1-beta)*nearest_neighbor_mean_fast;\n  cv::addWeighted(current_frame_, params_.alpha_fast, occupancy_grid_fast_, (1 - params_.alpha_fast), 0, occupancy_grid_fast_);\n  cv::addWeighted(occupancy_grid_fast_, params_.beta, nearest_neighbor_mean_fast, (1 - params_.beta), 0, occupancy_grid_fast_);\n  // occupancy_grid_slow_ = beta*(alpha_slow*current_frame_ + (1.0-alpha_slow)*occupancy_grid_slow) + (1-beta)*nearest_neighbor_mean_slow;\n  cv::addWeighted(current_frame_, params_.alpha_slow, occupancy_grid_slow_, (1 - params_.alpha_slow), 0, occupancy_grid_slow_);\n  cv::addWeighted(occupancy_grid_slow_, params_.beta, nearest_neighbor_mean_slow, (1 - params_.beta), 0, occupancy_grid_slow_);\n\n  // 1) Obstacles should be detected after a minimum response of the fast filter\n  //    occupancy_grid_fast_ > min_occupancy_probability\n  cv::threshold(occupancy_grid_fast_, occupancy_grid_fast_, params_.min_occupancy_probability, 0 /*unused*/, cv::THRESH_TOZERO);\n  // 2) Moving obstacles have a minimum difference between the responses of the slow and fast filter\n  //    occupancy_grid_fast_-occupancy_grid_slow_ > min_sep_between_fast_and_slow_filter\n  cv::threshold(occupancy_grid_fast_ - occupancy_grid_slow_, fg_mask, params_.min_sep_between_fast_and_slow_filter, 255,\n                cv::THRESH_BINARY);\n  // 3) Dismiss static obstacles\n  //    nearest_neighbor_mean_slow < max_occupancy_neighbors\n  cv::threshold(nearest_neighbor_mean_slow, nearest_neighbor_mean_slow, params_.max_occupancy_neighbors, 255, cv::THRESH_BINARY_INV);\n  cv::bitwise_and(nearest_neighbor_mean_slow, fg_mask, fg_mask);\n\n  //visualize(\"Current frame\", currentFrame_);\n  cv::Mat setBorderToZero = cv::Mat(current_frame_.size(), CV_8UC1, 0.0);\n  int border = 5;\n  setBorderToZero(cv::Rect(border, border, current_frame_.cols-2*border, current_frame_.rows-2*border)) = 255;\n\n  cv::bitwise_and(setBorderToZero, fg_mask, fg_mask);\n\n  // cv::imwrite(\"/home/albers/Desktop/currentFrame.png\", currentFrame_);\n  // visualize(\"Foreground mask\", fgMask);\n\n  // Closing Operation\n  cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE,\n                                              cv::Size(2*params_.morph_size + 1, 2*params_.morph_size + 1),\n                                              cv::Point(params_.morph_size, params_.morph_size));\n  cv::dilate(fg_mask, fg_mask, element);\n  cv::dilate(fg_mask, fg_mask, element);\n  cv::erode(fg_mask, fg_mask, element);\n}\n\nvoid BackgroundSubtractor::transformToCurrentFrame(int shift_x, int shift_y)\n{\n  // TODO: initialize with current_frame (first observed image) rather than zeros\n\n  // Translate (shift) image in costmap coordinates\n  // in cv::Mat: shift X -> to the left; shift y -> to the top\n  cv::Mat temp_fast, temp_slow;\n  cv::Mat translation_mat = (cv::Mat_<double>(2, 3, CV_64F) << 1, 0, -shift_x, 0, 1, -shift_y);\n  cv::warpAffine(occupancy_grid_fast_, temp_fast, translation_mat, occupancy_grid_fast_.size()); // can't operate in-place\n  cv::warpAffine(occupancy_grid_slow_, temp_slow, translation_mat, occupancy_grid_slow_.size()); // can't operate in-place\n\n  // cvv::debugFilter(occupancy_grid_fast_, temp_fast);\n\n  occupancy_grid_fast_ = temp_fast;\n  occupancy_grid_slow_ = temp_slow;\n}\n\nvoid BackgroundSubtractor::visualize(const std::string& name, const cv::Mat& image)\n{\n  if (!image.empty())\n  {\n    cv::Mat im = image.clone();\n    cv::flip(im, im, 0);\n    cv::imshow(name, im);\n    cv::waitKey(1);\n  }\n}\n\nvoid BackgroundSubtractor::updateParameters(const Params &parameters)\n{\n  params_ = parameters;\n}\n"
  },
  {
    "path": "src/costmap_to_dynamic_obstacles/blob_detector.cpp",
    "content": "#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>\n#include <opencv2/opencv.hpp>\n#include <iostream>\n\nBlobDetector::BlobDetector(const SimpleBlobDetector::Params& parameters) : params_(parameters) {}\n\ncv::Ptr<BlobDetector> BlobDetector::create(const cv::SimpleBlobDetector::Params& params)\n{\n  return cv::Ptr<BlobDetector> (new BlobDetector(params)); // compatibility with older versions\n  //return cv::makePtr<BlobDetector>(params);\n}\n\nvoid BlobDetector::detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat&)\n{\n  // TODO: support mask\n  contours_.clear();\n\n  keypoints.clear();\n  cv::Mat grayscale_image;\n  if (image.channels() == 3)\n    cv::cvtColor(image, grayscale_image, cv::COLOR_BGR2GRAY);\n  else\n    grayscale_image = image;\n\n  if (grayscale_image.type() != CV_8UC1)\n  {\n    //CV_Error(cv::Error::StsUnsupportedFormat, \"Blob detector only supports 8-bit images!\");\n    std::cerr << \"Blob detector only supports 8-bit images!\\n\";\n  }\n\n  std::vector<std::vector<Center>> centers;\n  std::vector<std::vector<cv::Point>> contours;\n  for (double thresh = params_.minThreshold; thresh < params_.maxThreshold; thresh += params_.thresholdStep)\n  {\n    cv::Mat binarized_image;\n    cv::threshold(grayscale_image, binarized_image, thresh, 255, cv::THRESH_BINARY);\n\n    std::vector<Center> cur_centers;\n    std::vector<std::vector<cv::Point>> cur_contours, new_contours;\n    findBlobs(grayscale_image, binarized_image, cur_centers, cur_contours);\n    std::vector<std::vector<Center>> new_centers;\n    for (std::size_t i = 0; i < cur_centers.size(); ++i)\n    {\n      bool isNew = true;\n      for (std::size_t j = 0; j < centers.size(); ++j)\n      {\n        double dist = cv::norm(centers[j][centers[j].size() / 2].location - cur_centers[i].location);\n        isNew = dist >= params_.minDistBetweenBlobs && dist >= centers[j][centers[j].size() / 2].radius &&\n                dist >= cur_centers[i].radius;\n        if (!isNew)\n        {\n          centers[j].push_back(cur_centers[i]);\n\n          size_t k = centers[j].size() - 1;\n          while (k > 0 && centers[j][k].radius < centers[j][k - 1].radius)\n          {\n            centers[j][k] = centers[j][k - 1];\n            k--;\n          }\n          centers[j][k] = cur_centers[i];\n\n          break;\n        }\n      }\n      if (isNew)\n      {\n        new_centers.push_back(std::vector<Center>(1, cur_centers[i]));\n        new_contours.push_back(cur_contours[i]);\n      }\n    }\n    std::copy(new_centers.begin(), new_centers.end(), std::back_inserter(centers));\n    std::copy(new_contours.begin(), new_contours.end(), std::back_inserter(contours));\n  }\n\n  for (size_t i = 0; i < centers.size(); ++i)\n  {\n    if (centers[i].size() < params_.minRepeatability)\n      continue;\n    cv::Point2d sum_point(0, 0);\n    double normalizer = 0;\n    for (std::size_t j = 0; j < centers[i].size(); ++j)\n    {\n      sum_point += centers[i][j].confidence * centers[i][j].location;\n      normalizer += centers[i][j].confidence;\n    }\n    sum_point *= (1. / normalizer);\n    cv::KeyPoint kpt(sum_point, (float)(centers[i][centers[i].size() / 2].radius));\n    keypoints.push_back(kpt);\n    contours_.push_back(contours[i]);\n  }\n}\n\nvoid BlobDetector::findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,\n                             std::vector<std::vector<cv::Point>>& cur_contours) const\n{\n  (void)image;\n  centers.clear();\n  cur_contours.clear();\n\n  std::vector<std::vector<cv::Point>> contours;\n  cv::Mat tmp_binary_image = binary_image.clone();\n  cv::findContours(tmp_binary_image, contours, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);\n\n  for (std::size_t contour_idx = 0; contour_idx < contours.size(); ++contour_idx)\n  {\n    Center center;\n    center.confidence = 1;\n    cv::Moments moms = cv::moments(cv::Mat(contours[contour_idx]));\n    if (params_.filterByArea)\n    {\n      double area = moms.m00;\n      if (area < params_.minArea || area >= params_.maxArea)\n        continue;\n    }\n\n    if (params_.filterByCircularity)\n    {\n      double area = moms.m00;\n      double perimeter = cv::arcLength(cv::Mat(contours[contour_idx]), true);\n      double ratio = 4 * CV_PI * area / (perimeter * perimeter);\n      if (ratio < params_.minCircularity || ratio >= params_.maxCircularity)\n        continue;\n    }\n\n    if (params_.filterByInertia)\n    {\n      double denominator = std::sqrt(std::pow(2 * moms.mu11, 2) + std::pow(moms.mu20 - moms.mu02, 2));\n      const double eps = 1e-2;\n      double ratio;\n      if (denominator > eps)\n      {\n        double cosmin = (moms.mu20 - moms.mu02) / denominator;\n        double sinmin = 2 * moms.mu11 / denominator;\n        double cosmax = -cosmin;\n        double sinmax = -sinmin;\n\n        double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin;\n        double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax;\n        ratio = imin / imax;\n      }\n      else\n      {\n        ratio = 1;\n      }\n\n      if (ratio < params_.minInertiaRatio || ratio >= params_.maxInertiaRatio)\n        continue;\n\n      center.confidence = ratio * ratio;\n    }\n\n    if (params_.filterByConvexity)\n    {\n      std::vector<cv::Point> hull;\n      cv::convexHull(cv::Mat(contours[contour_idx]), hull);\n      double area = cv::contourArea(cv::Mat(contours[contour_idx]));\n      double hullArea = cv::contourArea(cv::Mat(hull));\n      double ratio = area / hullArea;\n      if (ratio < params_.minConvexity || ratio >= params_.maxConvexity)\n        continue;\n    }\n\n    if (moms.m00 == 0.0)\n      continue;\n    center.location = cv::Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00);\n\n    if (params_.filterByColor)\n    {\n      if (binary_image.at<uchar>(cvRound(center.location.y), cvRound(center.location.x)) != params_.blobColor)\n        continue;\n    }\n\n    // compute blob radius\n    {\n      std::vector<double> dists;\n      for (std::size_t point_idx = 0; point_idx < contours[contour_idx].size(); ++point_idx)\n      {\n        cv::Point2d pt = contours[contour_idx][point_idx];\n        dists.push_back(cv::norm(center.location - pt));\n      }\n      std::sort(dists.begin(), dists.end());\n      center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.;\n    }\n\n    centers.push_back(center);\n    cur_contours.push_back(contours[contour_idx]);\n  }\n}\n\nvoid BlobDetector::updateParameters(const Params& parameters)\n{\n  params_ = parameters;\n}\n"
  },
  {
    "path": "src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp",
    "content": "#include <costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h>\n\n#include <pluginlib/class_list_macros.h>\n#include <tf/tf.h>\n\nPLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToDynamicObstacles, costmap_converter::BaseCostmapToPolygons)\n\nnamespace costmap_converter\n{\n\nCostmapToDynamicObstacles::CostmapToDynamicObstacles() : BaseCostmapToDynamicObstacles()\n{\n  ego_vel_.x = ego_vel_.y = ego_vel_.z = 0;\n  costmap_ = nullptr;\n  dynamic_recfg_ = nullptr;\n}\n\nCostmapToDynamicObstacles::~CostmapToDynamicObstacles()\n{\n  if(dynamic_recfg_ != nullptr)\n    delete dynamic_recfg_;\n}\n\nvoid CostmapToDynamicObstacles::initialize(ros::NodeHandle nh)\n{\n  costmap_ = nullptr;\n\n  // We need the odometry from the robot to compensate the ego motion\n  ros::NodeHandle gn; // create odom topic w.r.t. global node handle\n  odom_sub_ = gn.subscribe(odom_topic_, 1, &CostmapToDynamicObstacles::odomCallback, this);\n\n  nh.param(\"publish_static_obstacles\", publish_static_obstacles_, publish_static_obstacles_);\n\n  //////////////////////////////////\n  // Foreground detection parameters\n  BackgroundSubtractor::Params bg_sub_params;\n\n  bg_sub_params.alpha_slow = 0.3;\n  nh.param(\"alpha_slow\", bg_sub_params.alpha_slow, bg_sub_params.alpha_slow);\n\n  bg_sub_params.alpha_fast = 0.85;\n  nh.param(\"alpha_fast\", bg_sub_params.alpha_fast, bg_sub_params.alpha_fast);\n\n  bg_sub_params.beta = 0.85;\n  nh.param(\"beta\", bg_sub_params.beta, bg_sub_params.beta);\n\n  bg_sub_params.min_occupancy_probability = 180;\n  nh.param(\"min_occupancy_probability\", bg_sub_params.min_occupancy_probability, bg_sub_params.min_occupancy_probability);\n\n  bg_sub_params.min_sep_between_fast_and_slow_filter = 80;\n  nh.param(\"min_sep_between_slow_and_fast_filter\", bg_sub_params.min_sep_between_fast_and_slow_filter, bg_sub_params.min_sep_between_fast_and_slow_filter);\n\n  bg_sub_params.max_occupancy_neighbors = 100;\n  nh.param(\"max_occupancy_neighbors\", bg_sub_params.max_occupancy_neighbors, bg_sub_params.max_occupancy_neighbors);\n\n  bg_sub_params.morph_size = 1;\n  nh.param(\"morph_size\", bg_sub_params.morph_size, bg_sub_params.morph_size);\n\n  bg_sub_ = std::unique_ptr<BackgroundSubtractor>(new BackgroundSubtractor(bg_sub_params));\n\n  ////////////////////////////\n  // Blob detection parameters\n  BlobDetector::Params blob_det_params;\n\n  blob_det_params.filterByColor = true; // actually filterByIntensity, always true\n  blob_det_params.blobColor = 255;      // Extract light blobs\n  blob_det_params.thresholdStep = 256;  // Input for blob detection is already a binary image\n  blob_det_params.minThreshold = 127;\n  blob_det_params.maxThreshold = 255;\n  blob_det_params.minRepeatability = 1;\n\n  blob_det_params.minDistBetweenBlobs = 10;\n  nh.param(\"min_distance_between_blobs\", blob_det_params.minDistBetweenBlobs, blob_det_params.minDistBetweenBlobs);\n\n  blob_det_params.filterByArea = true;\n  nh.param(\"filter_by_area\", blob_det_params.filterByArea, blob_det_params.filterByArea);\n\n  blob_det_params.minArea = 3; // Filter out blobs with less pixels\n  nh.param(\"min_area\", blob_det_params.minArea, blob_det_params.minArea);\n\n  blob_det_params.maxArea = 300;\n  nh.param(\"max_area\", blob_det_params.maxArea, blob_det_params.maxArea);\n\n  blob_det_params.filterByCircularity = true; // circularity = 4*pi*area/perimeter^2\n  nh.param(\"filter_by_circularity\", blob_det_params.filterByCircularity, blob_det_params.filterByCircularity);\n\n  blob_det_params.minCircularity = 0.2;\n  nh.param(\"min_circularity\", blob_det_params.minCircularity, blob_det_params.minCircularity);\n\n  blob_det_params.maxCircularity = 1; // maximal 1 (in case of a circle)\n  nh.param(\"max_circularity\", blob_det_params.maxCircularity, blob_det_params.maxCircularity);\n\n  blob_det_params.filterByInertia = true; // Filter blobs based on their elongation\n  nh.param(\"filter_by_intertia\", blob_det_params.filterByInertia, blob_det_params.filterByInertia);\n\n  blob_det_params.minInertiaRatio = 0.2;  // minimal 0 (in case of a line)\n  nh.param(\"min_inertia_ratio\", blob_det_params.minInertiaRatio, blob_det_params.minInertiaRatio);\n\n  blob_det_params.maxInertiaRatio = 1;    // maximal 1 (in case of a circle)\n  nh.param(\"max_intertia_ratio\", blob_det_params.maxInertiaRatio, blob_det_params.maxInertiaRatio);\n\n  blob_det_params.filterByConvexity = false; // Area of the Blob / Area of its convex hull\n  nh.param(\"filter_by_convexity\", blob_det_params.filterByConvexity, blob_det_params.filterByConvexity);\n\n  blob_det_params.minConvexity = 0;          // minimal 0\n  nh.param(\"min_convexity\", blob_det_params.minConvexity, blob_det_params.minConvexity);\n\n  blob_det_params.maxConvexity = 1;          // maximal 1\n  nh.param(\"max_convexity\", blob_det_params.maxConvexity, blob_det_params.maxConvexity);\n\n  blob_det_ = BlobDetector::create(blob_det_params);\n\n  ////////////////////////////////////\n  // Tracking parameters\n  CTracker::Params tracker_params;\n  tracker_params.dt = 0.2;\n  nh.param(\"dt\", tracker_params.dt, tracker_params.dt);\n\n  tracker_params.dist_thresh = 60.0;\n  nh.param(\"dist_thresh\", tracker_params.dist_thresh, tracker_params.dist_thresh);\n\n  tracker_params.max_allowed_skipped_frames = 3;\n  nh.param(\"max_allowed_skipped_frames\", tracker_params.max_allowed_skipped_frames, tracker_params.max_allowed_skipped_frames);\n\n  tracker_params.max_trace_length = 10;\n  nh.param(\"max_trace_length\", tracker_params.max_trace_length, tracker_params.max_trace_length);\n\n  tracker_ = std::unique_ptr<CTracker>(new CTracker(tracker_params));\n\n\n  ////////////////////////////////////\n  // Static costmap conversion parameters\n  std::string static_converter_plugin = \"costmap_converter::CostmapToPolygonsDBSMCCH\";\n  nh.param(\"static_converter_plugin\", static_converter_plugin, static_converter_plugin);\n  loadStaticCostmapConverterPlugin(static_converter_plugin, nh);\n\n\n  // setup dynamic reconfigure\n  dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>(nh);\n  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>::CallbackType cb = boost::bind(&CostmapToDynamicObstacles::reconfigureCB, this, _1, _2);\n  dynamic_recfg_->setCallback(cb);\n}\n\nvoid CostmapToDynamicObstacles::compute()\n{\n  if (costmap_mat_.empty())\n    return;\n\n  /////////////////////////// Foreground detection ////////////////////////////////////\n  // Dynamic obstacles are separated from static obstacles\n  int origin_x = round(costmap_->getOriginX() / costmap_->getResolution());\n  int origin_y = round(costmap_->getOriginY() / costmap_->getResolution());\n  // ROS_INFO(\"Origin x  [m]: %f    Origin_y  [m]: %f\", costmap_->getOriginX(), costmap_->getOriginY());\n  // ROS_INFO(\"Origin x [px]: %d \\t Origin_y [px]: %d\", originX, originY);\n\n  bg_sub_->apply(costmap_mat_, fg_mask_, origin_x, origin_y);\n\n  // if no foreground object is detected, no ObstacleMsgs need to be published\n  if (fg_mask_.empty())\n    return;\n\n  cv::Mat bg_mat;\n  if (publish_static_obstacles_)\n  {\n    // Get static obstacles\n    bg_mat = costmap_mat_ - fg_mask_;\n    // visualize(\"bg_mat\", bg_mat);\n  }\n\n\n  /////////////////////////////// Blob detection /////////////////////////////////////\n  // Centers and contours of Blobs are detected\n  blob_det_->detect(fg_mask_, keypoints_);\n  std::vector<std::vector<cv::Point>> contours = blob_det_->getContours();\n\n\n  ////////////////////////////// Tracking ////////////////////////////////////////////\n  // Objects are assigned to objects from previous frame based on Hungarian Algorithm\n  // Object velocities are estimated using a Kalman Filter\n  std::vector<Point_t> detected_centers(keypoints_.size());\n  for (int i = 0; i < keypoints_.size(); i++)\n  {\n    detected_centers.at(i).x = keypoints_.at(i).pt.x;\n    detected_centers.at(i).y = keypoints_.at(i).pt.y;\n    detected_centers.at(i).z = 0; // Currently unused!\n  }\n\n  tracker_->Update(detected_centers, contours);\n\n\n  ///////////////////////////////////// Output ///////////////////////////////////////\n  /*\n  cv::Mat fg_mask_with_keypoints = cv::Mat::zeros(fg_mask.size(), CV_8UC3);\n  cv::cvtColor(fg_mask, fg_mask_with_keypoints, cv::COLOR_GRAY2BGR);\n\n  for (int i = 0; i < (int)tracker_->tracks.size(); ++i)\n    cv::rectangle(fg_mask_with_keypoints, cv::boundingRect(tracker_->tracks[i]->getLastContour()),\n                  cv::Scalar(0, 0, 255), 1);\n\n  //visualize(\"fgMaskWithKeyPoints\", fgMaskWithKeypoints);\n  //cv::imwrite(\"/home/albers/Desktop/fgMask.png\", fgMask);\n  //cv::imwrite(\"/home/albers/Desktop/fgMaskWithKeyPoints.png\", fgMaskWithKeypoints);\n  */\n\n  //////////////////////////// Fill ObstacleContainerPtr /////////////////////////////\n  ObstacleArrayPtr obstacles(new ObstacleArrayMsg);\n  // header.seq is automatically filled\n  obstacles->header.stamp = ros::Time::now();\n  obstacles->header.frame_id = \"/map\"; //Global frame /map\n\n  // For all tracked objects\n  for (unsigned int i = 0; i < (unsigned int)tracker_->tracks.size(); ++i)\n  {\n    geometry_msgs::Polygon polygon;\n\n    // TODO directly create polygon inside getContour and avoid copy\n    std::vector<Point_t> contour;\n    getContour(i, contour); // this method also transforms map to world coordinates\n\n    // convert contour to polygon\n    for (const Point_t& pt : contour)\n    {\n      polygon.points.emplace_back();\n      polygon.points.back().x = pt.x;\n      polygon.points.back().y = pt.y;\n      polygon.points.back().z = 0;\n    }\n\n    obstacles->obstacles.emplace_back();\n    obstacles->obstacles.back().polygon = polygon;\n\n    // Set obstacle ID\n    obstacles->obstacles.back().id = tracker_->tracks.at(i)->track_id;\n\n    // Set orientation\n    geometry_msgs::QuaternionStamped orientation;\n\n    Point_t vel = getEstimatedVelocityOfObject(i);\n    double yaw = std::atan2(vel.y, vel.x);\n    //ROS_INFO(\"yaw: %f\", yaw);\n    obstacles->obstacles.back().orientation = tf::createQuaternionMsgFromYaw(yaw);\n\n    // Set velocity\n    geometry_msgs::TwistWithCovariance velocities;\n    //velocities.twist.linear.x = std::sqrt(vel.x*vel.x + vel.y*vel.y);\n    //velocities.twist.linear.y = 0;\n    velocities.twist.linear.x = vel.x;\n    velocities.twist.linear.y = vel.y; // TODO(roesmann): don't we need to consider the transformation between opencv's and costmap's coordinate frames?\n    velocities.twist.linear.z = 0;\n    velocities.twist.angular.x = 0;\n    velocities.twist.angular.y = 0;\n    velocities.twist.angular.z = 0;\n\n    // TODO: use correct covariance matrix\n    velocities.covariance = {1, 0, 0, 0, 0, 0,\n                             0, 1, 0, 0, 0, 0,\n                             0, 0, 1, 0, 0, 0,\n                             0, 0, 0, 1, 0, 0,\n                             0, 0, 0, 0, 1, 0,\n                             0, 0, 0, 0, 0, 1};\n\n    obstacles->obstacles.back().velocities = velocities;\n  }\n\n  ////////////////////////// Static obstacles ////////////////////////////\n  if (publish_static_obstacles_)\n  {\n    uchar* img_data = bg_mat.data;\n    int width = bg_mat.cols;\n    int height = bg_mat.rows;\n    int stride = bg_mat.step;\n\n    if (stackedCostmapConversion())\n    {\n      // Create new costmap with static obstacles (background)\n      boost::shared_ptr<costmap_2d::Costmap2D> static_costmap(new costmap_2d::Costmap2D(costmap_->getSizeInCellsX(),\n                                                                                        costmap_->getSizeInCellsY(),\n                                                                                        costmap_->getResolution(),\n                                                                                        costmap_->getOriginX(),\n                                                                                        costmap_->getOriginY()));\n      for(int i = 0; i < height; i++)\n      {\n        for(int j = 0; j < width; j++)\n        {\n          static_costmap->setCost(j, i, img_data[i * stride + j]);\n        }\n      }\n\n      // Apply static obstacle conversion plugin\n      setStaticCostmap(static_costmap);\n      convertStaticObstacles();\n\n      // Store converted static obstacles for publishing\n      auto static_polygons = getStaticPolygons();\n      for (auto it = static_polygons->begin(); it != static_polygons->end(); ++it)\n      {\n        obstacles->obstacles.emplace_back();\n        obstacles->obstacles.back().polygon = *it;\n        obstacles->obstacles.back().velocities.twist.linear.x = 0;\n        obstacles->obstacles.back().velocities.twist.linear.y = 0;\n        obstacles->obstacles.back().id = -1;\n      }\n    }\n    // Otherwise leave static obstacles point-shaped\n    else\n    {\n      for(int i = 0; i < height; i++)\n      {\n        for(int j = 0; j < width; j++)\n        {\n            uchar value = img_data[i * stride + j];\n            if (value > 0)\n            {\n              // obstacle found\n              obstacles->obstacles.emplace_back();\n              geometry_msgs::Point32 pt;\n              pt.x = (double)j*costmap_->getResolution() + costmap_->getOriginX();\n              pt.y = (double)i*costmap_->getResolution() + costmap_->getOriginY();\n              obstacles->obstacles.back().polygon.points.push_back(pt);\n              obstacles->obstacles.back().velocities.twist.linear.x = 0;\n              obstacles->obstacles.back().velocities.twist.linear.y = 0;\n              obstacles->obstacles.back().id = -1;\n            }\n        }\n      }\n    }\n  }\n\n  updateObstacleContainer(obstacles);\n}\n\nvoid CostmapToDynamicObstacles::setCostmap2D(costmap_2d::Costmap2D* costmap)\n{\n  if (!costmap)\n    return;\n\n  costmap_ = costmap;\n\n  updateCostmap2D();\n}\n\nvoid CostmapToDynamicObstacles::updateCostmap2D()\n{\n  if (!costmap_->getMutex())\n  {\n    ROS_ERROR(\"Cannot update costmap since the mutex pointer is null\");\n    return;\n  }\n  costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());\n\n  // Initialize costmapMat_ directly with the unsigned char array of costmap_\n  //costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,\n  //                      costmap_->getCharMap()).clone(); // Deep copy: TODO\n  costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,\n                        costmap_->getCharMap());\n}\n\nObstacleArrayConstPtr CostmapToDynamicObstacles::getObstacles()\n{\n  boost::mutex::scoped_lock lock(mutex_);\n  return obstacles_;\n}\n\nvoid CostmapToDynamicObstacles::updateObstacleContainer(ObstacleArrayPtr obstacles)\n{\n  boost::mutex::scoped_lock lock(mutex_);\n  obstacles_ = obstacles;\n}\n\nPoint_t CostmapToDynamicObstacles::getEstimatedVelocityOfObject(unsigned int idx)\n{\n  // vel [px/s] * costmapResolution [m/px] = vel [m/s]\n  Point_t vel = tracker_->tracks.at(idx)->getEstimatedVelocity() * costmap_->getResolution() + ego_vel_;\n\n  //ROS_INFO(\"vel x: %f, vel y: %f, vel z: %f\", vel.x, vel.y, vel.z);\n  // velocity in /map frame\n  return vel;\n}\n\nvoid CostmapToDynamicObstacles::odomCallback(const nav_msgs::Odometry::ConstPtr& msg)\n{\n  ROS_INFO_ONCE(\"CostmapToDynamicObstacles: odom received.\");\n\n  tf::Quaternion pose;\n  tf::quaternionMsgToTF(msg->pose.pose.orientation, pose);\n\n  tf::Vector3 twistLinear;\n  tf::vector3MsgToTF(msg->twist.twist.linear, twistLinear);\n\n  // velocity of the robot in x, y and z coordinates\n  tf::Vector3 vel = tf::quatRotate(pose, twistLinear);\n  ego_vel_.x = vel.x();\n  ego_vel_.y = vel.y();\n  ego_vel_.z = vel.z();\n}\n\nvoid CostmapToDynamicObstacles::reconfigureCB(CostmapToDynamicObstaclesConfig& config, uint32_t level)\n{\n  publish_static_obstacles_ = config.publish_static_obstacles;\n\n  // BackgroundSubtraction Parameters\n  BackgroundSubtractor::Params bg_sub_params;\n  bg_sub_params.alpha_slow = config.alpha_slow;\n  bg_sub_params.alpha_fast = config.alpha_fast;\n  bg_sub_params.beta = config.beta;\n  bg_sub_params.min_sep_between_fast_and_slow_filter = config.min_sep_between_slow_and_fast_filter;\n  bg_sub_params.min_occupancy_probability = config.min_occupancy_probability;\n  bg_sub_params.max_occupancy_neighbors = config.max_occupancy_neighbors;\n  bg_sub_params.morph_size = config.morph_size;\n  bg_sub_->updateParameters(bg_sub_params);\n\n  // BlobDetector Parameters\n  BlobDetector::Params blob_det_params;\n  // necessary, because blobDetParams are otherwise initialized with default values for dark blobs\n  blob_det_params.filterByColor = true; // actually filterByIntensity, always true\n  blob_det_params.blobColor = 255;      // Extract light blobs\n  blob_det_params.thresholdStep = 256;  // Input for blobDetection is already a binary image\n  blob_det_params.minThreshold = 127;\n  blob_det_params.maxThreshold = 255;\n  blob_det_params.minRepeatability = 1;\n  blob_det_params.minDistBetweenBlobs = config.min_distance_between_blobs; // TODO: Currently not working as expected\n  blob_det_params.filterByArea = config.filter_by_area;\n  blob_det_params.minArea = config.min_area;\n  blob_det_params.maxArea = config.max_area;\n  blob_det_params.filterByCircularity = config.filter_by_circularity;\n  blob_det_params.minCircularity = config.min_circularity;\n  blob_det_params.maxCircularity = config.max_circularity;\n  blob_det_params.filterByInertia = config.filter_by_inertia;\n  blob_det_params.minInertiaRatio = config.min_inertia_ratio;\n  blob_det_params.maxInertiaRatio = config.max_inertia_ratio;\n  blob_det_params.filterByConvexity = config.filter_by_convexity;\n  blob_det_params.minConvexity = config.min_convexity;\n  blob_det_params.maxConvexity = config.max_convexity;\n  blob_det_->updateParameters(blob_det_params);\n\n  // Tracking Parameters\n  CTracker::Params tracking_params;\n  tracking_params.dt = config.dt;\n  tracking_params.dist_thresh = config.dist_thresh;\n  tracking_params.max_allowed_skipped_frames = config.max_allowed_skipped_frames;\n  tracking_params.max_trace_length = config.max_trace_length;\n  tracker_->updateParameters(tracking_params);\n}\n\nvoid CostmapToDynamicObstacles::getContour(unsigned int idx, std::vector<Point_t>& contour)\n{\n  assert(!tracker_->tracks.empty() && idx < tracker_->tracks.size());\n\n  contour.clear();\n\n  // contour [px] * costmapResolution [m/px] = contour [m]\n  std::vector<cv::Point> contour2i = tracker_->tracks.at(idx)->getLastContour();\n\n  contour.reserve(contour2i.size());\n\n  Point_t costmap_origin(costmap_->getOriginX(), costmap_->getOriginY(), 0);\n\n  for (std::size_t i = 0; i < contour2i.size(); ++i)\n  {\n    contour.push_back((Point_t(contour2i.at(i).x, contour2i.at(i).y, 0.0)*costmap_->getResolution())\n                        + costmap_origin); // Shift to /map\n  }\n\n}\n\nvoid CostmapToDynamicObstacles::visualize(const std::string& name, const cv::Mat& image)\n{\n  if (!image.empty())\n  {\n    cv::Mat im = image.clone();\n    cv::flip(im, im, 0);\n    cv::imshow(name, im);\n    cv::waitKey(1);\n  }\n}\n}\n"
  },
  {
    "path": "src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp",
    "content": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this directory.\n\n#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>\n\n// ---------------------------------------------------------------------------\n// Tracker. Manage tracks. Create, remove, update.\n// ---------------------------------------------------------------------------\nCTracker::CTracker(const Params &parameters)\n    : params(parameters),\n      NextTrackID(0)\n{\n}\n// ---------------------------------------------------------------------------\n//\n// ---------------------------------------------------------------------------\nvoid CTracker::Update(const std::vector<Point_t>& detectedCentroid, const std::vector< std::vector<cv::Point> >& contours)\n{\n  // Each contour has a centroid\n  assert(detectedCentroid.size() == contours.size());\n\n  // -----------------------------------\n  // If there is no tracks yet, then every cv::Point begins its own track.\n  // -----------------------------------\n  if (tracks.size() == 0)\n  {\n    // If no tracks yet\n    for (size_t i = 0; i < detectedCentroid.size(); ++i)\n    {\n      tracks.push_back(\n          std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));\n    }\n  }\n\n  size_t N = tracks.size();\n  size_t M = detectedCentroid.size();\n\n  assignments_t assignment;\n\n  if (!tracks.empty())\n  {\n    // Distance matrix of N-th Track to the M-th detectedCentroid\n    distMatrix_t Cost(N * M);\n\n    // calculate distance between the blobs centroids\n    for (size_t i = 0; i < tracks.size(); i++)\n    {\n      for (size_t j = 0; j < detectedCentroid.size(); j++)\n      {\n        Cost[i + j * N] = tracks[i]->CalcDist(detectedCentroid[j]);\n      }\n    }\n\n    // -----------------------------------\n    // Solving assignment problem (tracks and predictions of Kalman filter)\n    // -----------------------------------\n    AssignmentProblemSolver APS;\n    APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal);\n\n    // -----------------------------------\n    // clean assignment from pairs with large distance\n    // -----------------------------------\n    for (size_t i = 0; i < assignment.size(); i++)\n    {\n      if (assignment[i] != -1)\n      {\n        if (Cost[i + assignment[i] * N] > params.dist_thresh)\n        {\n          assignment[i] = -1;\n          tracks[i]->skipped_frames = 1;\n        }\n      }\n      else\n      {\n        // If track have no assigned detect, then increment skipped frames counter.\n        tracks[i]->skipped_frames++;\n      }\n    }\n\n    // -----------------------------------\n    // If track didn't get detects long time, remove it.\n    // -----------------------------------\n    for (int i = 0; i < static_cast<int>(tracks.size()); i++)\n    {\n      if (tracks[i]->skipped_frames > params.max_allowed_skipped_frames)\n      {\n        tracks.erase(tracks.begin() + i);\n        assignment.erase(assignment.begin() + i);\n        i--;\n      }\n    }\n  }\n\n  // -----------------------------------\n  // Search for unassigned detects and start new tracks for them.\n  // -----------------------------------\n  for (size_t i = 0; i < detectedCentroid.size(); ++i)\n  {\n    if (find(assignment.begin(), assignment.end(), i) == assignment.end())\n    {\n      tracks.push_back(std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));\n    }\n  }\n\n  // Update Kalman Filters state\n\n  for (size_t i = 0; i < assignment.size(); i++)\n  {\n    // If track updated less than one time, than filter state is not correct.\n\n    if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,\n    {\n      tracks[i]->skipped_frames = 0;\n      tracks[i]->Update(detectedCentroid[assignment[i]], contours[assignment[i]], true, params.max_trace_length);\n    }\n    else // if not continue using predictions\n    {\n      tracks[i]->Update(Point_t(), std::vector<cv::Point>(), false, params.max_trace_length);\n    }\n  }\n}\n\nvoid CTracker::updateParameters(const Params &parameters)\n{\n  params = parameters;\n}\n// ---------------------------------------------------------------------------\n//\n// ---------------------------------------------------------------------------\nCTracker::~CTracker(void) {}\n"
  },
  {
    "path": "src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp",
    "content": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this directory.\n\n#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h>\n#include <limits>\n\nAssignmentProblemSolver::AssignmentProblemSolver() {}\n\nAssignmentProblemSolver::~AssignmentProblemSolver() {}\n\ntrack_t AssignmentProblemSolver::Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns,\n                                       std::vector<int>& assignment, TMethod Method)\n{\n  assignment.resize(nOfRows, -1);\n\n  track_t cost = 0;\n\n  switch (Method)\n  {\n  case optimal:\n    assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns);\n    break;\n\n  case many_forbidden_assignments:\n    assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns);\n    break;\n\n  case without_forbidden_assignments:\n    assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns);\n    break;\n  }\n\n  return cost;\n}\n// --------------------------------------------------------------------------\n// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost,\n                                                const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)\n{\n  // Generate distance cv::Matrix\n  // and check cv::Matrix elements positiveness :)\n\n  // Total elements number\n  size_t nOfElements = nOfRows * nOfColumns;\n  // Memory allocation\n  track_t* distMatrix = (track_t*)malloc(nOfElements * sizeof(track_t));\n  // Pointer to last element\n  track_t* distMatrixEnd = distMatrix + nOfElements;\n\n  for (size_t row = 0; row < nOfElements; row++)\n  {\n    track_t value = distMatrixIn[row];\n    assert(value >= 0);\n    distMatrix[row] = value;\n  }\n\n  // Memory allocation\n  bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool));\n  bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool));\n  bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool));\n  bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool));\n  bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */\n\n  /* preliminary steps */\n  if (nOfRows <= nOfColumns)\n  {\n    for (size_t row = 0; row < nOfRows; row++)\n    {\n      /* find the smallest element in the row */\n      track_t* distMatrixTemp = distMatrix + row;\n      track_t minValue = *distMatrixTemp;\n      distMatrixTemp += nOfRows;\n      while (distMatrixTemp < distMatrixEnd)\n      {\n        track_t value = *distMatrixTemp;\n        if (value < minValue)\n        {\n          minValue = value;\n        }\n        distMatrixTemp += nOfRows;\n      }\n      /* subtract the smallest element from each element of the row */\n      distMatrixTemp = distMatrix + row;\n      while (distMatrixTemp < distMatrixEnd)\n      {\n        *distMatrixTemp -= minValue;\n        distMatrixTemp += nOfRows;\n      }\n    }\n    /* Steps 1 and 2a */\n    for (size_t row = 0; row < nOfRows; row++)\n    {\n      for (size_t col = 0; col < nOfColumns; col++)\n      {\n        if (distMatrix[row + nOfRows * col] == 0)\n        {\n          if (!coveredColumns[col])\n          {\n            starMatrix[row + nOfRows * col] = true;\n            coveredColumns[col] = true;\n            break;\n          }\n        }\n      }\n    }\n  }\n  else /* if(nOfRows > nOfColumns) */\n  {\n    for (size_t col = 0; col < nOfColumns; col++)\n    {\n      /* find the smallest element in the column */\n      track_t* distMatrixTemp = distMatrix + nOfRows * col;\n      track_t* columnEnd = distMatrixTemp + nOfRows;\n      track_t minValue = *distMatrixTemp++;\n      while (distMatrixTemp < columnEnd)\n      {\n        track_t value = *distMatrixTemp++;\n        if (value < minValue)\n        {\n          minValue = value;\n        }\n      }\n      /* subtract the smallest element from each element of the column */\n      distMatrixTemp = distMatrix + nOfRows * col;\n      while (distMatrixTemp < columnEnd)\n      {\n        *distMatrixTemp++ -= minValue;\n      }\n    }\n    /* Steps 1 and 2a */\n    for (size_t col = 0; col < nOfColumns; col++)\n    {\n      for (size_t row = 0; row < nOfRows; row++)\n      {\n        if (distMatrix[row + nOfRows * col] == 0)\n        {\n          if (!coveredRows[row])\n          {\n            starMatrix[row + nOfRows * col] = true;\n            coveredColumns[col] = true;\n            coveredRows[row] = true;\n            break;\n          }\n        }\n      }\n    }\n\n    for (size_t row = 0; row < nOfRows; row++)\n    {\n      coveredRows[row] = false;\n    }\n  }\n  /* move to step 2b */\n  step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,\n         nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns);\n  /* compute cost and remove invalid assignments */\n  computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);\n  /* free allocated memory */\n  free(distMatrix);\n  free(coveredColumns);\n  free(coveredRows);\n  free(starMatrix);\n  free(primeMatrix);\n  free(newStarMatrix);\n  return;\n}\n// --------------------------------------------------------------------------\n//\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows,\n                                                    size_t nOfColumns)\n{\n  for (size_t row = 0; row < nOfRows; row++)\n  {\n    for (size_t col = 0; col < nOfColumns; col++)\n    {\n      if (starMatrix[row + nOfRows * col])\n      {\n        assignment[row] = static_cast<int>(col);\n        break;\n      }\n    }\n  }\n}\n// --------------------------------------------------------------------------\n//\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost,\n                                                    const distMatrix_t& distMatrixIn, size_t nOfRows)\n{\n  for (size_t row = 0; row < nOfRows; row++)\n  {\n    const int col = assignment[row];\n    if (col >= 0)\n    {\n      cost += distMatrixIn[row + nOfRows * col];\n    }\n  }\n}\n\n// --------------------------------------------------------------------------\n//\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,\n                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,\n                                     size_t nOfRows, size_t nOfColumns, size_t minDim)\n{\n  bool* starMatrixTemp, *columnEnd;\n  /* cover every column containing a starred zero */\n  for (size_t col = 0; col < nOfColumns; col++)\n  {\n    starMatrixTemp = starMatrix + nOfRows * col;\n    columnEnd = starMatrixTemp + nOfRows;\n    while (starMatrixTemp < columnEnd)\n    {\n      if (*starMatrixTemp++)\n      {\n        coveredColumns[col] = true;\n        break;\n      }\n    }\n  }\n  /* move to step 3 */\n  step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,\n         nOfColumns, minDim);\n}\n\n// --------------------------------------------------------------------------\n//\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,\n                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,\n                                     size_t nOfRows, size_t nOfColumns, size_t minDim)\n{\n  /* count covered columns */\n  size_t nOfCoveredColumns = 0;\n  for (size_t col = 0; col < nOfColumns; col++)\n  {\n    if (coveredColumns[col])\n    {\n      nOfCoveredColumns++;\n    }\n  }\n  if (nOfCoveredColumns == minDim)\n  {\n    /* algorithm finished */\n    buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);\n  }\n  else\n  {\n    /* move to step 3 */\n    step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,\n          nOfColumns, minDim);\n  }\n}\n\n// --------------------------------------------------------------------------\n//\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,\n                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,\n                                    size_t nOfRows, size_t nOfColumns, size_t minDim)\n{\n  bool zerosFound = true;\n  while (zerosFound)\n  {\n    zerosFound = false;\n    for (size_t col = 0; col < nOfColumns; col++)\n    {\n      if (!coveredColumns[col])\n      {\n        for (size_t row = 0; row < nOfRows; row++)\n        {\n          if ((!coveredRows[row]) && (distMatrix[row + nOfRows * col] == 0))\n          {\n            /* prime zero */\n            primeMatrix[row + nOfRows * col] = true;\n            /* find starred zero in current row */\n            size_t starCol = 0;\n            for (; starCol < nOfColumns; starCol++)\n            {\n              if (starMatrix[row + nOfRows * starCol])\n              {\n                break;\n              }\n            }\n            if (starCol == nOfColumns) /* no starred zero found */\n            {\n              /* move to step 4 */\n              step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows,\n                    nOfRows, nOfColumns, minDim, row, col);\n              return;\n            }\n            else\n            {\n              coveredRows[row] = true;\n              coveredColumns[starCol] = false;\n              zerosFound = true;\n              break;\n            }\n          }\n        }\n      }\n    }\n  }\n  /* move to step 5 */\n  step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,\n        nOfColumns, minDim);\n}\n\n// --------------------------------------------------------------------------\n//\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,\n                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,\n                                    size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col)\n{\n  const size_t nOfElements = nOfRows * nOfColumns;\n  /* generate temporary copy of starMatrix */\n  for (size_t n = 0; n < nOfElements; n++)\n  {\n    newStarMatrix[n] = starMatrix[n];\n  }\n  /* star current zero */\n  newStarMatrix[row + nOfRows * col] = true;\n  /* find starred zero in current column */\n  size_t starCol = col;\n  size_t starRow = 0;\n  for (; starRow < nOfRows; starRow++)\n  {\n    if (starMatrix[starRow + nOfRows * starCol])\n    {\n      break;\n    }\n  }\n  while (starRow < nOfRows)\n  {\n    /* unstar the starred zero */\n    newStarMatrix[starRow + nOfRows * starCol] = false;\n    /* find primed zero in current row */\n    size_t primeRow = starRow;\n    size_t primeCol = 0;\n    for (; primeCol < nOfColumns; primeCol++)\n    {\n      if (primeMatrix[primeRow + nOfRows * primeCol])\n      {\n        break;\n      }\n    }\n    /* star the primed zero */\n    newStarMatrix[primeRow + nOfRows * primeCol] = true;\n    /* find starred zero in current column */\n    starCol = primeCol;\n    for (starRow = 0; starRow < nOfRows; starRow++)\n    {\n      if (starMatrix[starRow + nOfRows * starCol])\n      {\n        break;\n      }\n    }\n  }\n  /* use temporary copy as new starMatrix */\n  /* delete all primes, uncover all rows */\n  for (size_t n = 0; n < nOfElements; n++)\n  {\n    primeMatrix[n] = false;\n    starMatrix[n] = newStarMatrix[n];\n  }\n  for (size_t n = 0; n < nOfRows; n++)\n  {\n    coveredRows[n] = false;\n  }\n  /* move to step 2a */\n  step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,\n         nOfColumns, minDim);\n}\n\n// --------------------------------------------------------------------------\n//\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,\n                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,\n                                    size_t nOfRows, size_t nOfColumns, size_t minDim)\n{\n  /* find smallest uncovered element h */\n  float h = std::numeric_limits<track_t>::max();\n  for (size_t row = 0; row < nOfRows; row++)\n  {\n    if (!coveredRows[row])\n    {\n      for (size_t col = 0; col < nOfColumns; col++)\n      {\n        if (!coveredColumns[col])\n        {\n          const float value = distMatrix[row + nOfRows * col];\n          if (value < h)\n          {\n            h = value;\n          }\n        }\n      }\n    }\n  }\n  /* add h to each covered row */\n  for (size_t row = 0; row < nOfRows; row++)\n  {\n    if (coveredRows[row])\n    {\n      for (size_t col = 0; col < nOfColumns; col++)\n      {\n        distMatrix[row + nOfRows * col] += h;\n      }\n    }\n  }\n  /* subtract h from each uncovered column */\n  for (size_t col = 0; col < nOfColumns; col++)\n  {\n    if (!coveredColumns[col])\n    {\n      for (size_t row = 0; row < nOfRows; row++)\n      {\n        distMatrix[row + nOfRows * col] -= h;\n      }\n    }\n  }\n  /* move to step 3 */\n  step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,\n        nOfColumns, minDim);\n}\n\n// --------------------------------------------------------------------------\n// Computes a suboptimal solution. Good for cases without forbidden assignments.\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost,\n                                                    const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)\n{\n  /* make working copy of distance Matrix */\n  const size_t nOfElements = nOfRows * nOfColumns;\n  float* distMatrix = (float*)malloc(nOfElements * sizeof(float));\n  for (size_t n = 0; n < nOfElements; n++)\n  {\n    distMatrix[n] = distMatrixIn[n];\n  }\n\n  /* recursively search for the minimum element and do the assignment */\n  for (;;)\n  {\n    /* find minimum distance observation-to-track pair */\n    float minValue = std::numeric_limits<track_t>::max();\n    size_t tmpRow = 0;\n    size_t tmpCol = 0;\n    for (size_t row = 0; row < nOfRows; row++)\n    {\n      for (size_t col = 0; col < nOfColumns; col++)\n      {\n        const float value = distMatrix[row + nOfRows * col];\n        if (value != std::numeric_limits<track_t>::max() && (value < minValue))\n        {\n          minValue = value;\n          tmpRow = row;\n          tmpCol = col;\n        }\n      }\n    }\n\n    if (minValue != std::numeric_limits<track_t>::max())\n    {\n      assignment[tmpRow] = static_cast<int>(tmpCol);\n      cost += minValue;\n      for (size_t n = 0; n < nOfRows; n++)\n      {\n        distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();\n      }\n      for (size_t n = 0; n < nOfColumns; n++)\n      {\n        distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();\n      }\n    }\n    else\n    {\n      break;\n    }\n  }\n\n  free(distMatrix);\n}\n// --------------------------------------------------------------------------\n// Computes a suboptimal solution. Good for cases with many forbidden assignments.\n// --------------------------------------------------------------------------\nvoid AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost,\n                                                    const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)\n{\n  /* make working copy of distance Matrix */\n  const size_t nOfElements = nOfRows * nOfColumns;\n  float* distMatrix = (float*)malloc(nOfElements * sizeof(float));\n  for (size_t n = 0; n < nOfElements; n++)\n  {\n    distMatrix[n] = distMatrixIn[n];\n  }\n\n  /* allocate memory */\n  int* nOfValidObservations = (int*)calloc(nOfRows, sizeof(int));\n  int* nOfValidTracks = (int*)calloc(nOfColumns, sizeof(int));\n\n  /* compute number of validations */\n  bool infiniteValueFound = false;\n  bool finiteValueFound = false;\n  for (size_t row = 0; row < nOfRows; row++)\n  {\n    for (size_t col = 0; col < nOfColumns; col++)\n    {\n      if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())\n      {\n        nOfValidTracks[col] += 1;\n        nOfValidObservations[row] += 1;\n        finiteValueFound = true;\n      }\n      else\n      {\n        infiniteValueFound = true;\n      }\n    }\n  }\n\n  if (infiniteValueFound)\n  {\n    if (!finiteValueFound)\n    {\n      free(nOfValidTracks);\n      free(nOfValidObservations);\n      free(distMatrix);\n      return;\n    }\n    bool repeatSteps = true;\n\n    while (repeatSteps)\n    {\n      repeatSteps = false;\n\n      /* step 1: reject assignments of multiply validated tracks to singly validated observations\t\t */\n      for (size_t col = 0; col < nOfColumns; col++)\n      {\n        bool singleValidationFound = false;\n        for (size_t row = 0; row < nOfRows; row++)\n        {\n          if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() &&\n              (nOfValidObservations[row] == 1))\n          {\n            singleValidationFound = true;\n            break;\n          }\n\n          if (singleValidationFound)\n          {\n            for (size_t row = 0; row < nOfRows; row++)\n              if ((nOfValidObservations[row] > 1) &&\n                  distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())\n              {\n                distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();\n                nOfValidObservations[row] -= 1;\n                nOfValidTracks[col] -= 1;\n                repeatSteps = true;\n              }\n          }\n        }\n      }\n\n      /* step 2: reject assignments of multiply validated observations to singly validated tracks */\n      if (nOfColumns > 1)\n      {\n        for (size_t row = 0; row < nOfRows; row++)\n        {\n          bool singleValidationFound = false;\n          for (size_t col = 0; col < nOfColumns; col++)\n          {\n            if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() && (nOfValidTracks[col] == 1))\n            {\n              singleValidationFound = true;\n              break;\n            }\n          }\n\n          if (singleValidationFound)\n          {\n            for (size_t col = 0; col < nOfColumns; col++)\n            {\n              if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())\n              {\n                distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();\n                nOfValidObservations[row] -= 1;\n                nOfValidTracks[col] -= 1;\n                repeatSteps = true;\n              }\n            }\n          }\n        }\n      }\n    } /* while(repeatSteps) */\n\n    /* for each multiply validated track that validates only with singly validated  */\n    /* observations, choose the observation with minimum distance */\n    for (size_t row = 0; row < nOfRows; row++)\n    {\n      if (nOfValidObservations[row] > 1)\n      {\n        bool allSinglyValidated = true;\n        float minValue = std::numeric_limits<track_t>::max();\n        size_t tmpCol = 0;\n        for (size_t col = 0; col < nOfColumns; col++)\n        {\n          const float value = distMatrix[row + nOfRows * col];\n          if (value != std::numeric_limits<track_t>::max())\n          {\n            if (nOfValidTracks[col] > 1)\n            {\n              allSinglyValidated = false;\n              break;\n            }\n            else if ((nOfValidTracks[col] == 1) && (value < minValue))\n            {\n              tmpCol = col;\n              minValue = value;\n            }\n          }\n        }\n\n        if (allSinglyValidated)\n        {\n          assignment[row] = static_cast<int>(tmpCol);\n          cost += minValue;\n          for (size_t n = 0; n < nOfRows; n++)\n          {\n            distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();\n          }\n          for (size_t n = 0; n < nOfColumns; n++)\n          {\n            distMatrix[row + nOfRows * n] = std::numeric_limits<track_t>::max();\n          }\n        }\n      }\n    }\n\n    // for each multiply validated observation that validates only with singly validated  track, choose the track with\n    // minimum distance\n    for (size_t col = 0; col < nOfColumns; col++)\n    {\n      if (nOfValidTracks[col] > 1)\n      {\n        bool allSinglyValidated = true;\n        float minValue = std::numeric_limits<track_t>::max();\n        size_t tmpRow = 0;\n        for (size_t row = 0; row < nOfRows; row++)\n        {\n          const float value = distMatrix[row + nOfRows * col];\n          if (value != std::numeric_limits<track_t>::max())\n          {\n            if (nOfValidObservations[row] > 1)\n            {\n              allSinglyValidated = false;\n              break;\n            }\n            else if ((nOfValidObservations[row] == 1) && (value < minValue))\n            {\n              tmpRow = row;\n              minValue = value;\n            }\n          }\n        }\n\n        if (allSinglyValidated)\n        {\n          assignment[tmpRow] = static_cast<int>(col);\n          cost += minValue;\n          for (size_t n = 0; n < nOfRows; n++)\n          {\n            distMatrix[n + nOfRows * col] = std::numeric_limits<track_t>::max();\n          }\n          for (size_t n = 0; n < nOfColumns; n++)\n          {\n            distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();\n          }\n        }\n      }\n    }\n  } /* if(infiniteValueFound) */\n\n  /* now, recursively search for the minimum element and do the assignment */\n  for (;;)\n  {\n    /* find minimum distance observation-to-track pair */\n    float minValue = std::numeric_limits<track_t>::max();\n    size_t tmpRow = 0;\n    size_t tmpCol = 0;\n    for (size_t row = 0; row < nOfRows; row++)\n    {\n      for (size_t col = 0; col < nOfColumns; col++)\n      {\n        const float value = distMatrix[row + nOfRows * col];\n        if (value != std::numeric_limits<track_t>::max() && (value < minValue))\n        {\n          minValue = value;\n          tmpRow = row;\n          tmpCol = col;\n        }\n      }\n    }\n\n    if (minValue != std::numeric_limits<track_t>::max())\n    {\n      assignment[tmpRow] = static_cast<int>(tmpCol);\n      cost += minValue;\n      for (size_t n = 0; n < nOfRows; n++)\n      {\n        distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();\n      }\n      for (size_t n = 0; n < nOfColumns; n++)\n      {\n        distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();\n      }\n    }\n    else\n    {\n      break;\n    }\n  }\n\n  /* free allocated memory */\n  free(nOfValidObservations);\n  free(nOfValidTracks);\n  free(distMatrix);\n}\n"
  },
  {
    "path": "src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp",
    "content": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this directory.\n\n#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h>\n#include \"opencv2/opencv.hpp\"\n#include <iostream>\n#include <vector>\n\n//---------------------------------------------------------------------------\n//---------------------------------------------------------------------------\nTKalmanFilter::TKalmanFilter(Point_t pt, track_t deltatime)\n{\n  // time increment (lower values makes target more \"massive\")\n  dt = deltatime;\n\n  // 6 state variables [x y z xdot ydot zdot], 3 measurements [x y z]\n  kalman = new cv::KalmanFilter(6, 3, 0);\n  // Transition cv::Matrix\n  kalman->transitionMatrix = (cv::Mat_<track_t>(6, 6) <<\n                              1, 0, 0, dt,  0,  0,\n                              0, 1, 0,  0, dt,  0,\n                              0, 0, 1,  0,  0, dt,\n                              0, 0, 0,  1,  0,  0,\n                              0, 0, 0,  0,  1,  0,\n                              0, 0, 0,  0,  0,  1);\n  // init...\n  LastPosition = pt;\n  kalman->statePre.at<track_t>(0) = pt.x;\n  kalman->statePre.at<track_t>(1) = pt.y;\n  kalman->statePre.at<track_t>(2) = pt.z;\n\n  kalman->statePre.at<track_t>(3) = 0;\n  kalman->statePre.at<track_t>(4) = 0;\n  kalman->statePre.at<track_t>(5) = 0;\n\n  kalman->statePost.at<track_t>(0) = pt.x;\n  kalman->statePost.at<track_t>(1) = pt.y;\n  kalman->statePost.at<track_t>(2) = pt.z;\n\n  // Nur x, y und z Koordinaten messbar!\n  kalman->measurementMatrix = (cv::Mat_<track_t>(3, 6) <<\n                               1, 0, 0, 0, 0, 0,\n                               0, 1, 0, 0, 0, 0,\n                               0, 0, 1, 0, 0, 0);\n\n  float sigma = 0.5; // Assume standard deviation for acceleration, todo: dynamic reconfigure\n  float c1 = pow(dt,4.0)/4.0;\n  float c2 = pow(dt,2.0);\n  float c3 = pow(dt,3.0)/2.0;\n  kalman->processNoiseCov = sigma*sigma*(cv::Mat_<float>(6, 6) <<\n                             c1,  0,  0, c3,  0,  0,\n                              0, c1,  0,  0, c3,  0,\n                              0,  0, c1,  0,  0, c3,\n                             c3,  0,  0, c2,  0,  0,\n                              0, c3,  0,  0, c2,  0,\n                              0,  0, c3,  0,  0, c2);\n\n  cv::setIdentity(kalman->measurementNoiseCov, cv::Scalar::all(5e-2));\n\n  cv::setIdentity(kalman->errorCovPost, cv::Scalar::all(1000000));\n}\n//---------------------------------------------------------------------------\nTKalmanFilter::~TKalmanFilter() { delete kalman; }\n\n//---------------------------------------------------------------------------\nvoid TKalmanFilter::Prediction()\n{\n  cv::Mat prediction = kalman->predict();\n  LastPosition = Point_t(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));\n  LastVelocity = Point_t(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5));\n}\n\n//---------------------------------------------------------------------------\nPoint_t TKalmanFilter::Update(Point_t p, bool DataCorrect)\n{\n  cv::Mat measurement(3, 1, Mat_t(1));\n  if (!DataCorrect)\n  {\n    measurement.at<track_t>(0) = LastPosition.x; // update using prediction\n    measurement.at<track_t>(1) = LastPosition.y;\n    measurement.at<track_t>(2) = LastPosition.z;\n  }\n  else\n  {\n    measurement.at<track_t>(0) = p.x; // update using measurements\n    measurement.at<track_t>(1) = p.y;\n    measurement.at<track_t>(2) = p.z;\n  }\n  // Correction\n  cv::Mat estimated = kalman->correct(measurement);\n  LastPosition.x = estimated.at<track_t>(0); // update using measurements\n  LastPosition.y = estimated.at<track_t>(1);\n  LastPosition.z = estimated.at<track_t>(2);\n  LastVelocity.x = estimated.at<track_t>(3);\n  LastVelocity.y = estimated.at<track_t>(4);\n  LastVelocity.z = estimated.at<track_t>(5);\n  return LastPosition;\n}\n//---------------------------------------------------------------------------\n"
  },
  {
    "path": "src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md",
    "content": "Multitarget Tracker\n===================\n\nThis code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].\nIt is utilized for the *CostmapToDynamicObstacles* plugin.\n\nThe *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].\nThe package itself depends on other third party packages with different licenses (refer to the package repository).\n\nTODO: Include the whole package as external CMake project.\n\n\n\n"
  },
  {
    "path": "src/costmap_to_lines_convex_hull.cpp",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann, Otniel Rinaldo\n *********************************************************************/\n\n#include <costmap_converter/costmap_to_lines_convex_hull.h>\n#include <costmap_converter/misc.h>\n#include <boost/thread.hpp>\n#include <boost/thread/mutex.hpp>\n#include <pluginlib/class_list_macros.h>\n\nPLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::BaseCostmapToPolygons)\n\nnamespace costmap_converter\n{\n\nCostmapToLinesDBSMCCH::CostmapToLinesDBSMCCH() : CostmapToPolygonsDBSMCCH() \n{\n    dynamic_recfg_ = NULL;\n}\n  \nCostmapToLinesDBSMCCH::~CostmapToLinesDBSMCCH() \n{\n  if (dynamic_recfg_ != NULL)\n    delete dynamic_recfg_;\n}\n  \nvoid CostmapToLinesDBSMCCH::initialize(ros::NodeHandle nh)\n{ \n    // DB SCAN\n    nh.param(\"cluster_max_distance\", parameter_.max_distance_, 0.4);\n    nh.param(\"cluster_min_pts\", parameter_.min_pts_, 2);\n    nh.param(\"cluster_max_pts\", parameter_.max_pts_, 30);\n    // convex hull\n    nh.param(\"convex_hull_min_pt_separation\", parameter_.min_keypoint_separation_, 0.1);\n    parameter_buffered_ = parameter_;\n    \n    // Line extraction\n    nh.param(\"support_pts_max_dist\", support_pts_max_dist_, 0.3);\n    nh.param(\"support_pts_max_dist_inbetween\", support_pts_max_dist_inbetween_, 1.0);\n    nh.param(\"min_support_pts\", min_support_pts_, 2);\n    \n    // setup dynamic reconfigure\n    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>(nh);\n    dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSMCCH::reconfigureCB, this, _1, _2);\n    dynamic_recfg_->setCallback(cb);\n    \n    // deprecated\n    if (nh.hasParam(\"support_pts_min_dist_\") || nh.hasParam(\"support_pts_min_dist\"))\n      ROS_WARN(\"CostmapToLinesDBSMCCH: Parameter 'support_pts_min_dist' is deprecated and not included anymore.\");\n    if (nh.hasParam(\"min_support_pts_\"))\n      ROS_WARN(\"CostmapToLinesDBSMCCH: Parameter 'min_support_pts_' is not found. Remove the underscore.\");\n}  \n  \nvoid CostmapToLinesDBSMCCH::compute()\n{\n    std::vector< std::vector<KeyPoint> > clusters;\n    dbScan(clusters);\n    \n    // Create new polygon container\n    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());  \n    \n    \n    // add convex hulls to polygon container\n    for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise\n    {\n      geometry_msgs::Polygon polygon;\n      convexHull2(clusters[i], polygon );\n      \n      // now extract lines of the polygon (by searching for support points in the cluster)  \n      // and add them to the polygon container\n      extractPointsAndLines(clusters[i], polygon, std::back_inserter(*polygons));\n    }\n    \n    // add our non-cluster points to the polygon container (as single points)\n    if (!clusters.empty())\n    {\n      for (int i=0; i < clusters.front().size(); ++i)\n      {\n        polygons->push_back( geometry_msgs::Polygon() );\n        convertPointToPolygon(clusters.front()[i], polygons->back());\n      }\n    }\n        \n    // replace shared polygon container\n    updatePolygonContainer(polygons);\n}\n\ntypedef CostmapToLinesDBSMCCH CL;\nbool sort_keypoint_x(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) \n{ return (cluster[i].x<cluster[j].x) || (cluster[i].x == cluster[j].x && cluster[i].y < cluster[j].y); }\nbool sort_keypoint_y(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) \n{ return (cluster[i].y<cluster[j].y) || (cluster[i].y == cluster[j].y && cluster[i].x < cluster[j].x); }\n\nvoid CostmapToLinesDBSMCCH::extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::Polygon& polygon,\n                                                  std::back_insert_iterator< std::vector<geometry_msgs::Polygon> > lines)\n{\n   if (polygon.points.empty())\n     return;\n  \n   if (polygon.points.size() < 2)\n   {\n     lines = polygon; // our polygon is just a point, push back onto the output sequence\n     return;\n   }\n   int n = (int)polygon.points.size();\n     \n   for (int i=1; i<n; ++i) // this implemenation requires an explicitly closed polygon (start vertex = end vertex)\n   {\n        const geometry_msgs::Point32* vertex1 = &polygon.points[i-1];\n        const geometry_msgs::Point32* vertex2 = &polygon.points[i];\n\n        // check how many vertices belong to the line (sometimes the convex hull algorithm finds multiple vertices on a line,\n        // in case of small coordinate deviations)\n        double dx = vertex2->x - vertex1->x;\n        double dy = vertex2->y - vertex1->y;\n//         double norm = std::sqrt(dx*dx + dy*dy);\n//         dx /= norm;\n//         dy /= norm;\n//         for (int j=i; j<(int)polygon.points.size() - 2; ++j)\n//         {\n//           const geometry_msgs::Point32* vertex_jp2 = &polygon.points[j+2];\n//           double dx2 = vertex_jp2->x - vertex2->x;\n//           double dy2 = vertex_jp2->y - vertex2->y;\n//           double norm2 = std::sqrt(dx2*dx2 + dy2*dy2);\n//           dx2 /= norm2;\n//           dy2 /= norm2;\n//           if (std::abs(dx*dx2 + dy*dy2) < 0.05) //~3 degree\n//           {\n//             vertex2 = &polygon.points[j+2];\n//             i = j; // DO NOT USE \"i\" afterwards\n//           }\n//           else break;\n//         }\n       \n        //Search support points\n        std::vector<std::size_t> support_points; // store indices of cluster\n        \n        for (std::size_t k = 0; k < cluster.size(); ++k)\n        {\n            bool is_inbetween = false;\n            double dist_line_to_point = computeDistanceToLineSegment( cluster[k], *vertex1, *vertex2, &is_inbetween );\n\n            if(is_inbetween && dist_line_to_point <= support_pts_max_dist_)\n            {\n              support_points.push_back(k);\n              continue;\n            }\n        }\n\n        // now check if the inlier models a line by checking the minimum distance between all support points (avoid lines over gaps)\n        // and by checking if the minium number of points is reached // deactivate by setting support_pts_max_dist_inbetween_==0\n        bool is_line=true;\n        if (support_pts_max_dist_inbetween_!=0)\n        {\n          if ((int)support_points.size() >= min_support_pts_ + 2) // +2 since start and goal are included\n          {          \n            // sort points\n            if (std::abs(dx) >= std::abs(dy))\n              std::sort(support_points.begin(), support_points.end(), boost::bind(sort_keypoint_x, _1, _2, boost::cref(cluster)));\n            else \n              std::sort(support_points.begin(), support_points.end(), boost::bind(sort_keypoint_y, _1, _2, boost::cref(cluster)));\n            \n            // now calculate distances\n            for (int k = 1; k < int(support_points.size()); ++k)\n            {\n              double dist_x = cluster[support_points[k]].x - cluster[support_points[k-1]].x;\n              double dist_y = cluster[support_points[k]].y - cluster[support_points[k-1]].y;\n              double dist = std::sqrt( dist_x*dist_x + dist_y*dist_y);\n              if (dist > support_pts_max_dist_inbetween_)\n              {\n                is_line = false;\n                break;\n              }\n            }\n            \n          }\n          else\n            is_line = false;\n        }\n        \n        if (is_line)\n        {\n          // line found:\n          geometry_msgs::Polygon line;\n          line.points.push_back(*vertex1);\n          line.points.push_back(*vertex2);\n          lines = line; // back insertion\n          \n          // remove inlier from list\n          // iterate in reverse order, otherwise indices are not valid after erasing elements\n          std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();\n          for (; support_it != support_points.rend(); ++support_it)\n          {\n            cluster.erase(cluster.begin() + *support_it);\n          }\n        }\n        else\n        {\n            // remove goal, since it will be added in the subsequent iteration\n            //support_points.pop_back();\n          // old:\n//             // add vertex 1 as single point\n//             geometry_msgs::Polygon point;\n//             point.points.push_back(*vertex1);\n//             lines = point; // back insertion\n\n           // add complete inlier set as points \n//            for (int k = 0; k < int(support_points.size()); ++k)\n//            {\n//               geometry_msgs::Polygon polygon;\n//               convertPointToPolygon(cluster[support_points[k]], polygon);\n//               lines = polygon; // back insertion\n//            }\n           \n          // remove inlier from list and add them as point obstacles\n          // iterate in reverse order, otherwise indices are not valid after erasing elements\n          std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();\n          for (; support_it != support_points.rend(); ++support_it)\n          {\n            geometry_msgs::Polygon polygon;\n            convertPointToPolygon(cluster[*support_it], polygon);\n            lines = polygon; // back insertion\n            \n            cluster.erase(cluster.begin() + *support_it);\n          }\n        }\n    }\n \n    // add all remaining cluster points, that do not belong to a line\n    for (int i=0; i<(int)cluster.size();++i)\n    {\n        geometry_msgs::Polygon polygon;\n        convertPointToPolygon(cluster[i], polygon);\n        lines = polygon; // back insertion\n    }\n\n}\n\nvoid CostmapToLinesDBSMCCH::reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level)\n{\n    boost::mutex::scoped_lock lock(parameter_mutex_);\n    parameter_buffered_.max_distance_ = config.cluster_max_distance;\n    parameter_buffered_.min_pts_ = config.cluster_min_pts;\n    parameter_buffered_.max_pts_ = config.cluster_max_pts;\n    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;\n    support_pts_max_dist_ = config.support_pts_max_dist;\n    support_pts_max_dist_inbetween_ = config.support_pts_max_dist_inbetween;\n    min_support_pts_ = config.min_support_pts;\n}\n\n\n\n}//end namespace costmap_converter\n\n\n"
  },
  {
    "path": "src/costmap_to_lines_ransac.cpp",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann\n *********************************************************************/\n\n#include <costmap_converter/costmap_to_lines_ransac.h>\n#include <boost/thread.hpp>\n#include <boost/thread/mutex.hpp>\n#include <pluginlib/class_list_macros.h>\n\nPLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::BaseCostmapToPolygons)\n\nnamespace costmap_converter\n{\n\nCostmapToLinesDBSRANSAC::CostmapToLinesDBSRANSAC() : CostmapToPolygonsDBSMCCH() \n{\n  dynamic_recfg_ = NULL;\n}\n\nCostmapToLinesDBSRANSAC::~CostmapToLinesDBSRANSAC() \n{\n  if (dynamic_recfg_ != NULL)\n    delete dynamic_recfg_;\n}\n  \nvoid CostmapToLinesDBSRANSAC::initialize(ros::NodeHandle nh)\n{ \n    // DB SCAN\n    nh.param(\"cluster_max_distance\", parameter_.max_distance_, 0.4);\n    nh.param(\"cluster_min_pts\", parameter_.min_pts_, 2);\n    nh.param(\"cluster_max_pts\", parameter_.max_pts_, 30);\n    // convex hull (only necessary if outlier filtering is enabled)\n    nh.param(\"convex_hull_min_pt_separation\", parameter_.min_keypoint_separation_, 0.1);\n    parameter_buffered_ = parameter_;\n\n    // ransac\n    nh.param(\"ransac_inlier_distance\", ransac_inlier_distance_, 0.2);\n    nh.param(\"ransac_min_inliers\", ransac_min_inliers_, 10);\n    nh.param(\"ransac_no_iterations\", ransac_no_iterations_, 2000);\n    nh.param(\"ransac_remainig_outliers\", ransac_remainig_outliers_, 3);\n    nh.param(\"ransac_convert_outlier_pts\", ransac_convert_outlier_pts_, true);\n    nh.param(\"ransac_filter_remaining_outlier_pts\", ransac_filter_remaining_outlier_pts_, false);\n    \n    // setup dynamic reconfigure\n    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>(nh);\n    dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSRANSAC::reconfigureCB, this, _1, _2);\n    dynamic_recfg_->setCallback(cb);\n}  \n  \nvoid CostmapToLinesDBSRANSAC::compute()\n{\n    std::vector< std::vector<KeyPoint> > clusters;\n    dbScan(clusters);\n    \n    // Create new polygon container\n    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());  \n    \n    \n    // fit lines using ransac for each cluster\n    for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise\n    { \n      \n      while (clusters[i].size() > ransac_remainig_outliers_)\n      {\n//         std::vector<KeyPoint> inliers;\n        std::vector<KeyPoint> outliers;\n        std::pair<KeyPoint,KeyPoint> model;\n        if (!lineRansac(clusters[i], ransac_inlier_distance_, ransac_no_iterations_, ransac_min_inliers_, model, NULL, &outliers ) )\n          break;\n        \n        // add to polygon container\n        geometry_msgs::Polygon line;\n        line.points.resize(2);\n        model.first.toPointMsg(line.points.front());\n        model.second.toPointMsg(line.points.back());\n        polygons->push_back(line);\n        \n        clusters[i] = outliers;\n      }\n      // create point polygons for remaining outliers\n      if (ransac_convert_outlier_pts_)\n      {\n        if (ransac_filter_remaining_outlier_pts_)\n        {\n          // take edge points of a convex polygon\n          // these points define a cluster and since all lines are extracted,\n          // we remove points from the interior...\n          geometry_msgs::Polygon polygon;\n          convexHull2(clusters[i], polygon);\n          for (int j=0; j < (int)polygon.points.size(); ++j)\n          {\n            polygons->push_back(geometry_msgs::Polygon());\n            convertPointToPolygon(polygon.points[j], polygons->back());\n          }\n        }\n        else\n        {\n          for (int j = 0; j < (int)clusters[i].size(); ++j)\n          {\n            polygons->push_back(geometry_msgs::Polygon());\n            convertPointToPolygon(clusters[i][j], polygons->back());           \n          }\n        }\n\n      }\n      \n      \n    }\n    \n    // add our non-cluster points to the polygon container (as single points)\n    if (!clusters.empty())\n    {\n      for (int i=0; i < clusters.front().size(); ++i)\n      {\n        polygons->push_back( geometry_msgs::Polygon() );\n        convertPointToPolygon(clusters.front()[i], polygons->back());\n      }\n    }\n        \n    // replace shared polygon container\n    updatePolygonContainer(polygons);\n}\n\n\nbool CostmapToLinesDBSRANSAC::lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations,\n                                         int min_inliers, std::pair<KeyPoint, KeyPoint>& best_model, \n                                         std::vector<KeyPoint>* inliers, std::vector<KeyPoint>* outliers)\n{\n  if (data.size() < 2 ||  data.size() < min_inliers)\n  {\n    return false;\n  }\n  \n  boost::random::uniform_int_distribution<> distribution(0, data.size()-1);\n  \n  std::pair<int, int> best_model_idx;\n  int best_no_inliers = -1;\n  \n  \n  for (int i=0; i < no_iterations; ++i)\n  {\n    // choose random points to define a line candidate\n    int start_idx = distribution(rnd_generator_);\n    int end_idx = start_idx;\n    while (end_idx == start_idx)\n      end_idx = distribution(rnd_generator_);\n    \n    \n    // compute inliers\n    int no_inliers = 0;\n    for (int j=0; j<(int)data.size(); ++j)\n    {\n      if ( isInlier(data[j], data[start_idx], data[end_idx], inlier_distance) )\n        ++no_inliers;\n    }\n    \n    if (no_inliers > best_no_inliers)\n    {\n      best_no_inliers = no_inliers;\n      best_model_idx.first = start_idx;\n      best_model_idx.second = end_idx;\n    }\n  }\n  \n  best_model.first = data[best_model_idx.first];\n  best_model.second = data[best_model_idx.second];\n  \n  if (best_no_inliers < min_inliers)\n    return false;\n  \n  // Now repeat the calculation for the best model in order to obtain the inliers and outliers set\n  // This might be faster if no_iterations is large, but TEST\n  if (inliers || outliers)\n  {\n    if (inliers)\n      inliers->clear();\n    if (outliers)\n      outliers->clear();\n    \n    int no_inliers = 0;\n    for (int i=0; i<(int)data.size(); ++i)\n    {\n        if ( isInlier( data[i], best_model.first, best_model.second, inlier_distance ) )\n        {\n          if (inliers)\n            inliers->push_back( data[i] );\n        }\n        else\n        {\n          if (outliers)\n            outliers->push_back( data[i] );\n        }\n    }\n  }\n  \n  return true;\n}\n\nbool CostmapToLinesDBSRANSAC::linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept, \n                                               double* mean_x_out, double* mean_y_out)\n{\n  if (data.size() < 2)\n  {\n    ROS_ERROR(\"CostmapToLinesDBSRANSAC: at least 2 data points required for linear regression\");\n    return false;\n  }\n  \n  double mean_x = 0;\n  double mean_y = 0;\n  for (int i=0; i<(int)data.size(); ++i)\n  {\n    mean_x += data[i].x;\n    mean_y += data[i].y;\n  }\n  mean_x /= double(data.size());\n  mean_y /= double(data.size());\n  \n  if (mean_x_out)\n    *mean_x_out = mean_x;\n \n  if (mean_y_out)\n    *mean_y_out = mean_y;\n  \n  double numerator = 0.0;\n  double denominator = 0.0;\n\n  for(int i=0; i<(int)data.size(); ++i)\n  {\n      double dx = data[i].x - mean_x;\n      numerator += dx * (data[i].y - mean_y);\n      denominator += dx*dx;\n  }\n  \n  if (denominator == 0)\n  {\n    ROS_ERROR(\"CostmapToLinesDBSRANSAC: linear regression failed, denominator 0\");\n    return false;\n  }\n  else\n    slope = numerator / denominator;\n  \n  intercept = mean_y - slope * mean_x;\n  return true;\n}\n\n\nvoid CostmapToLinesDBSRANSAC::reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level)\n{\n    boost::mutex::scoped_lock lock(parameter_mutex_);\n    parameter_buffered_.max_distance_ = config.cluster_max_distance;\n    parameter_buffered_.min_pts_ = config.cluster_min_pts;\n    parameter_buffered_.max_pts_ = config.cluster_max_pts;\n    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;\n    ransac_inlier_distance_ = config.ransac_inlier_distance;\n    ransac_min_inliers_ = config.ransac_min_inliers;\n    ransac_no_iterations_ = config.ransac_no_iterations;\n    ransac_remainig_outliers_ = config.ransac_remainig_outliers;\n    ransac_convert_outlier_pts_ = config.ransac_convert_outlier_pts;\n    ransac_filter_remaining_outlier_pts_ = config.ransac_filter_remaining_outlier_pts;\n}\n\n\n/*\nvoid CostmapToLinesDBSRANSAC::adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2, \n                                               KeyPoint& line_start, KeyPoint& line_end)\n{\n  line_start = linept1; \n  line_end = linept2;\n\n  // infinite line is defined by linept1 and linept2\n  double dir_x = line_end.x - line_start.x;\n  double dir_y = line_end.y - line_start.y;\n  double norm = std::sqrt(dir_x*dir_x + dir_y*dir_y);\n  dir_x /= norm;\n  dir_y /= norm;\n  \n  // project data onto the line and check if the distance is increased in both directions\n  for (int i=0; i < (int) data.size(); ++i)\n  {\n    double dx = data[i].x - line_start.x;\n    double dy = data[i].y - line_start.y;\n    // check scalar product at start\n    double extension = dx*dir_x + dy*dir_y;\n    if (extension<0)\n    {\n      line_start.x -=  dir_x*extension;\n      line_start.y -=  dir_y*extension;\n    }\n    else\n    {\n      dx = data[i].x - line_end.x;\n      dy = data[i].y - line_end.y;\n      // check scalar product at end\n      double extension = dx*dir_x + dy*dir_y;\n      if (extension>0)\n      {\n        line_end.x +=  dir_x*extension;\n        line_end.y +=  dir_y*extension;\n      }\n    }\n  }\n  \n}*/\n\n\n}//end namespace costmap_converter\n\n\n"
  },
  {
    "path": "src/costmap_to_polygons.cpp",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann, Otniel Rinaldo\n *********************************************************************/\n\n#include <costmap_converter/costmap_to_polygons.h>\n#include <costmap_converter/misc.h>\n#include <boost/thread.hpp>\n#include <boost/thread/mutex.hpp>\n#include <pluginlib/class_list_macros.h>\n\nPLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons)\n\nnamespace\n{\n\n/**\n * @brief Douglas-Peucker Algorithm for fitting lines into ordered set of points\n * \n * Douglas-Peucker Algorithm, see https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm\n * \n * @param begin iterator pointing to the begin of the range of points\n * @param end interator pointing to the end of the range of points\n * @param epsilon distance criteria for removing points if it is closer to the line segment than this\n * @param result the simplified polygon\n */\nstd::vector<geometry_msgs::Point32> douglasPeucker(std::vector<geometry_msgs::Point32>::iterator begin,\n  std::vector<geometry_msgs::Point32>::iterator end, double epsilon)\n{\n  if (std::distance(begin, end) <= 2)\n  {\n    return std::vector<geometry_msgs::Point32>(begin, end);\n  }\n\n  // Find the point with the maximum distance from the line [begin, end)\n  double dmax = std::numeric_limits<double>::lowest();\n  std::vector<geometry_msgs::Point32>::iterator max_dist_it;\n  std::vector<geometry_msgs::Point32>::iterator last = std::prev(end);\n  for (auto it = std::next(begin); it != last; ++it)\n  {\n    double d = costmap_converter::computeSquaredDistanceToLineSegment(*it, *begin, *last);\n    if (d > dmax)\n    {\n      max_dist_it = it;\n      dmax = d;\n    }\n  }\n\n  if (dmax < epsilon * epsilon)\n  { // termination criterion reached, line is good enough\n    std::vector<geometry_msgs::Point32> result;\n    result.push_back(*begin);\n    result.push_back(*last);\n    return result;\n  }\n\n  // Recursive calls for the two splitted parts\n  auto firstLineSimplified = douglasPeucker(begin, std::next(max_dist_it), epsilon);\n  auto secondLineSimplified = douglasPeucker(max_dist_it, end, epsilon);\n\n  // Combine the two lines into one line and return the merged line.\n  // Note that we have to skip the first point of the second line, as it is duplicated above.\n  firstLineSimplified.insert(firstLineSimplified.end(),\n    std::make_move_iterator(std::next(secondLineSimplified.begin())),\n    std::make_move_iterator(secondLineSimplified.end()));\n  return firstLineSimplified;\n}\n\n} // end namespace\n\nnamespace costmap_converter\n{\n    \nCostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons()\n{\n  costmap_ = NULL;\n  dynamic_recfg_ = NULL;\n  neighbor_size_x_ = neighbor_size_y_ = -1;\n  offset_x_ = offset_y_ = 0.;\n}\n\nCostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH() \n{\n  if (dynamic_recfg_ != NULL)\n    delete dynamic_recfg_;\n}\n\nvoid CostmapToPolygonsDBSMCCH::initialize(ros::NodeHandle nh)\n{\n    costmap_ = NULL;\n   \n    nh.param(\"cluster_max_distance\", parameter_.max_distance_, 0.4);\n    nh.param(\"cluster_min_pts\", parameter_.min_pts_, 2);\n    nh.param(\"cluster_max_pts\", parameter_.max_pts_, 30);\n    nh.param(\"convex_hull_min_pt_separation\", parameter_.min_keypoint_separation_, 0.1);\n    \n    parameter_buffered_ = parameter_;\n    \n    // setup dynamic reconfigure\n    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>(nh);\n    dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2);\n    dynamic_recfg_->setCallback(cb);\n}\n\n\nvoid CostmapToPolygonsDBSMCCH::compute()\n{\n    std::vector< std::vector<KeyPoint> > clusters;\n    dbScan(clusters);\n    \n    // Create new polygon container\n    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());\n    \n    \n    // add convex hulls to polygon container\n    for (std::size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise\n    {\n      polygons->push_back( geometry_msgs::Polygon() );\n      convexHull2(clusters[i], polygons->back() );\n    }\n    \n    // add our non-cluster points to the polygon container (as single points)\n    if (!clusters.empty())\n    {\n      for (std::size_t i=0; i < clusters.front().size(); ++i)\n      {\n        polygons->push_back( geometry_msgs::Polygon() );\n        convertPointToPolygon(clusters.front()[i], polygons->back());\n      }\n    }\n    \n    // replace shared polygon container\n    updatePolygonContainer(polygons);\n}\n\nvoid CostmapToPolygonsDBSMCCH::setCostmap2D(costmap_2d::Costmap2D *costmap)\n{\n    if (!costmap)\n      return;\n\n    costmap_ = costmap;     \n    \n    updateCostmap2D();\n}\n\nvoid CostmapToPolygonsDBSMCCH::updateCostmap2D()\n{\n      occupied_cells_.clear();\n      \n      if (!costmap_->getMutex())\n      {\n        ROS_ERROR(\"Cannot update costmap since the mutex pointer is null\");\n        return;\n      }\n      \n      { // get a copy of our parameters from dynamic reconfigure\n        boost::mutex::scoped_lock lock(parameter_mutex_);\n        parameter_ = parameter_buffered_;\n      }\n      \n      costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());\n\n      // allocate neighbor lookup\n      int cells_x = int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1;\n      int cells_y = int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1;\n\n      if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_) {\n        neighbor_size_x_ = cells_x;\n        neighbor_size_y_ = cells_y;\n        neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_);\n      }\n      offset_x_ = costmap_->getOriginX();\n      offset_y_ = costmap_->getOriginY();\n      for (auto& n : neighbor_lookup_)\n        n.clear();\n\n      // get indices of obstacle cells\n      for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)\n      {\n        for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)\n        {\n          int value = costmap_->getCost(i,j);\n          if(value >= costmap_2d::LETHAL_OBSTACLE)\n          {\n            double x, y;\n            costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);\n            addPoint(x, y);\n          }\n        }\n      }\n}\n\n\nvoid CostmapToPolygonsDBSMCCH::dbScan(std::vector< std::vector<KeyPoint> >& clusters)\n{\n  std::vector<bool> visited(occupied_cells_.size(), false);\n\n  clusters.clear();  \n  \n  //DB Scan Algorithm\n  int cluster_id = 0; // current cluster_id\n  clusters.push_back(std::vector<KeyPoint>());\n  for(int i = 0; i< (int)occupied_cells_.size(); i++)\n  {\n    if(!visited[i]) //keypoint has not been visited before\n    {\n      visited[i] = true; // mark as visited\n      std::vector<int> neighbors;\n      regionQuery(i, neighbors); //Find neighbors around the keypoint\n      if((int)neighbors.size() < parameter_.min_pts_) //If not enough neighbors are found, mark as noise\n      {\t\t\n        clusters[0].push_back(occupied_cells_[i]);\n      }\n      else\n      {\n        ++cluster_id; // increment current cluster_id\n        clusters.push_back(std::vector<KeyPoint>());\n        \n        // Expand the cluster\n        clusters[cluster_id].push_back(occupied_cells_[i]);\n        for(int j = 0; j<(int)neighbors.size(); j++)\n        {\n          if ((int)clusters[cluster_id].size() == parameter_.max_pts_)\n            break;\n          \n          if(!visited[neighbors[j]]) //keypoint has not been visited before\n          {\n            visited[neighbors[j]] = true;  // mark as visited\n            std::vector<int> further_neighbors;\n            regionQuery(neighbors[j], further_neighbors); //Find more neighbors around the new keypoint\n//             if(further_neighbors.size() < min_pts_)\n//             {\t  \n//               clusters[0].push_back(occupied_cells[neighbors[j]]);\n//             }\n//             else\n            if ((int)further_neighbors.size() >= parameter_.min_pts_)\n            {\n              // neighbors found\n              neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end());  //Add these newfound P' neighbour to P neighbour vector \"nb_indeces\"\n              clusters[cluster_id].push_back(occupied_cells_[neighbors[j]]);\n            }\n          }\n        }\n      }\t      \n    }\n  } \n}\n  \nvoid CostmapToPolygonsDBSMCCH::regionQuery(int curr_index, std::vector<int>& neighbors)\n{\n    neighbors.clear();\n\n    double dist_sqr_threshold = parameter_.max_distance_ * parameter_.max_distance_;\n    const KeyPoint& kp = occupied_cells_[curr_index];\n    int cx, cy;\n    pointToNeighborCells(kp, cx,cy);\n    \n    // loop over the neighboring cells for looking up the points\n    const int offsets[9][2] = {{-1, -1}, {0, -1}, {1, -1},\n                               {-1,  0}, {0,  0}, {1,  0},\n                               {-1,  1}, {0,  1}, {1,  1}};\n    for (int i = 0; i < 9; ++i)\n    {\n      int idx = neighborCellsToIndex(cx + offsets[i][0], cy + offsets[i][1]);\n      if (idx < 0 || idx >= int(neighbor_lookup_.size()))\n        continue;\n      const std::vector<int>& pointIndicesToCheck = neighbor_lookup_[idx];\n      for (int point_idx : pointIndicesToCheck) {\n        if (point_idx == curr_index) // point is not a neighbor to itself\n          continue;\n        const KeyPoint& other = occupied_cells_[point_idx];\n        double dx = other.x - kp.x;\n        double dy = other.y - kp.y;\n        double dist_sqr = dx*dx + dy*dy;\n        if (dist_sqr <= dist_sqr_threshold)\n          neighbors.push_back(point_idx);\n      }\n    }\n}\n\nbool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1, const CostmapToPolygonsDBSMCCH::KeyPoint& p2)\n{\n  return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);\n}\n\nvoid CostmapToPolygonsDBSMCCH::convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)\n{\n    //Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull\n  \n    int k = 0;\n    int n = cluster.size();\n    \n    // sort points according to x coordinate (TODO. is it already sorted due to the map representation?)\n    std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller);\n    \n    polygon.points.resize(2*n);\n      \n    // lower hull\n    for (int i = 0; i < n; ++i)\n    {\n      while (k >= 2 && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0) \n      {\n        --k;\n      }\n      cluster[i].toPointMsg(polygon.points[k]);\n      ++k;\n    }\n      \n    // upper hull  \n    for (int i = n-2, t = k+1; i >= 0; --i) \n    {\n      while (k >= t && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)\n      {\n        --k;\n      }\n      cluster[i].toPointMsg(polygon.points[k]);\n      ++k;\n    }\n    \n\n    polygon.points.resize(k); // original\n    // TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice.\n//     polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost\n\n    simplifyPolygon(polygon);\n}\n\n\n\nvoid CostmapToPolygonsDBSMCCH::convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)\n{\n    std::vector<KeyPoint>& P = cluster;\n    std::vector<geometry_msgs::Point32>& points = polygon.points;\n\n    // Sort P by x and y\n    std::sort(P.begin(), P.end(), isXCoordinateSmaller);\n\n    // the output array H[] will be used as the stack\n    int i;                 // array scan index\n\n    // Get the indices of points with min x-coord and min|max y-coord\n    int minmin = 0, minmax;\n    double xmin = P[0].x;\n    for (i = 1; i < (int)P.size(); i++)\n        if (P[i].x != xmin) break;\n    minmax = i - 1;\n    if (minmax == (int)P.size() - 1)\n    {   // degenerate case: all x-coords == xmin\n        points.push_back(geometry_msgs::Point32());\n        P[minmin].toPointMsg(points.back());\n        if (P[minmax].y != P[minmin].y) // a  nontrivial segment\n        {\n            points.push_back(geometry_msgs::Point32());\n            P[minmax].toPointMsg(points.back());\n        }\n        // add polygon endpoint\n        points.push_back(geometry_msgs::Point32());\n        P[minmin].toPointMsg(points.back());\n        return;\n    }\n\n    // Get the indices of points with max x-coord and min|max y-coord\n    int maxmin, maxmax = (int)P.size() - 1;\n    double xmax = P.back().x;\n    for (i = P.size() - 2; i >= 0; i--)\n        if (P[i].x != xmax) break;\n    maxmin = i+1;\n\n    // Compute the lower hull on the stack H\n    // push  minmin point onto stack\n    points.push_back(geometry_msgs::Point32());\n    P[minmin].toPointMsg(points.back());\n    i = minmax;\n    while (++i <= maxmin)\n    {\n        // the lower line joins P[minmin]  with P[maxmin]\n        if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)\n            continue;           // ignore P[i] above or on the lower line\n\n        while (points.size() > 1)         // there are at least 2 points on the stack\n        {\n            // test if  P[i] is left of the line at the stack top\n            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)\n                break;         // P[i] is a new hull  vertex\n            points.pop_back();         // pop top point off  stack\n        }\n        // push P[i] onto stack\n        points.push_back(geometry_msgs::Point32());\n        P[i].toPointMsg(points.back());\n    }\n\n    // Next, compute the upper hull on the stack H above  the bottom hull\n    if (maxmax != maxmin)      // if  distinct xmax points\n    {\n         // push maxmax point onto stack\n         points.push_back(geometry_msgs::Point32());\n         P[maxmax].toPointMsg(points.back());\n    }\n    int bot = (int)points.size();                  // the bottom point of the upper hull stack\n    i = maxmin;\n    while (--i >= minmax)\n    {\n        // the upper line joins P[maxmax]  with P[minmax]\n        if (cross( P[maxmax], P[minmax], P[i])  >= 0 && i > minmax)\n            continue;           // ignore P[i] below or on the upper line\n\n        while ((int)points.size() > bot)     // at least 2 points on the upper stack\n        {\n            // test if  P[i] is left of the line at the stack top\n            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)\n                break;         // P[i] is a new hull  vertex\n            points.pop_back();         // pop top point off stack\n        }\n        // push P[i] onto stack\n        points.push_back(geometry_msgs::Point32());\n        P[i].toPointMsg(points.back());\n    }\n    if (minmax != minmin)\n    {\n        // push  joining endpoint onto stack\n        points.push_back(geometry_msgs::Point32());\n        P[minmin].toPointMsg(points.back());\n    }\n    \n    simplifyPolygon(polygon);\n}\n\nvoid CostmapToPolygonsDBSMCCH::simplifyPolygon(geometry_msgs::Polygon& polygon)\n{\n  size_t triangleThreshold = 3;\n  // check if first and last point are the same. If yes, a triangle has 4 points\n  if (polygon.points.size() > 1\n      && std::abs(polygon.points.front().x - polygon.points.back().x) < 1e-5\n      && std::abs(polygon.points.front().y - polygon.points.back().y) < 1e-5)\n  {\n    triangleThreshold = 4;\n  }\n  if (polygon.points.size() <= triangleThreshold) // nothing to do for triangles or lines\n    return;\n  // TODO Reason about better start conditions for splitting lines, e.g., by\n  // https://en.wikipedia.org/wiki/Rotating_calipers\n  polygon.points = douglasPeucker(polygon.points.begin(), polygon.points.end(), parameter_.min_keypoint_separation_);;\n}\n\nvoid CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons)\n{\n  boost::mutex::scoped_lock lock(mutex_);\n  polygons_ = polygons;\n}\n\n\nPolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()\n{\n  boost::mutex::scoped_lock lock(mutex_);\n  PolygonContainerConstPtr polygons = polygons_;\n  return polygons;\n}\n\nvoid CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level)\n{\n  boost::mutex::scoped_lock lock(parameter_mutex_);\n  parameter_buffered_.max_distance_ = config.cluster_max_distance;\n  parameter_buffered_.min_pts_ = config.cluster_min_pts;\n  parameter_buffered_.max_pts_ = config.cluster_max_pts;\n  parameter_buffered_.min_keypoint_separation_ = config.convex_hull_min_pt_separation;\n}\n\n}//end namespace costmap_converter\n\n\n"
  },
  {
    "path": "src/costmap_to_polygons_concave.cpp",
    "content": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *\n *  Copyright (c) 2016,\n *  TU Dortmund - Institute of Control Theory and Systems Engineering.\n *  All rights reserved.\n *\n *  Redistribution and use in source and binary forms, with or without\n *  modification, are permitted provided that the following conditions\n *  are met:\n *\n *   * Redistributions of source code must retain the above copyright\n *     notice, this list of conditions and the following disclaimer.\n *   * Redistributions in binary form must reproduce the above\n *     copyright notice, this list of conditions and the following\n *     disclaimer in the documentation and/or other materials provided\n *     with the distribution.\n *   * Neither the name of the institute nor the names of its\n *     contributors may be used to endorse or promote products derived\n *     from this software without specific prior written permission.\n *\n *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS\n *  \"AS IS\" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT\n *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS\n *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE\n *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,\n *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,\n *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\n *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\n *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT\n *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN\n *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n *  POSSIBILITY OF SUCH DAMAGE.\n *\n * Author: Christoph Rösmann, Otniel Rinaldo\n *********************************************************************/\n\n#include <costmap_converter/costmap_to_polygons_concave.h>\n\n#include <pluginlib/class_list_macros.h>\n\nPLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSConcaveHull, costmap_converter::BaseCostmapToPolygons)\n\nnamespace costmap_converter\n{\n    \nCostmapToPolygonsDBSConcaveHull::CostmapToPolygonsDBSConcaveHull() : CostmapToPolygonsDBSMCCH()\n{\n  dynamic_recfg_ = NULL;\n}\n\nCostmapToPolygonsDBSConcaveHull::~CostmapToPolygonsDBSConcaveHull() \n{\n  if (dynamic_recfg_ != NULL)\n    delete dynamic_recfg_;\n}\n\nvoid CostmapToPolygonsDBSConcaveHull::initialize(ros::NodeHandle nh)\n{\n    nh.param(\"cluster_max_distance\", parameter_.max_distance_, 0.4);\n    nh.param(\"cluster_min_pts\", parameter_.min_pts_, 2);\n    nh.param(\"cluster_max_pts\", parameter_.max_pts_, 30);\n    nh.param(\"convex_hull_min_pt_separation\", parameter_.min_keypoint_separation_, 0.1);\n    parameter_buffered_ = parameter_;\n    \n    nh.param(\"concave_hull_depth\", concave_hull_depth_, 2.0);\n    \n    // setup dynamic reconfigure\n    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>(nh);\n    dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSConcaveHull::reconfigureCB, this, _1, _2);\n    dynamic_recfg_->setCallback(cb);\n}\n\n\nvoid CostmapToPolygonsDBSConcaveHull::compute()\n{\n    std::vector< std::vector<KeyPoint> > clusters;\n    dbScan(clusters);\n    \n    // Create new polygon container\n    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());\n    \n    \n    // add convex hulls to polygon container\n    for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise\n    {\n      polygons->push_back( geometry_msgs::Polygon() );\n      concaveHull(clusters[i], concave_hull_depth_, polygons->back() );\n    }\n    \n    // add our non-cluster points to the polygon container (as single points)\n    if (!clusters.empty())\n    {\n      for (int i=0; i < clusters.front().size(); ++i)\n      {\n        polygons->push_back( geometry_msgs::Polygon() );\n        convertPointToPolygon(clusters.front()[i], polygons->back());\n      }\n    }\n    \n    // replace shared polygon container\n    updatePolygonContainer(polygons);\n}\n\n\nvoid CostmapToPolygonsDBSConcaveHull::concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon)\n{\n    // start with convex hull\n    convexHull2(cluster, polygon);\n\n    std::vector<geometry_msgs::Point32>& concave_list = polygon.points;\n\n    for (int i = 0; i < (int)concave_list.size() - 1; ++i)\n    {\n      \n        // find nearest inner point pk from line (vertex1 -> vertex2)\n        const geometry_msgs::Point32& vertex1 = concave_list[i];\n        const geometry_msgs::Point32& vertex2 = concave_list[i+1];\n\n        bool found;\n        size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);\n        if (!found) \n          continue;  \n        \n        double line_length = norm2d(vertex1, vertex2);\n                \n        double dst1 = norm2d(cluster[nearest_idx], vertex1);\n        double dst2 = norm2d(cluster[nearest_idx], vertex2);\n        double dd = std::min(dst1, dst2);\n        if (dd<1e-8)\n          continue;\n\n        if (line_length / dd > depth)\n        {\n            // Check that new candidate edge will not intersect existing edges.\n            bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);\n            intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);\n            if (!intersects) \n            {\n              geometry_msgs::Point32 new_point;\n              cluster[nearest_idx].toPointMsg(new_point);\n              concave_list.insert(concave_list.begin() + i + 1, new_point);\n              i--;\n            }\n        }\n    }\n}\n\n\nvoid CostmapToPolygonsDBSConcaveHull::concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon)\n{\n    // start with convex hull\n    convexHull2(cluster, polygon);\n\n    std::vector<geometry_msgs::Point32>& concave_list = polygon.points;\n    \n    // get line length\n    double mean_length = 0;\n    for (int i = 0; i < (int)concave_list.size() - 1; ++i)\n    {\n      mean_length += norm2d(concave_list[i],concave_list[i+1]);\n    }\n    mean_length /= double(concave_list.size());\n\n    for (int i = 0; i < (int)concave_list.size() - 1; ++i)\n    {\n      \n        // find nearest inner point pk from line (vertex1 -> vertex2)\n        const geometry_msgs::Point32& vertex1 = concave_list[i];\n        const geometry_msgs::Point32& vertex2 = concave_list[i+1];\n\n        double line_length = norm2d(vertex1, vertex2);\n        \n        bool found;\n        size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);\n        if (!found) \n        {\n          continue;  \n        }\n        \n                \n        double dst1 = norm2d(cluster[nearest_idx], vertex1);\n        double dst2 = norm2d(cluster[nearest_idx], vertex2);\n        double dd = std::min(dst1, dst2);\n        if (dd<1e-8)\n          continue;\n\n        if (line_length / dd > depth)\n        {\n            // Check that new candidate edge will not intersect existing edges.\n            bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);\n            intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);\n            if (!intersects) \n            {\n              geometry_msgs::Point32 new_point;\n              cluster[nearest_idx].toPointMsg(new_point);\n              concave_list.insert(concave_list.begin() + i + 1, new_point);\n              i--;\n            }\n        }\n    }\n}\n\n\n\n\nvoid CostmapToPolygonsDBSConcaveHull::reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level)\n{\n    boost::mutex::scoped_lock lock(parameter_mutex_);\n    parameter_buffered_.max_distance_ = config.cluster_max_distance;\n    parameter_buffered_.min_pts_ = config.cluster_min_pts;\n    parameter_buffered_.max_pts_ = config.cluster_max_pts;\n    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;\n    concave_hull_depth_ = config.concave_hull_depth;\n}\n\n}//end namespace costmap_converter\n\n\n"
  },
  {
    "path": "test/costmap_polygons.cpp",
    "content": "#include <random>\n#include <memory>\n#include <gtest/gtest.h>\n\n#include <costmap_converter/costmap_to_polygons.h>\n\nnamespace {\ngeometry_msgs::Point32 create_point(double x, double y)\n{\n  geometry_msgs::Point32 result;\n  result.x = x;\n  result.y = y;\n  result.z = 0.;\n  return result;\n}\n} // end namespace\n\n// make things accesible in the test\nclass CostmapToPolygons : public costmap_converter::CostmapToPolygonsDBSMCCH\n{\n  public:\n    const std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint>& points() const {return occupied_cells_;}\n    costmap_converter::CostmapToPolygonsDBSMCCH::Parameters& parameters() {return parameter_;}\n    using costmap_converter::CostmapToPolygonsDBSMCCH::addPoint;\n    using costmap_converter::CostmapToPolygonsDBSMCCH::regionQuery;\n    using costmap_converter::CostmapToPolygonsDBSMCCH::dbScan;\n    using costmap_converter::CostmapToPolygonsDBSMCCH::convexHull2;\n    using costmap_converter::CostmapToPolygonsDBSMCCH::simplifyPolygon;\n};\n\nclass CostmapToPolygonsDBSMCCHTest : public ::testing::Test\n{\n  protected:\n    void SetUp() override {\n      // parameters\n      costmap_to_polygons.parameters().max_distance_ = 0.5;\n      costmap_to_polygons.parameters().max_pts_ = 100;\n      costmap_to_polygons.parameters().min_pts_ = 2;\n      costmap_to_polygons.parameters().min_keypoint_separation_ = 0.1;\n\n      costmap.reset(new costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));\n      costmap_to_polygons.setCostmap2D(costmap.get());\n\n      std::random_device rand_dev;\n      std::mt19937 generator(rand_dev());\n      std::uniform_real_distribution<> random_angle(-M_PI, M_PI);\n      std::uniform_real_distribution<> random_dist(0.,   costmap_to_polygons.parameters().max_distance_);\n\n      costmap_to_polygons.addPoint(0., 0.);\n      costmap_to_polygons.addPoint(1.3, 1.3);\n\n      // adding random points\n      double center_x = costmap_to_polygons.points()[0].x;\n      double center_y = costmap_to_polygons.points()[0].y;\n      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_ - 1; ++i)\n      {\n        double alpha = random_angle(generator);\n        double dist  = random_dist(generator);\n        double wx = center_x + std::cos(alpha) * dist;\n        double wy = center_y + std::sin(alpha) * dist;\n        costmap_to_polygons.addPoint(wx, wy);\n      }\n\n      // some noisy points not belonging to a cluster\n      costmap_to_polygons.addPoint(-1, -1);\n      costmap_to_polygons.addPoint(-2, -2);\n      \n      // adding random points\n      center_x = costmap_to_polygons.points()[1].x;\n      center_y = costmap_to_polygons.points()[1].y;\n      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_/2; ++i)\n      {\n        double alpha = random_angle(generator);\n        double dist  = random_dist(generator);\n        double wx = center_x + std::cos(alpha) * dist;\n        double wy = center_y + std::sin(alpha) * dist;\n        costmap_to_polygons.addPoint(wx, wy);\n      }\n    }\n\n    void regionQueryTrivial(int curr_index, std::vector<int>& neighbor_indices)\n    {\n      neighbor_indices.clear();\n      const auto& query_point = costmap_to_polygons.points()[curr_index];\n      for (int i = 0; i < int(costmap_to_polygons.points().size()); ++i)\n      {\n        if (i == curr_index)\n          continue;\n        const auto& kp = costmap_to_polygons.points()[i];\n        double dx = query_point.x - kp.x;\n        double dy = query_point.y - kp.y;\n        double dist = sqrt(dx*dx + dy*dy);\n        if (dist < costmap_to_polygons.parameters().max_distance_)\n          neighbor_indices.push_back(i);\n      }\n    }\n\n    CostmapToPolygons costmap_to_polygons;\n    std::shared_ptr<costmap_2d::Costmap2D> costmap;\n};\n\nTEST_F(CostmapToPolygonsDBSMCCHTest, regionQuery)\n{\n  std::vector<int> neighbors, neighbors_trivial;\n  costmap_to_polygons.regionQuery(0, neighbors);\n  regionQueryTrivial(0, neighbors_trivial);\n  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_ - 1, int(neighbors.size()));\n  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());\n  std::sort(neighbors.begin(), neighbors.end());\n  ASSERT_EQ(neighbors_trivial, neighbors);\n\n  costmap_to_polygons.regionQuery(1, neighbors);\n  regionQueryTrivial(1, neighbors_trivial);\n  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2, int(neighbors.size()));\n  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());\n  std::sort(neighbors.begin(), neighbors.end());\n  ASSERT_EQ(neighbors_trivial, neighbors);\n}\n\nTEST_F(CostmapToPolygonsDBSMCCHTest, dbScan)\n{\n  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;\n  costmap_to_polygons.dbScan(clusters);\n  \n  ASSERT_EQ(3, clusters.size());\n  ASSERT_EQ(2, clusters[0].size()); // noisy points not belonging to a cluster\n  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_, clusters[1].size()); // first cluster at (0,0)\n  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2 + 1, clusters[2].size()); // second cluster at (1,1)\n}\n\nTEST(CostmapToPolygonsDBSMCCH, EmptyMap)\n{\n  std::shared_ptr<costmap_2d::Costmap2D> costmap =\n    std::make_shared<costmap_2d::Costmap2D>(costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));\n  CostmapToPolygons costmap_to_polygons;\n  costmap_to_polygons.setCostmap2D(costmap.get());\n\n  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;\n  costmap_to_polygons.dbScan(clusters);\n  ASSERT_EQ(1, clusters.size());    // noise cluster exists\n  ASSERT_EQ(0, clusters[0].size()); // noise clsuter is empty\n}\n\nTEST(CostmapToPolygonsDBSMCCH, SimplifyPolygon)\n{\n  const double simplification_threshold = 0.1;\n  CostmapToPolygons costmap_to_polygons;\n  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;\n  \n  geometry_msgs::Polygon polygon;\n  polygon.points.push_back(create_point(0., 0.));\n  polygon.points.push_back(create_point(1., 0.));\n\n  // degenerate case with just two points\n  geometry_msgs::Polygon original_polygon = polygon;\n  costmap_to_polygons.simplifyPolygon(polygon);\n  ASSERT_EQ(2, polygon.points.size());\n  for (size_t i = 0; i < polygon.points.size(); ++i)\n  {\n    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);\n    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  \n  }\n\n  // degenerate case with three points\n  polygon.points.push_back(create_point(1., simplification_threshold / 2.));\n  original_polygon = polygon;\n  costmap_to_polygons.simplifyPolygon(polygon);\n  ASSERT_EQ(3, polygon.points.size());\n  for (size_t i = 0; i < polygon.points.size(); ++i)\n  {\n    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);\n    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  \n  }\n\n  polygon.points.push_back(create_point(1., 1.));\n  original_polygon = polygon;\n  // remove the point that has to be removed by the simplification\n  original_polygon.points.erase(original_polygon.points.begin()+2);\n  costmap_to_polygons.simplifyPolygon(polygon);\n  ASSERT_EQ(3, polygon.points.size());\n  for (size_t i = 0; i < polygon.points.size(); ++i)\n  {\n    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);\n    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);\n  }\n}\n\nTEST(CostmapToPolygonsDBSMCCH, SimplifyPolygonPerfectLines)\n{\n  const double simplification_threshold = 0.1;\n  CostmapToPolygons costmap_to_polygons;\n  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;\n  \n  geometry_msgs::Polygon polygon;\n  for (int i = 0; i <= 100; ++i)\n    polygon.points.push_back(create_point(i*1., 0.));\n  geometry_msgs::Point32 lastPoint = polygon.points.back();\n  for (int i = 0; i <= 100; ++i)\n    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));\n  lastPoint = polygon.points.back();\n  for (int i = 0; i <= 100; ++i)\n    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));\n  lastPoint = polygon.points.back();\n  for (int i = 0; i <= 100; ++i)\n    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));\n  lastPoint = polygon.points.back();\n  for (int i = 0; i <= 100; ++i)\n    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));\n\n  costmap_to_polygons.simplifyPolygon(polygon);\n  ASSERT_EQ(6, polygon.points.size());\n  ASSERT_FLOAT_EQ(  0., polygon.points[0].x);\n  ASSERT_FLOAT_EQ(  0., polygon.points[0].y);\n  ASSERT_FLOAT_EQ(100., polygon.points[1].x);\n  ASSERT_FLOAT_EQ(  0., polygon.points[1].y);\n  ASSERT_FLOAT_EQ(100., polygon.points[2].x);\n  ASSERT_FLOAT_EQ(100., polygon.points[2].y);\n  ASSERT_FLOAT_EQ(200., polygon.points[3].x);\n  ASSERT_FLOAT_EQ(100., polygon.points[3].y);\n  ASSERT_FLOAT_EQ(200., polygon.points[4].x);\n  ASSERT_FLOAT_EQ(200., polygon.points[4].y);\n  ASSERT_FLOAT_EQ(300., polygon.points[5].x);\n  ASSERT_FLOAT_EQ(200., polygon.points[5].y);\n}\n\nint main(int argc, char** argv)\n{\n  testing::InitGoogleTest(&argc, argv);\n  ros::init(argc, argv, \"CostmapConverterTester\");\n  ros::NodeHandle nh;\n  return RUN_ALL_TESTS();\n}"
  },
  {
    "path": "test/costmap_polygons.test",
    "content": "<launch>\n  <test test-name=\"costmap_polygons\" pkg=\"costmap_converter\" type=\"test_costmap_polygons\" />\n</launch>\n"
  }
]