git commit -m "first commit"

navigations/costmap_converter/.gitlab-ci.yml

@@ -0,0 +1,23 @@
+build-kinetic:
+  variables:
+    ROSDISTRO: "kinetic"
+    CATKIN_WS: "/root/catkin_ws"
+  stage: build
+  image: osrf/ros:$ROSDISTRO-desktop-full
+  before_script: 
+    - apt-get -qq update
+    - apt-get -qq install git-core python-catkin-tools curl
+    - mkdir -p $CATKIN_WS/src
+    - cp -a . $CATKIN_WS/src/
+    - cd $CATKIN_WS
+    - rosdep update
+    - rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false
+  script: 
+    - source /ros_entrypoint.sh
+    - cd $CATKIN_WS
+    - catkin build -i -s --no-status
+  only:
+    - master
+  tags:
+    - ubuntu
+    - docker

navigations/costmap_converter/.travis.yml

@@ -0,0 +1,109 @@
+# Generic .travis.yml file for running continuous integration on Travis-CI with
+# any ROS package.
+#
+# This installs ROS on a clean Travis-CI virtual machine, creates a ROS
+# workspace, resolves all listed dependencies, and sets environment variables
+# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are
+# no compilation errors), and runs all the tests. If any of the compilation/test
+# phases fail, the build is marked as a failure.
+#
+# We handle two types of package dependencies:
+#   - packages (ros and otherwise) available through apt-get. These are installed
+#     using rosdep, based on the information in the ROS package.xml.
+#   - dependencies that must be checked out from source. These are handled by
+#     'wstool', and should be listed in a file named dependencies.rosinstall.
+#
+# There are two variables you may want to change:
+#   - ROS_DISTRO (default is indigo). Note that packages must be available for
+#     ubuntu 14.04 trusty.
+#   - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo
+#     root). This should list all necessary repositories in wstool format (see
+#     the ros wiki). If the file does not exists then nothing happens.
+#
+# See the README.md for more information.
+#
+# Author: Felix Duvallet <felixd@gmail.com>
+
+# NOTE: The build lifecycle on Travis.ci is something like this:
+#    before_install
+#    install
+#    before_script
+#    script
+#    after_success or after_failure
+#    after_script
+#    OPTIONAL before_deploy
+#    OPTIONAL deploy
+#    OPTIONAL after_deploy
+
+################################################################################
+
+# Use ubuntu trusty (14.04) with sudo privileges.
+dist: trusty
+sudo: required
+language:
+  - generic
+cache:
+  - apt
+
+# Configuration variables. All variables are global now, but this can be used to
+# trigger a build matrix for different ROS distributions if desired.
+env:
+  global:
+    - ROS_CI_DESKTOP="`lsb_release -cs`"  # e.g. [precise|trusty|...]
+    - CI_SOURCE_PATH=$(pwd)
+    - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
+    - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
+    - ROS_PARALLEL_JOBS='-j8 -l6'
+  matrix:
+    - ROS_DISTRO=indigo
+    - ROS_DISTRO=jade
+
+
+################################################################################
+
+# Install system dependencies, namely a very barebones ROS setup.
+before_install:
+  - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
+  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+  - sudo apt-get update -qq
+  - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin
+  - source /opt/ros/$ROS_DISTRO/setup.bash
+  # Prepare rosdep to install dependencies.
+  - sudo rosdep init
+  - rosdep update
+
+# Create a catkin workspace with the package under integration.
+install:
+  - mkdir -p ~/catkin_ws/src
+  - cd ~/catkin_ws/src
+  - catkin_init_workspace
+  # Create the devel/setup.bash (run catkin_make with an empty workspace) and
+  # source it to set the path variables.
+  - cd ~/catkin_ws
+  - catkin_make
+  - source devel/setup.bash
+  # Add the package under integration to the workspace using a symlink.
+  - cd ~/catkin_ws/src
+  - ln -s $CI_SOURCE_PATH .
+
+# Install all dependencies, using wstool and rosdep.
+# wstool looks for a ROSINSTALL_FILE defined in the environment variables.
+before_script:
+  # source dependencies: install using wstool.
+  - cd ~/catkin_ws/src
+  - wstool init
+  - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
+  - wstool up
+  # package depdencies: install using rosdep.
+  - cd ~/catkin_ws
+  - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
+
+# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to
+# catkin_make.
+script:
+  - cd ~/catkin_ws
+  - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS )
+  # Testing: Use both run_tests (to see the output) and test (to error out).
+  - catkin_make run_tests  # This always returns 0, but looks pretty.
+  - catkin_make test  # This will return non-zero if a test fails.
+

navigations/costmap_converter/CHANGELOG.rst

@@ -0,0 +1,95 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package costmap_converter
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.0.13 (2020-05-13)
+-------------------
+* Changed minimum CMake version to 3.1
+* Fixed wrong return type of method pointToNeighborCells
+* OpenCV 4 compatibility fix (Thanks to daviddudas)
+* Contributors: Christoph Rösmann, daviddudas
+
+0.0.12 (2019-12-02)
+-------------------
+* CostmapToPolygons: Simplification of the polygon by Douglas-Peucker algorithm (reduces the density of points in the polygon).
+* Bugfixes
+* Contributors: Rainer Kümmerle
+
+0.0.11 (2019-10-26)
+-------------------
+* rostest integration to avoid running a roscore separately for unit testing
+* Contributors: Christoph Rösmann
+
+0.0.10 (2019-10-26)
+-------------------
+* Runtime improvements for CostmapToPolygonsDBSMCCH (`#12 <https://github.com/rst-tu-dortmund/costmap_converter/issues/12>`_)
+  * Grid lookup for regionQuery
+  * use a grid structure for looking up nearest neighbors
+  * parameters in a struct
+  * guard the parameters by drawing a copy from dynamic reconfigure
+  * Adding some test cases for regionQuery and dbScan
+  * Avoid computing sqrt at the end of convexHull2
+  * Add doxygen comments for the neighbor lookup
+  * Change the param read to one liners
+  * Add test on empty map for dbScan
+* Contributors: Rainer Kümmerle
+
+0.0.9 (2018-05-28)
+------------------
+* Moved plugin loader for static costmap conversion to BaseCostmapToDynamicObstacles.
+  The corresponding ROS parameter `static_converter_plugin` is now defined in the CostmapToDynamicObstacles namespace. 
+* Contributors: Christoph Rösmann
+
+0.0.8 (2018-05-17)
+------------------
+* Standalone converter subscribes now to costmap updates. Fixes `#1 <https://github.com/rst-tu-dortmund/costmap_converter/issues/1>`_
+* Adds radius field for circular obstacles to ObstacleMsg
+* Stacked costmap conversion (`#7 <https://github.com/rst-tu-dortmund/costmap_converter/issues/7>`_).
+  E.g., it is now possible combine a dynamic obstacle and static obstacle converter plugin.
+* Contributors: Christoph Rösmann, Franz Albers
+
+0.0.7 (2017-09-20)
+------------------
+* Fixed some compilation issues (C++11 compiler flags and opencv2 on indigo/jade).
+* Dynamic obstacle plugin: obstacle velocity is now published for both x and y coordinates rather than the absolute value
+
+0.0.6 (2017-09-18)
+------------------
+* This pull request adds the costmap to dynamic obstacles plugin (written by Franz Albers).
+  It detects the dynamic foreground of the costmap (based on the temporal evolution of the costmap)
+  including blobs representing the obstacles. Furthermore, Kalman-based tracking is applied to estimate
+  the current velocity for each obstacle.
+  **Note, this plugin is still experimental.**
+* New message types are introduced: costmap\_converter::ObstacleMsg and costmap\_converter::ObstacleArrayMsg.
+  These types extend the previous polygon representation by additional velocity, orientation and id information.
+* The API has been extended to provide obstacles via the new ObstacleArrayMsg type instead of vector of polygons.
+* Contributors: Franz Albers, Christoph Rösmann
+
+0.0.5 (2016-02-01)
+------------------
+* Major changes regarding the line detection based on the convex hull
+  (it should be much more robust now).
+* Concave hull plugin added.
+* The cluster size can now be limited from above using a specific parameter.
+  This implicitly avoids large clusters forming a 'L' or 'U'.
+* All parameters can now be adjusted using dynamic_reconfigure (rqt_reconfigure).
+* Some parameter names changed.
+* Line plugin based on ransac: line inliers must now be placed inbetween the start and end of a line.
+
+0.0.4 (2016-01-11)
+------------------
+* Fixed conversion from map to world coordinates if the costmap is not quadratic.
+
+0.0.3 (2015-12-23)
+------------------
+* 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.
+* 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).
+
+0.0.2 (2015-12-22)
+------------------
+* Added a plugin for converting the costmap to lines using ransac
+
+0.0.1 (2015-12-21)
+------------------
+* First release of the package including a pluginlib interface, two plugins (costmap to polygons and costmap to lines) and a standalone conversion node.
+

navigations/costmap_converter/CMakeLists.txt

@@ -0,0 +1,244 @@
+cmake_minimum_required(VERSION 3.1)
+project(costmap_converter)
+
+# Set to Release in order to speed up the program significantly
+set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  roscpp
+  std_msgs
+  message_generation
+  costmap_2d
+  dynamic_reconfigure
+  pluginlib
+  cv_bridge
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(OpenCV REQUIRED)
+
+###set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR})
+###find_package(Eigen3 REQUIRED)
+###set(EXTERNAL_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS})
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+endif()
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+## C++11 support
+IF(NOT MSVC)
+  include(CheckCXXCompilerFlag)
+  CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+  CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+else()
+  message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support which is required 
+  by linked third party packages starting from ROS Jade. Ignore this message for ROS Indigo.")
+endif()
+endif()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  ObstacleMsg.msg
+  ObstacleArrayMsg.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   geometry_msgs std_msgs
+)
+
+#add dynamic reconfigure api
+#find_package(catkin REQUIRED dynamic_reconfigure)
+generate_dynamic_reconfigure_options(
+  cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg
+  cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg
+  cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg
+  cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg
+  cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES costmap_converter
+  CATKIN_DEPENDS 
+    geometry_msgs
+    pluginlib
+    roscpp 
+    std_msgs
+    message_runtime 
+    dynamic_reconfigure
+    costmap_2d
+  #DEPENDS 
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+add_library(costmap_converter   src/costmap_to_polygons.cpp 
+                                src/costmap_to_polygons_concave.cpp
+                                src/costmap_to_lines_convex_hull.cpp
+                                src/costmap_to_lines_ransac.cpp
+                                src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp
+                                src/costmap_to_dynamic_obstacles/background_subtractor.cpp
+                                src/costmap_to_dynamic_obstacles/blob_detector.cpp
+                                src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp
+                                src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp
+                                src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp
+)
+target_link_libraries(costmap_converter ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+# target_compile_features(costmap_converter PUBLIC cxx_nullptr cxx_range_for)
+
+# Dynamic reconfigure: make sure configure headers raare built before any node using them
+add_dependencies(costmap_converter ${PROJECT_NAME}_gencfg)
+# Generate messages before compiling the lib
+add_dependencies(costmap_converter ${PROJECT_NAME}_generate_messages_cpp)
+
+## Declare a cpp executable
+add_executable(standalone_converter src/costmap_converter_node.cpp)
+target_link_libraries(standalone_converter ${catkin_LIBRARIES} )
+add_dependencies(standalone_converter costmap_converter)
+  
+# (un)set: cmake -DCVV_DEBUG_MODE=OFF ..
+option(CVV_DEBUG_MODE "cvvisual-debug-mode" ON)
+if(CVV_DEBUG_MODE MATCHES ON)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DCVVISUAL_DEBUGMODE")
+endif()
+
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+
+## Mark executables and/or libraries for installation
+install(TARGETS costmap_converter
+   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+install(TARGETS standalone_converter
+   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   #FILES_MATCHING PATTERN "*.h"
+   PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES
+  plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY
+  cfg
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)
+
+
+#############
+## Testing ##
+#############
+
+if (CATKIN_ENABLE_TESTING)
+  add_rostest_gtest(test_costmap_polygons test/costmap_polygons.test test/costmap_polygons.cpp)
+  if(TARGET test_costmap_polygons)
+     target_link_libraries(test_costmap_polygons costmap_converter)  
+  endif()
+endif()
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_drawing.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

navigations/costmap_converter/README.md

@@ -0,0 +1,30 @@
+costmap_converter ROS Package
+=============================
+
+A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types
+
+Build status of the *master* branch:
+- 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/)
+- 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/)
+
+
+### Contributors
+
+- Christoph Rösmann
+- Franz Albers (*CostmapToDynamicObstacles* plugin)
+- Otniel Rinaldo
+
+
+### License
+
+The *costmap_converter* package is licensed under the BSD license.
+It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed.
+
+Some third-party dependencies are included that are licensed under different terms:
+ - *MultitargetTracker*, GNU GPLv3, https://github.com/Smorodov/Multitarget-tracker
+   (partially required for the *CostmapToDynamicObstacles* plugin)
+
+All 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.
+
+
+

navigations/costmap_converter/cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg

@@ -0,0 +1,136 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+##################################################################
+###################### Foreground detection ######################
+gen.add("alpha_slow", double_t, 0,
+	"Foreground detection: Learning rate of the slow filter",
+	0.3, 0.0, 1.0)
+
+gen.add("alpha_fast", double_t, 0,
+	"Foreground detection: Learning rate of the fast filter",
+	0.85, 0.0, 1.0)
+
+gen.add("beta", double_t, 0,
+	"Foreground detection: Weighting coefficient between a pixels value and the mean of its nearest neighbors",
+	0.85, 0.0, 1.0)
+
+gen.add("min_sep_between_slow_and_fast_filter", int_t, 0,
+	"Foreground detection: Minimal difference between the fast and the slow filter to recognize a obstacle as dynamic",
+	80, 0, 255)
+
+gen.add("min_occupancy_probability", int_t, 0,
+	"Foreground detection: Minimal value of the fast filter to recognize a obstacle as dynamic",
+	180, 0, 255)
+
+gen.add("max_occupancy_neighbors", int_t, 0,
+	"Foreground detection: Maximal mean value of the nearest neighbors of a pixel in the slow filter",
+	80, 0, 255)
+
+gen.add("morph_size", int_t, 0,
+	"Foreground detection: Size of the structuring element for the closing operation",
+	1, 0, 10)
+
+gen.add("publish_static_obstacles", bool_t, 0,
+        "Include static obstacles as single-point polygons",
+        True)
+
+############################################################
+###################### Blob detection ######################
+
+# These parameters are commented out, because the input image for the blob detection is already binary -> irrelevant
+#gen.add("threshold_step", double_t, 0,
+#	"Blob detection: Distance between neighboring thresholds",
+#	256.0, 0.0, 256.0)
+#
+#gen.add("min_threshold", double_t, 0,
+#	"Blob detection: Convert the source image to binary images by applying several thresholds, starting at min_threshold",
+#	1, 0, 255)
+#
+#gen.add("max_threshold", double_t, 0,
+#	"Blob detection: Convert the source image to binary images by applying several thresholds, ending at max_threshold",
+#	255, 0, 255)
+#
+#gen.add("min_repeatability", int_t, 0,
+#	"Blob detection: Minimal number of detections of a blob in the several thresholds to be considered as real blob",
+#	1, 1, 10)
+#
+gen.add("min_distance_between_blobs", double_t, 0,
+	"Blob detection: Minimal distance between centers of two blobs to be considered as seperate blobs",
+	10, 0.0, 300.0)
+
+gen.add("filter_by_area", bool_t, 0,
+	"Blob detection: Filter blobs based on number of pixels",
+	True)
+
+gen.add("min_area", int_t, 0,
+	"Blob detection: Minimal number of pixels a blob consists of",
+	3, 0, 300)
+
+gen.add("max_area", int_t, 0,
+	"Blob detection: Maximal number of pixels a blob consists of",
+	300, 0, 300)
+
+gen.add("filter_by_circularity", bool_t, 0,
+	"Blob detection: Filter blobs based on their circularity",
+	True)
+
+gen.add("min_circularity", double_t, 0,
+	"Blob detection: Minimal circularity value (0 in case of a line)",
+	0.2, 0.0, 1.0)
+
+gen.add("max_circularity", double_t, 0,
+	"Blob detection: Maximal circularity value (1 in case of a circle)",
+	1.0, 0.0, 1.0)
+
+gen.add("filter_by_inertia", bool_t, 0,
+	"Blob detection: Filter blobs based on their inertia ratio",
+	True)
+
+gen.add("min_inertia_ratio", double_t, 0,
+	"Blob detection: Minimal inertia ratio",
+	0.2, 0.0, 1.0)
+
+gen.add("max_inertia_ratio", double_t, 0,
+	"Blob detection: Maximal inertia ratio",
+	1.0, 0.0, 1.0)
+
+gen.add("filter_by_convexity", bool_t, 0,
+	"Blob detection: Filter blobs based on their convexity (Blob area / area of its convex hull)",
+	False)
+
+gen.add("min_convexity", double_t, 0,
+	"Blob detection: Minimum convexity ratio",
+	0.0, 0.0, 1.0)
+
+gen.add("max_convexity", double_t, 0,
+	"Blob detection: Maximal convexity ratio",
+	1.0, 0.0, 1.0)
+
+################################################################
+#################### Tracking ##################################
+gen.add("dt", double_t, 0,
+	"Tracking: Time for one timestep of the kalman filter",
+	0.2, 0.1, 3.0)
+
+gen.add("dist_thresh", double_t, 0,
+	"Tracking: Maximum distance between two points to be considered in the assignment problem",
+	20.0, 0.0, 150.0)
+
+gen.add("max_allowed_skipped_frames", int_t, 0,
+	"Tracking: Maximum number of frames a object is tracked while it is not seen",
+	3, 0, 10)
+
+gen.add("max_trace_length", int_t, 0,
+	"Tracking: Maximum number of Points in a objects trace",
+	10, 1, 100)
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToDynamicObstacles"))

navigations/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg

@@ -0,0 +1,41 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+
+gen.add("cluster_max_distance", double_t, 0, 
+	"Parameter for DB_Scan, maximum distance to neighbors [m]",
+	0.4, 0.0, 10.0)
+
+gen.add("cluster_min_pts", int_t, 0, 
+	"Parameter for DB_Scan: minimum number of points that define a cluster",
+	2, 1, 20)
+
+gen.add("cluster_max_pts", int_t, 0, 
+  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
+  30, 2, 200)
+
+gen.add("convex_hull_min_pt_separation",   double_t,   0,
+	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
+	0.1, 0.0, 10.0)	
+
+gen.add("support_pts_max_dist",   double_t,   0,
+	"Minimum distance from a point to the line to be counted as support point",
+	0.3, 0.0, 10.0)
+
+gen.add("support_pts_max_dist_inbetween",   double_t,   0,
+  "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.",
+  1.0, 0.0, 10.0)
+
+gen.add("min_support_pts", int_t, 0, 
+	"Minimum number of support points to represent a line",
+	2, 0, 50)	
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSMCCH"))

navigations/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+
+gen.add("cluster_max_distance", double_t, 0, 
+	"Parameter for DB_Scan, maximum distance to neighbors [m]",
+	0.4, 0.0, 10.0)
+
+gen.add("cluster_min_pts", int_t, 0, 
+	"Parameter for DB_Scan: minimum number of points that define a cluster",
+	2, 1, 20)
+
+gen.add("cluster_max_pts", int_t, 0, 
+  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
+  30, 2, 200)
+
+gen.add("ransac_inlier_distance", double_t, 0, 
+	"Maximum distance to the line segment for inliers",
+	0.2, 0.0, 10.0)
+
+gen.add("ransac_min_inliers", int_t, 0, 
+	"Minimum numer of inliers required to form a line",
+	10, 0, 100)
+
+gen.add("ransac_no_iterations", int_t, 0, 
+	"Number of ransac iterations",
+	2000, 1, 10000)
+
+gen.add("ransac_remainig_outliers", int_t, 0, 
+	"Repeat ransac until the number of outliers is as specified here",
+	3, 0, 50)
+
+gen.add("ransac_convert_outlier_pts",   bool_t,   0, 
+	"Convert remaining outliers to single points.",
+	True)
+
+gen.add("ransac_filter_remaining_outlier_pts",   bool_t,   0, 
+	"Filter the interior of remaining outliers and keep only keypoints of their convex hull",
+	False)
+
+gen.add("convex_hull_min_pt_separation",   double_t,   0,
+	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
+	0.1, 0.0, 10.0)	
+
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSRANSAC"))

navigations/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg

@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+
+gen.add("cluster_max_distance", double_t, 0, 
+	"Parameter for DB_Scan, maximum distance to neighbors [m]",
+	0.4, 0.0, 10.0)
+
+gen.add("cluster_min_pts", int_t, 0, 
+	"Parameter for DB_Scan: minimum number of points that define a cluster",
+	2, 1, 20)
+
+gen.add("cluster_max_pts", int_t, 0, 
+  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
+  30, 2, 200)
+
+gen.add("convex_hull_min_pt_separation",   double_t,   0,
+	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
+	0.1, 0.0, 10.0)	
+
+gen.add("concave_hull_depth",   double_t,   0,
+  "Smaller depth: sharper surface, depth -> high value: convex hull",
+  2.0, 0.0, 100.0) 
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSConcaveHull"))

navigations/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+
+gen.add("cluster_max_distance", double_t, 0, 
+	"Parameter for DB_Scan, maximum distance to neighbors [m]",
+	0.4, 0.0, 10.0)
+
+gen.add("cluster_min_pts", int_t, 0, 
+	"Parameter for DB_Scan: minimum number of points that define a cluster",
+	2, 1, 20)
+
+gen.add("cluster_max_pts", int_t, 0, 
+  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
+  30, 2, 200)
+
+gen.add("convex_hull_min_pt_separation",   double_t,   0,
+	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
+	0.1, 0.0, 10.0)	
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSMCCH"))

navigations/costmap_converter/include/costmap_converter/costmap_converter_interface.h

@@ -0,0 +1,368 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#ifndef COSTMAP_CONVERTER_INTERFACE_H_
+#define COSTMAP_CONVERTER_INTERFACE_H_
+
+//#include <costmap_2d/costmap_2d_ros.h>
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <boost/thread.hpp>
+#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <geometry_msgs/Polygon.h>
+#include <costmap_converter/ObstacleArrayMsg.h>
+
+
+namespace costmap_converter
+{
+//! Typedef for a shared dynamic obstacle container
+typedef boost::shared_ptr<ObstacleArrayMsg> ObstacleArrayPtr;
+//! Typedef for a shared dynamic obstacle container (read-only access)
+typedef boost::shared_ptr< const ObstacleArrayMsg > ObstacleArrayConstPtr;
+
+//! Typedef for a shared polygon container 
+typedef boost::shared_ptr< std::vector<geometry_msgs::Polygon> > PolygonContainerPtr;
+//! Typedef for a shared polygon container (read-only access)
+typedef boost::shared_ptr< const std::vector<geometry_msgs::Polygon> > PolygonContainerConstPtr;
+  
+
+/**
+ * @class BaseCostmapToPolygons
+ * @brief This abstract class defines the interface for plugins that convert the costmap into polygon types
+ * 
+ * Plugins must accept a costmap_2d::Costmap2D datatype as information source.
+ * The interface provides two different use cases:
+ * 1. Manual call to conversion routines: setCostmap2D(), compute() and getPolygons() 
+ *    (in subsequent calls setCostmap2D() can be substituted by updateCostmap2D())
+ * 2. Repeatedly process costmap with a specific rate (startWorker() and stopWorker()):
+ *    Make sure that the costmap is valid while the worker is active (you can specify your own spinner or activate a threaded spinner).
+ *    Costmaps can be obtained by invoking getPolygons().
+ */
+class BaseCostmapToPolygons
+{
+public: 
+  
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(ros::NodeHandle nh) = 0;
+    
+    /**
+     * @brief Destructor
+     */
+    virtual ~BaseCostmapToPolygons() 
+    {
+      stopWorker();
+    }
+
+    
+    /**
+     * @brief Pass a pointer to the costap to the plugin.
+     * @warning The plugin should handle the costmap's mutex locking.
+     * @sa updateCostmap2D
+     * @param costmap Pointer to the costmap2d source
+     */
+    virtual void setCostmap2D(costmap_2d::Costmap2D* costmap) = 0;
+    
+    /**
+     * @brief Get updated data from the previously set Costmap2D
+     * @warning The plugin should handle the costmap's mutex locking.
+     * @sa setCostmap2D
+     */
+    virtual void updateCostmap2D() = 0;
+    
+     /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute() = 0;
+    
+    /**
+     * @brief Get a shared instance of the current polygon container
+     *
+     * If this method is not implemented by any subclass (plugin) the returned shared
+     * pointer is empty.
+     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
+     * @warning The underlying plugin must ensure that this method is thread safe.
+     * @return Shared instance of the current polygon container
+     */
+    virtual PolygonContainerConstPtr getPolygons(){return PolygonContainerConstPtr();}
+
+  /**
+   * @brief Get a shared instance of the current obstacle container
+   * If this method is not overwritten by the underlying plugin, the obstacle container just imports getPolygons().
+   * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
+   * @warning The underlying plugin must ensure that this method is thread safe.
+   * @return Shared instance of the current obstacle container
+   * @sa getPolygons
+   */
+    virtual ObstacleArrayConstPtr getObstacles()
+    {
+      ObstacleArrayPtr obstacles = boost::make_shared<ObstacleArrayMsg>();
+      PolygonContainerConstPtr polygons = getPolygons();
+      if (polygons)
+      {
+        for (const geometry_msgs::Polygon& polygon : *polygons)
+        {
+          obstacles->obstacles.emplace_back();
+          obstacles->obstacles.back().polygon = polygon;
+        }
+      }
+      return obstacles;
+    }
+
+    /**
+     * @brief Set name of robot's odometry topic
+     *
+     * Some plugins might require odometry information
+     * to compensate the robot's ego motion
+     * @param odom_topic topic name
+     */
+    virtual void setOdomTopic(const std::string& odom_topic) {}
+
+    /**
+     * @brief Determines whether an additional plugin for subsequent costmap conversion is specified
+     *
+     * @return false, since all plugins for static costmap conversion are independent
+     */
+    virtual bool stackedCostmapConversion() {return false;}
+
+     /**
+      * @brief Instantiate a worker that repeatedly coverts the most recent costmap to polygons.
+      * The worker is implemented as a timer event that is invoked at a specific \c rate.
+      * The passed \c costmap pointer must be valid at the complete time and must be lockable.
+      * By specifying the argument \c spin_thread the timer event is invoked in a separate
+      * thread and callback queue or otherwise it is called from the global callback queue (of the
+      * node in which the plugin is used).
+      * @param rate The rate that specifies how often the costmap should be updated
+      * @param costmap Pointer to the underlying costmap (must be valid and lockable as long as the worker is active
+      * @param spin_thread if \c true,the timer is invoked in a separate thread, otherwise in the default callback queue)
+     */
+    void startWorker(ros::Rate rate, costmap_2d::Costmap2D* costmap, bool spin_thread = false)
+    {
+      setCostmap2D(costmap);
+      
+      if (spin_thread_)
+      {
+        {
+          boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
+          need_to_terminate_ = true;
+        }
+        spin_thread_->join();
+        delete spin_thread_;
+      }
+      
+      if (spin_thread)
+      {
+        ROS_DEBUG_NAMED("costmap_converter", "Spinning up a thread for the CostmapToPolygons plugin");
+        need_to_terminate_ = false;
+        spin_thread_ = new boost::thread(boost::bind(&BaseCostmapToPolygons::spinThread, this));
+        nh_.setCallbackQueue(&callback_queue_);
+      }
+      else
+      {
+        spin_thread_ = NULL;
+        nh_.setCallbackQueue(ros::getGlobalCallbackQueue());
+      }
+      
+      worker_timer_ = nh_.createTimer(rate, &BaseCostmapToPolygons::workerCallback, this);
+    }
+    
+    /**
+     * @brief Stop the worker that repeatedly converts the costmap to polygons
+     */
+    void stopWorker()
+    {
+      worker_timer_.stop();
+      if (spin_thread_)
+      {
+        {
+          boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
+          need_to_terminate_ = true;
+        }
+        spin_thread_->join();
+        delete spin_thread_;
+      }
+    }
+
+protected:
+  
+    /**
+     * @brief Protected constructor that should be called by subclasses
+     */
+    BaseCostmapToPolygons() : nh_("~costmap_to_polygons"), spin_thread_(NULL), need_to_terminate_(false) {}
+    
+    /**
+     * @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) 
+     */
+    void spinThread()
+    {
+      while (nh_.ok())
+      {
+        {
+          boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
+          if (need_to_terminate_)
+            break;
+        }
+        callback_queue_.callAvailable(ros::WallDuration(0.1f));
+      }
+    }
+    
+    /**
+     * @brief The callback of the worker that performs the actual work (updating the costmap and converting it to polygons)
+     */
+    void workerCallback(const ros::TimerEvent&)
+    {
+      updateCostmap2D();
+      compute();
+    }
+    
+private:
+  ros::Timer worker_timer_;
+  ros::NodeHandle nh_;
+  boost::thread* spin_thread_;
+  ros::CallbackQueue callback_queue_;
+  boost::mutex terminate_mutex_;
+  bool need_to_terminate_;
+};    
+
+
+/**
+ * @class BaseCostmapToDynamicPolygons
+ * @brief This class extends the BaseCostmapToPolygongs class for dynamic costmap conversion plugins in order to incorporate a subsequent costmap converter plugin for static obstacles
+ *
+ * This class should not be instantiated.
+ */
+class BaseCostmapToDynamicObstacles : public BaseCostmapToPolygons
+{
+public:
+
+  /**
+   * @brief Load underlying static costmap conversion plugin via plugin-loader
+   * @param plugin_name Exact class name of the plugin to be loaded, e.g.
+   *                    costmap_converter::CostmapToPolygonsDBSMCCH
+   * @param nh_parent   NodeHandle which is extended by the namespace of the static conversion plugin
+   */
+  void loadStaticCostmapConverterPlugin(const std::string& plugin_name, ros::NodeHandle nh_parent)
+  {
+    try
+    {
+      static_costmap_converter_ = static_converter_loader_.createInstance(plugin_name);
+
+      if(boost::dynamic_pointer_cast<BaseCostmapToDynamicObstacles>(static_costmap_converter_))
+      {
+        throw pluginlib::PluginlibException("The specified plugin for static costmap conversion is a dynamic plugin. Specify a static plugin.");
+      }
+      std::string raw_plugin_name = static_converter_loader_.getName(plugin_name);
+      static_costmap_converter_->initialize(ros::NodeHandle(nh_parent, raw_plugin_name));
+      setStaticCostmapConverterPlugin(static_costmap_converter_);
+      ROS_INFO_STREAM("CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles " << plugin_name << " loaded.");
+    }
+    catch(const pluginlib::PluginlibException& ex)
+    {
+      ROS_WARN("CostmapToDynamicObstacles: The specified costmap converter plugin cannot be loaded. Continuing without subsequent conversion of static obstacles. Error message: %s", ex.what());
+      static_costmap_converter_.reset();
+    }
+  }
+
+  /**
+   * @brief Set the underlying plugin for subsequent costmap conversion of the static background of the costmap
+   * @param static_costmap_converter shared pointer to the static costmap conversion plugin
+   */
+  void setStaticCostmapConverterPlugin(boost::shared_ptr<BaseCostmapToPolygons> static_costmap_converter)
+  {
+    static_costmap_converter_ = static_costmap_converter;
+  }
+
+  /**
+   * @brief Set the costmap for the underlying plugin
+   * @param static_costmap Costmap2D, which contains the static part of the original costmap
+   */
+  void setStaticCostmap(boost::shared_ptr<costmap_2d::Costmap2D> static_costmap)
+  {
+    static_costmap_converter_->setCostmap2D(static_costmap.get());
+  }
+
+  /**
+   * @brief Apply the underlying plugin for static costmap conversion
+   */
+  void convertStaticObstacles()
+  {
+    static_costmap_converter_->compute();
+  }
+
+  /**
+   * @brief Get the converted polygons from the underlying plugin
+   * @return Shared instance of the underlying plugins polygon container
+   */
+  PolygonContainerConstPtr getStaticPolygons()
+  {
+    return static_costmap_converter_->getPolygons();
+  }
+
+  /**
+   * @brief Determines whether an additional plugin for subsequent costmap conversion is specified
+   *
+   * @return true, if a plugin for subsequent costmap conversion is specified
+   */
+  bool stackedCostmapConversion()
+  {
+    if(static_costmap_converter_)
+      return true;
+    else
+      return false;
+  }
+
+protected:
+  /**
+   * @brief Protected constructor that should be called by subclasses
+   */
+  BaseCostmapToDynamicObstacles() : static_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), static_costmap_converter_() {}
+
+private:
+  pluginlib::ClassLoader<BaseCostmapToPolygons> static_converter_loader_;
+  boost::shared_ptr<BaseCostmapToPolygons> static_costmap_converter_;
+};
+
+
+}
+
+
+
+#endif // end COSTMAP_CONVERTER_INTERFACE_H_

navigations/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h

@@ -0,0 +1,115 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following code makes use of the OpenCV library.
+ * OpenCV is licensed under the terms of the 3-clause BSD License.
+ *
+ * Authors: Franz Albers, Christoph Rösmann
+ *********************************************************************/
+
+#ifndef BACKGROUNDSUBTRACTOR_H_
+#define BACKGROUNDSUBTRACTOR_H_
+
+#include <cv_bridge/cv_bridge.h>
+
+/**
+ * @class BackgroundSubtractor
+ * @brief Perform background subtraction to extract the "moving" foreground
+ *
+ * This class is based on OpenCV's background subtraction class cv::BackgroundSubtractor.
+ * It has been modified in order to incorporate a specialized bandpass filter.
+ *
+ * See http://docs.opencv.org/3.2.0/d7/df6/classcv_1_1BackgroundSubtractor.html for the original class.
+ */
+class BackgroundSubtractor
+{
+public:
+  struct Params
+  {
+    double alpha_slow; //!< Filter constant (learning rate) of the slow filter part
+    double alpha_fast; //!< Filter constant (learning rate) of the fast filter part
+    double beta;
+    double min_sep_between_fast_and_slow_filter;
+    double min_occupancy_probability;
+    double max_occupancy_neighbors;
+    int morph_size;
+  };
+
+  //! Constructor that accepts a specific parameter configuration
+  BackgroundSubtractor(const Params& parameters);
+
+  /**
+   * @brief Computes a foreground mask
+   * @param[in]  image    Next video frame
+   * @param[out] fg_mask  Foreground mask as an 8-bit binary image
+   * @param[in]  shift_x  Translation along the x axis between the current and previous image
+   * @param[in]  shift_y  Translation along the y axis between the current and previous image
+   */
+  void apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x = 0, int shift_y = 0);
+
+  /**
+   * @brief OpenCV Visualization
+   * @param name  Id/name of the opencv window
+   * @param image Image to be visualized
+   */
+  void visualize(const std::string& name, const cv::Mat& image);
+
+  /**
+   * @brief Export vector of matrices to yaml file
+   * @remarks This method is intended for debugging purposes
+   * @param filename   Desired filename including path and excluding file suffix
+   * @param mat_vec    Vector of cv::Mat to be exported
+   */
+  void writeMatToYAML(const std::string& filename, const std::vector<cv::Mat>& mat_vec);
+
+  //! Update internal parameters
+  void updateParameters(const Params& parameters);
+
+private:
+  //! Transform/shift all internal matrices/grids according to a given translation vector
+  void transformToCurrentFrame(int shift_x, int shift_y);
+
+  cv::Mat occupancy_grid_fast_;
+  cv::Mat occupancy_grid_slow_;
+  cv::Mat current_frame_;
+
+  int previous_shift_x_;
+  int previous_shift_y_;
+
+  Params params_;
+};
+
+#endif // BACKGROUNDSUBTRACTOR_H_

navigations/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h

@@ -0,0 +1,111 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following code makes use of the OpenCV library.
+ * OpenCV is licensed under the terms of the 3-clause BSD License.
+ *
+ * Authors: Franz Albers, Christoph Rösmann
+ *********************************************************************/
+
+#ifndef BLOBDETECTOR_H_
+#define BLOBDETECTOR_H_
+
+// Basically the OpenCV SimpleBlobDetector, extended with getContours()
+
+#include <opencv2/features2d/features2d.hpp>
+
+/**
+ * @class BlobDetector
+ * @brief Detect blobs in image (specialized for dynamic obstacles in the costmap)
+ *
+ * This class is based on OpenCV's blob detector cv::SimpleBlobDetector.
+ * It has been modified and specialized for dynamic obstacle tracking in the costmap:
+ * -> The modified version also returns contours of the blob.
+ *
+ * See http://docs.opencv.org/trunk/d0/d7a/classcv_1_1SimpleBlobDetector.html for the original class.
+ */
+class BlobDetector : public cv::SimpleBlobDetector
+{
+public:
+  //! Default constructor which optionally accepts custom parameters
+  BlobDetector(const cv::SimpleBlobDetector::Params& parameters = cv::SimpleBlobDetector::Params());
+
+  //! Create shared instance of the blob detector with given parameters
+  static cv::Ptr<BlobDetector> create(const BlobDetector::Params& params);
+
+  /**
+   * @brief Detects keypoints in an image and extracts contours
+   *
+   * In contrast to the original detect method, this extended version
+   * also extracts contours. Contours can be accessed by getContours()
+   * after invoking this method.
+   *
+   * @todo The mask option is currently not implemented.
+   *
+   * @param image     image
+   * @param keypoints The detected keypoints.
+   * @param mask      Mask specifying where to look for keypoints (optional). It must be a 8-bit integer
+   *                  matrix with non-zero values in the region of interest.
+   */
+  virtual void detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,
+                      const cv::Mat& mask = cv::Mat());
+
+  /**
+   * @brief Access contours extracted during detection stage
+   * @return Read-only reference to the contours set of the previous detect() run
+   */
+  const std::vector<std::vector<cv::Point>>& getContours() { return contours_; }
+
+  //! Update internal parameters
+  void updateParameters(const cv::SimpleBlobDetector::Params& parameters);
+
+protected:
+  struct Center
+  {
+    cv::Point2d location;
+    double radius;
+    double confidence;
+  };
+
+  virtual void findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,
+                         std::vector<std::vector<cv::Point>>& cur_contours) const;
+
+  std::vector<std::vector<cv::Point>> contours_;
+
+  Params params_;
+};
+
+#endif // BLOBDETECTOR_H_

navigations/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h

@@ -0,0 +1,212 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Notes:
+ * The following code makes use of the OpenCV library.
+ * OpenCV is licensed under the terms of the 3-clause BSD License.
+ *
+ * Authors: Franz Albers, Christoph Rösmann
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_DYNAMIC_OBSTACLES_H_
+#define COSTMAP_TO_DYNAMIC_OBSTACLES_H_
+
+// ROS
+#include <costmap_converter/costmap_converter_interface.h>
+#include <nav_msgs/Odometry.h>
+#include <pluginlib/class_loader.h>
+#include <ros/ros.h>
+
+// OpenCV
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/video/tracking.hpp>
+
+// dynamic reconfigure
+#include <costmap_converter/CostmapToDynamicObstaclesConfig.h>
+#include <dynamic_reconfigure/server.h>
+
+// Own includes
+#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>
+#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>
+#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>
+
+// STL
+#include <memory>
+
+namespace costmap_converter {
+
+/**
+ * @class CostmapToDynamicObstacles
+ * @brief This class converts the costmap_2d into dynamic obstacles.
+ *
+ * Static obstacles are treated as point obstacles.
+ */
+class CostmapToDynamicObstacles : public BaseCostmapToDynamicObstacles
+{
+public:
+  /**
+   * @brief Constructor
+   */
+  CostmapToDynamicObstacles();
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~CostmapToDynamicObstacles();
+
+  /**
+   * @brief Initialize the plugin
+   * @param nh Nodehandle that defines the namespace for parameters
+   */
+  virtual void initialize(ros::NodeHandle nh);
+
+  /**
+   * @brief This method performs the actual work (conversion of the costmap to
+   * obstacles)
+   */
+  virtual void compute();
+
+  /**
+   * @brief Pass a pointer to the costmap to the plugin.
+   * @sa updateCostmap2D
+   * @param costmap Pointer to the costmap2d source
+   */
+  virtual void setCostmap2D(costmap_2d::Costmap2D* costmap);
+
+  /**
+   * @brief Get updated data from the previously set Costmap2D
+   * @sa setCostmap2D
+   */
+  virtual void updateCostmap2D();
+
+  /**
+   * @brief Get a shared instance of the current obstacle container
+   * @remarks If compute() or startWorker() has not been called before, this
+   * method returns an empty instance!
+   * @return Shared instance of the current obstacle container
+   */
+  ObstacleArrayConstPtr getObstacles();
+
+  /**
+   * @brief Set name of robot's odometry topic
+   *
+   * @warning The method must be called before initialize()
+   *
+   * Some plugins might require odometry information
+   * to compensate the robot's ego motion
+   * @param odom_topic topic name
+   */
+  virtual void setOdomTopic(const std::string& odom_topic)
+  {
+    odom_topic_ = odom_topic;
+  }
+
+  /**
+   * @brief OpenCV Visualization
+   * @param name  Id/name of the opencv window
+   * @param image Image to be visualized
+   */
+  void visualize(const std::string& name, const cv::Mat& image);
+
+protected:
+  /**
+   * @brief Converts the estimated velocity of a tracked object to m/s and
+   * returns it
+   * @param idx Index of the tracked object in the tracker
+   * @return Point_t, which represents velocity in [m/s] of object(idx) in x,y,z
+   * coordinates
+   */
+  Point_t getEstimatedVelocityOfObject(unsigned int idx);
+
+  /**
+   * @brief Gets the last observed contour of a object and converts it from [px]
+   * to [m]
+   * @param[in]  idx Index of the tracked object in the tracker
+   * @param[out] contour vector of Point_t, which represents the last observed contour in [m]
+   *             in x,y,z coordinates
+   */
+  void getContour(unsigned int idx, std::vector<Point_t>& contour);
+
+  /**
+   * @brief Thread-safe update of the internal obstacle container (that is
+   * shared using getObstacles() from outside this
+   * class)
+   * @param obstacles Updated obstacle container
+   */
+  void updateObstacleContainer(ObstacleArrayPtr obstacles);
+
+private:
+  boost::mutex mutex_;
+  costmap_2d::Costmap2D* costmap_;
+  cv::Mat costmap_mat_;
+  ObstacleArrayPtr obstacles_;
+  cv::Mat fg_mask_;
+  std::unique_ptr<BackgroundSubtractor> bg_sub_;
+  cv::Ptr<BlobDetector> blob_det_;
+  std::vector<cv::KeyPoint> keypoints_;
+  std::unique_ptr<CTracker> tracker_;
+  ros::Subscriber odom_sub_;
+  Point_t ego_vel_;
+
+  std::string odom_topic_ = "/odom";
+  bool publish_static_obstacles_ = true;
+
+  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>*
+      dynamic_recfg_; //!< Dynamic reconfigure server to allow config
+                       //! modifications at runtime
+
+  /**
+   * @brief Callback for the odometry messages of the observing robot.
+   *
+   * Used to convert the velocity of obstacles to the /map frame.
+   * @param msg The Pointer to the nav_msgs::Odometry of the observing robot
+   */
+  void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
+
+  /**
+   * @brief Callback for the dynamic_reconfigure node.
+   *
+   * This callback allows to modify parameters dynamically at runtime without
+   * restarting the node
+   * @param config Reference to the dynamic reconfigure config
+   * @param level Dynamic reconfigure level
+   */
+  void reconfigureCB(CostmapToDynamicObstaclesConfig &config, uint32_t level);
+};
+
+} // end namespace costmap_converter
+
+#endif /* COSTMAP_TO_DYNAMIC_OBSTACLES_H_ */

navigations/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h

@@ -0,0 +1,96 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#pragma once
+#include "Kalman.h"
+#include "HungarianAlg.h"
+#include "defines.h"
+#include <iostream>
+#include <vector>
+#include <memory>
+#include <array>
+
+// --------------------------------------------------------------------------
+class CTrack
+{
+public:
+  CTrack(const Point_t& p, const std::vector<cv::Point>& contour, track_t dt, size_t trackID)
+      : track_id(trackID),
+        skipped_frames(0),
+        prediction(p),
+        lastContour(contour),
+        KF(p, dt)
+  {
+  }
+
+  track_t CalcDist(const Point_t& p)
+  {
+    Point_t diff = prediction - p;
+    return std::sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
+  }
+
+  void Update(const Point_t& p, const std::vector<cv::Point>& contour, bool dataCorrect, size_t max_trace_length)
+  {
+    KF.Prediction();
+    prediction = KF.Update(p, dataCorrect);
+
+    if (dataCorrect)
+    {
+      lastContour = contour;
+    }
+
+    if (trace.size() > max_trace_length)
+    {
+      trace.erase(trace.begin(), trace.end() - max_trace_length);
+    }
+
+    trace.push_back(prediction);
+  }
+
+  // Returns contour in [px], not in [m]!
+  std::vector<cv::Point> getLastContour() const
+  {
+    return lastContour;
+  }
+
+  // Returns velocity in [px/s], not in [m/s]!
+  Point_t getEstimatedVelocity() const
+  {
+    return KF.LastVelocity;
+  }
+
+  std::vector<Point_t> trace;
+  size_t track_id;
+  size_t skipped_frames;
+
+private:
+  Point_t prediction;
+  std::vector<cv::Point> lastContour; // Contour liegt immer auf ganzen Pixeln -> Integer Punkt
+  TKalmanFilter KF;
+};
+
+// --------------------------------------------------------------------------
+class CTracker
+{
+public:
+  struct Params{
+    track_t dt; // time for one step of the filter
+    track_t dist_thresh;// distance threshold. Pairs of points with higher distance are not considered in the assignment problem
+    int max_allowed_skipped_frames; // Maximum number of frames the track is maintained without seeing the object in the measurement data
+    int max_trace_length; // Maximum trace length
+  };
+
+  CTracker(const Params& parameters);
+  ~CTracker(void);
+
+  std::vector<std::unique_ptr<CTrack>> tracks;
+  void Update(const std::vector<Point_t>& detectedCentroid, const std::vector<std::vector<cv::Point> > &contour);
+
+  void updateParameters(const Params &parameters);
+
+private:
+  // Aggregated parameters for the tracker object
+  Params params;
+  // ID for the upcoming CTrack object
+  size_t NextTrackID;
+};

navigations/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h

@@ -0,0 +1,60 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#include <vector>
+#include <iostream>
+#include <limits>
+#include <time.h>
+#include "defines.h"
+
+// http://community.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm
+
+typedef std::vector<int> assignments_t;
+typedef std::vector<track_t> distMatrix_t;
+
+class AssignmentProblemSolver
+{
+private:
+  // --------------------------------------------------------------------------
+  // Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
+  // --------------------------------------------------------------------------
+  void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
+                         size_t nOfColumns);
+  void buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows, size_t nOfColumns);
+  void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn,
+                             size_t nOfRows);
+  void step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+              bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+  void step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+              bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+  void step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+  void step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row,
+             size_t col);
+  void step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+  // --------------------------------------------------------------------------
+  // Computes a suboptimal solution. Good for cases with many forbidden assignments.
+  // --------------------------------------------------------------------------
+  void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
+                             size_t nOfColumns);
+  // --------------------------------------------------------------------------
+  // Computes a suboptimal solution. Good for cases with many forbidden assignments.
+  // --------------------------------------------------------------------------
+  void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
+                             size_t nOfColumns);
+
+public:
+  enum TMethod
+  {
+    optimal,
+    many_forbidden_assignments,
+    without_forbidden_assignments
+  };
+
+  AssignmentProblemSolver();
+  ~AssignmentProblemSolver();
+  track_t Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns, assignments_t& assignment,
+                TMethod Method = optimal);
+};

navigations/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h

@@ -0,0 +1,20 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#pragma once
+#include "defines.h"
+#include <opencv2/opencv.hpp>
+
+// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/
+class TKalmanFilter
+{
+public:
+  TKalmanFilter(Point_t p, track_t deltatime = 0.2);
+  ~TKalmanFilter();
+  void Prediction();
+  Point_t Update(Point_t p, bool DataCorrect);
+  cv::KalmanFilter* kalman;
+  track_t dt;
+  Point_t LastPosition; // contour in [px]
+  Point_t LastVelocity; // velocity in [px/s]
+};

navigations/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md

@@ -0,0 +1,13 @@
+Multitarget Tracker
+===================
+
+This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].
+It is utilized for the *CostmapToDynamicObstacles* plugin.
+
+The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].
+The package itself depends on other third party packages with different licenses (refer to the package repository).
+
+TODO: Include the whole package as external CMake project.
+
+
+

navigations/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h

@@ -0,0 +1,9 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#pragma once
+#include <opencv2/opencv.hpp>
+
+typedef float track_t;
+typedef cv::Point3_<track_t> Point_t;
+#define Mat_t CV_32FC

navigations/costmap_converter/include/costmap_converter/costmap_to_lines_convex_hull.h

@@ -0,0 +1,137 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_LINES_CONVEX_HULL_H_
+#define COSTMAP_TO_LINES_CONVEX_HULL_H_
+
+#include <costmap_converter/costmap_converter_interface.h>
+#include <costmap_converter/costmap_to_polygons.h>
+
+// dynamic reconfigure
+#include <costmap_converter/CostmapToLinesDBSMCCHConfig.h>
+
+namespace costmap_converter
+{
+  
+/**
+ * @class CostmapToLinesDBSMCCH
+ * @brief This class converts the costmap_2d into a set of lines (and points)
+ * 
+ * The conversion is performed in three stages:
+ * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
+ *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
+ *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
+ *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
+ *    
+ * 2. In the subsequent stage, the convex hull of each cluster is determined using the monotone chain algorithm aka Andrew's algorithm:
+ *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
+ *    Reference:  A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
+ * 
+ * 3. In the third step extract lines from each polygon (convex hull) if there exist at least a predefined number of support points.
+ * 
+ * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)
+ */
+  class CostmapToLinesDBSMCCH : public CostmapToPolygonsDBSMCCH
+  {
+  public:
+   
+    /**
+     * @brief Constructor
+     */
+    CostmapToLinesDBSMCCH();
+       
+    /**
+     * @brief Destructor
+     */
+    virtual ~CostmapToLinesDBSMCCH();
+    
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(ros::NodeHandle nh);
+    
+    /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute();   
+    
+    
+  protected:
+    
+    /**
+     * @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
+     * @param cluster list of points in the cluster
+     * @param polygon convex hull of the cluster \c cluster
+     * @param[out] lines back_inserter object to a sequence of polygon msgs (new lines will be pushed back)
+     */
+    void extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::Polygon& polygon, std::back_insert_iterator< std::vector<geometry_msgs::Polygon> > lines);
+
+    
+    
+  protected:
+      
+    double support_pts_max_dist_inbetween_;
+    double support_pts_max_dist_;
+    int min_support_pts_;
+   
+  private:
+    
+    /**
+     * @brief Callback for the dynamic_reconfigure node.
+     * 
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+    void reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level);
+    
+    dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime   
+ 
+  };  
+  
+  
+  
+  
+
+
+  
+  
+} //end namespace teb_local_planner
+
+#endif /* COSTMAP_TO_LINES_CONVEX_HULL_H_ */

navigations/costmap_converter/include/costmap_converter/costmap_to_lines_ransac.h

@@ -0,0 +1,187 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_LINES_RANSAC_H_
+#define COSTMAP_TO_LINES_RANSAC_H_
+
+#include <costmap_converter/costmap_converter_interface.h>
+#include <costmap_converter/costmap_to_polygons.h>
+#include <costmap_converter/misc.h>
+#include <boost/random.hpp>
+
+#include <costmap_converter/CostmapToLinesDBSRANSACConfig.h>
+
+namespace costmap_converter
+{
+  
+/**
+ * @class CostmapToLinesDBSRANSAC
+ * @brief This class converts the costmap_2d into a set of lines (and points)
+ * 
+ * The conversion is performed in two stages:
+ * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
+ *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
+ *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
+ *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
+ *    
+ * 2. The RANSAC algorithm is used to find straight line segment models (https://en.wikipedia.org/wiki/RANSAC)
+ *    RANSAC is called repeatedly to find multiple lines per cluster until the number of inliners is below a specific threshold.
+ *    In that case the remaining outliers are used as points or keypoints of their convex hull are used as points (depending on a paramter).
+ *    The latter one applies as a filter. The convex assumption is not strict in practice, since the remaining regions/cluster (line inliers are removed)
+ *    should be dense and small. For details about the convex hull algorithm, refer to costmap_converter::CostmapToPolygonsDBSMCCH.
+ *    Resulting lines of RANSAC are currently not refined by linear regression.
+ * 
+ * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)
+ */
+  class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH
+  {
+  public:
+   
+    /**
+     * @brief Constructor
+     */
+    CostmapToLinesDBSRANSAC();
+       
+    /**
+     * @brief Destructor
+     */
+    virtual ~CostmapToLinesDBSRANSAC();
+    
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(ros::NodeHandle nh);
+    
+    /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute();   
+        
+     
+    /**
+     * @brief Check if the candidate point is an inlier.
+     * @param point generic 2D point type defining the reference point
+     * @param line_start generic 2D point type defining the start of the line
+     * @param line_end generic 2D point type defining the end of the line
+     * @param min_distance minimum distance allowed
+     * @tparam Point generic point type that should provide (writable) x and y member fiels.
+     * @tparam LinePoint generic point type that should provide (writable) x and y member fiels.
+     * @return \c true if inlier, \c false otherwise
+     */
+    template <typename Point, typename LinePoint>
+    static bool isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance);
+    
+  protected:
+    
+    double ransac_inlier_distance_; //!< Maximum distance to the line segment for inliers
+    int ransac_min_inliers_; //!< Minimum numer of inliers required to form a line
+    int ransac_no_iterations_; //!< Number of ransac iterations
+    int ransac_remainig_outliers_; //!< Repeat ransac until the number of outliers is as specified here
+    bool ransac_convert_outlier_pts_; //!< If \c true, convert remaining outliers to single points.
+    bool ransac_filter_remaining_outlier_pts_; //!< If \c true, filter the interior of remaining outliers and keep only keypoints of their convex hull
+   
+   
+   boost::random::mt19937 rnd_generator_; //!< Random number generator for ransac with default seed
+    
+    
+    /**
+     * @brief Find a straight line segment in a point cloud with RANSAC (without linear regression).
+     * @param data set of 2D data points
+     * @param inlier_distance maximum distance that inliers must satisfy
+     * @param no_iterations number of RANSAC iterations
+     * @param min_inliers minimum number of inliers to return true
+     * @param[out] best_model start and end of the best straight line segment 
+     * @param[out] inliers inlier keypoints are written to this container [optional]
+     * @param[out] outliers outlier keypoints are written to this container [optional]
+     * @return \c false, if \c min_inliers is not satisfied, \c true otherwise
+     */
+    bool lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations, int min_inliers,
+                    std::pair<KeyPoint, KeyPoint>& best_model, std::vector<KeyPoint>* inliers = NULL,
+                     std::vector<KeyPoint>* outliers = NULL);
+    
+    /**
+     * @brief Perform a simple linear regression in order to fit a straight line 'y = slope*x + intercept'
+     * @param data set of 2D data points
+     * @param[out] slope The slope of the fitted line 
+     * @param[out] intercept The intercept / offset of the line
+     * @param[out] mean_x_out mean of the x-values of the data [optional]
+     * @param[out] mean_y_out mean of the y-values of the data [optional]
+     * @return \c true, if a valid line has been fitted, \c false otherwise.
+     */
+    bool linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept,
+                          double* mean_x_out = NULL, double* mean_y_out = NULL);
+    
+    
+    
+//     void adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2, 
+//                           KeyPoint& line_start, KeyPoint& line_end);
+    
+    private:
+    
+    /**
+     * @brief Callback for the dynamic_reconfigure node.
+     * 
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+    void reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level);
+    
+    dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime   
+    
+  };  
+  
+  
+
+template <typename Point, typename LinePoint>
+bool CostmapToLinesDBSRANSAC::isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance)
+{
+  bool is_inbetween = false;
+  double distance = computeDistanceToLineSegment(point, line_start, line_end, &is_inbetween);
+  if (!is_inbetween)
+    return false;
+  if (distance <= min_distance)
+    return true;
+  return false;
+}
+     
+  
+} //end namespace teb_local_planner
+
+#endif /* COSTMAP_TO_LINES_RANSAC_H_ */

navigations/costmap_converter/include/costmap_converter/costmap_to_polygons.h

@@ -0,0 +1,336 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_POLYGONS_H_
+#define COSTMAP_TO_POLYGONS_H_
+
+#include <ros/ros.h>
+#include <costmap_converter/costmap_converter_interface.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <visualization_msgs/Marker.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Polygon.h>
+#include <vector>
+#include <algorithm>
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/thread.hpp>
+
+// dynamic reconfigure
+#include <costmap_converter/CostmapToPolygonsDBSMCCHConfig.h>
+#include <dynamic_reconfigure/server.h>
+
+
+namespace costmap_converter
+{
+  
+/**
+ * @class CostmapToPolygonsDBSMCCH
+ * @brief This class converts the costmap_2d into a set of convex polygons (and points)
+ * 
+ * The conversion is performed in two stages:
+ * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
+ *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
+ *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
+ *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
+ *    
+ * 2. In the subsequent stage, clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:
+ *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
+ *    Reference:  A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
+ *  
+ * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)
+ */
+class CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons
+{
+  public:
+    
+    /**
+     * @struct KeyPoint
+     * @brief Defines a keypoint in metric coordinates of the map 
+     */
+    struct KeyPoint
+    {
+      //! Default constructor
+      KeyPoint() {}
+      //! Constructor with point initialization
+      KeyPoint(double x_, double y_) : x(x_), y(y_) {}
+      double x; //!< x coordinate [m]
+      double y; //!< y coordinate [m]
+      
+      //! Convert keypoint to geometry_msgs::Point message type
+      void toPointMsg(geometry_msgs::Point& point) const {point.x=x; point.y=y; point.z=0;}
+      //! Convert keypoint to geometry_msgs::Point32 message type
+      void toPointMsg(geometry_msgs::Point32& point) const {point.x=x; point.y=y; point.z=0;}
+    };
+
+    /**
+     * @struct Parameters
+     * @brief Defines the parameters of the algorithm
+     */
+    struct Parameters
+    {
+      Parameters() : max_distance_(0.4), min_pts_(2), max_pts_(30), min_keypoint_separation_(0.1) {}
+      // DBSCAN parameters
+      double max_distance_; //!< Parameter for DB_Scan, maximum distance to neighbors [m]
+      int min_pts_; //!< Parameter for DB_Scan: minimum number of points that define a cluster
+      int max_pts_; //!< Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes)
+      
+      // convex hull parameters
+      double min_keypoint_separation_; //!< Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)
+    };
+    
+    /**
+     * @brief Constructor
+     */
+    CostmapToPolygonsDBSMCCH();
+    
+    /**
+     * @brief Destructor
+     */
+    virtual ~CostmapToPolygonsDBSMCCH();
+    
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(ros::NodeHandle nh);
+    
+    /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute();
+        
+    /**
+     * @brief Pass a pointer to the costap to the plugin.
+     * @sa updateCostmap2D
+     * @param costmap Pointer to the costmap2d source
+     */
+    virtual void setCostmap2D(costmap_2d::Costmap2D* costmap);
+    
+    /**
+     * @brief Get updated data from the previously set Costmap2D
+     * @sa setCostmap2D
+     */
+    virtual void updateCostmap2D();
+    
+    
+    /**
+     * @brief Convert a generi point type to a geometry_msgs::Polygon
+     * @param point generic 2D point type
+     * @param[out] polygon already instantiated polygon that will be filled with a single point
+     * @tparam Point generic point type that should provide (writable) x and y member fiels.
+     */
+    template< typename Point>
+    static void convertPointToPolygon(const Point& point, geometry_msgs::Polygon& polygon)
+    {
+      polygon.points.resize(1);
+      polygon.points.front().x = point.x;
+      polygon.points.front().y = point.y;
+      polygon.points.front().z = 0;
+    }
+    
+    /**
+     * @brief Get a shared instance of the current polygon container
+     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
+     * @return Shared instance of the current polygon container
+     */
+    PolygonContainerConstPtr getPolygons();
+
+    
+    
+  protected:
+    
+    /**
+     * @brief DBSCAN algorithm for clustering 
+     * 
+     * Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
+     *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
+     *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
+     *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
+     * 
+     * @param[out] clusters clusters will added to this output-argument (a sequence of keypoints for each cluster)
+     *                      the first cluster (clusters[0]) will contain all noise points (that does not fulfil the min_pts condition 
+     */
+    void dbScan(std::vector< std::vector<KeyPoint> >& clusters);
+    
+    /**
+     * @brief Helper function for dbScan to search for neighboring points 
+     * 
+     * @param curr_index index to the current item in \c occupied_cells
+     * @param[out] neighbor_indices list of neighbors (indices of \c occupied cells)
+     */
+    void regionQuery(int curr_index, std::vector<int>& neighbor_indices);
+
+    /**
+     * @brief helper function for adding a point to the lookup data structures
+     */
+    void addPoint(double x, double y)
+    {
+      int idx = occupied_cells_.size();
+      occupied_cells_.emplace_back(x, y);
+      int cx, cy;
+      pointToNeighborCells(occupied_cells_.back(), cx, cy);
+      int nidx = neighborCellsToIndex(cx, cy);
+      if (nidx >= 0)
+        neighbor_lookup_[nidx].push_back(idx);
+    }
+    
+    /**
+     * @brief Compute the convex hull for a single cluster (monotone chain algorithm)
+     * 
+     * Clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:
+     *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
+     *    Reference:  A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
+     * @remarks The algorithm seems to cut edges, thus we give a try to convexHull2().
+     * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)
+     * @remarks The last point is the same as the first one
+     * @param cluster list of keypoints that should be converted into a polygon
+     * @param[out] polygon the resulting convex polygon
+     */  
+    void convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon);
+    
+    /**
+     * @brief Compute the convex hull for a single cluster
+     * 
+     * Clusters are converted into convex polgons using an algorithm provided here:
+     *  https://bitbucket.org/vostreltsov/concave-hull/overview
+     * The convex hull algorithm is part of the concave hull algorithm.
+     * The license is WTFPL 2.0 and permits any usage.
+     * 
+     * @remarks The last point is the same as the first one
+     * @param cluster list of keypoints that should be converted into a polygon
+     * @param[out] polygon the resulting convex polygon
+     * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)
+     */  
+    void convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon);
+
+    /**
+     * @brief Simplify a polygon by removing points.
+     * 
+     * We apply the Douglas-Peucker Algorithm to simplify the edges of the polygon.
+     * Internally, the parameter min_keypoint_separation is used for deciding whether
+     * a point is considered close to an edge and to be merged into the line.
+     */
+    void simplifyPolygon(geometry_msgs::Polygon& polygon);
+    
+   /**
+    * @brief 2D Cross product of two vectors defined by two points and a common origin
+    * @param O Origin
+    * @param A First point
+    * @param B Second point
+    * @tparam P1 2D Point type with x and y member fields
+    * @tparam P2 2D Point type with x and y member fields
+    * @tparam P3 2D Point type with x and y member fields
+    */    
+    template <typename P1, typename P2, typename P3>
+    long double cross(const P1& O, const P2& A, const P3& B)
+    { 
+        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);
+    }
+    
+      
+   /**
+    * @brief Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class)
+    * @param polygons Updated polygon container
+    */
+   void updatePolygonContainer(PolygonContainerPtr polygons);   
+          
+   
+   std::vector<KeyPoint> occupied_cells_; //!< List of occupied cells in the current map (updated by updateCostmap2D())
+   
+    std::vector<std::vector<int> > neighbor_lookup_; //! array of cells for neighbor lookup
+    int neighbor_size_x_; //! size of the neighbour lookup in x (number of cells)
+    int neighbor_size_y_; //! size of the neighbour lookup in y (number of cells)
+    double offset_x_;     //! offset [meters] in x for the lookup grid
+    double offset_y_;     //! offset [meters] in y for the lookup grid
+
+    /**
+      * @brief convert a 2d cell coordinate into the 1D index of the array
+      * @param cx the x index of the cell
+      * @param cy the y index of the cell
+      */
+    int neighborCellsToIndex(int cx, int cy)
+    {
+      if (cx < 0 || cx >= neighbor_size_x_ || cy < 0 || cy >= neighbor_size_y_)
+        return -1;
+      return cy * neighbor_size_x_ + cx;
+    }
+
+    /**
+      * @brief compute the cell indices of a keypoint
+      * @param kp key point given in world coordinates [m, m]
+      * @param cx output cell index in x direction
+      * @param cy output cell index in y direction
+      */
+    void pointToNeighborCells(const KeyPoint& kp, int& cx, int& cy)
+    {
+      cx = int((kp.x - offset_x_) / parameter_.max_distance_);
+      cy = int((kp.y - offset_y_) / parameter_.max_distance_);
+    }
+
+
+    Parameters parameter_;          //< active parameters throughout computation
+    Parameters parameter_buffered_; //< the buffered parameters that are offered to dynamic reconfigure
+    boost::mutex parameter_mutex_;  //!< Mutex that keeps track about the ownership of the shared polygon instance
+   
+  private:
+       
+    /**
+     * @brief Callback for the dynamic_reconfigure node.
+     * 
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+    void reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level);
+    
+    
+    PolygonContainerPtr polygons_; //!< Current shared container of polygons
+    boost::mutex mutex_; //!< Mutex that keeps track about the ownership of the shared polygon instance
+    
+    dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime    
+   
+    costmap_2d::Costmap2D *costmap_; //!< Pointer to the costmap2d
+   
+}; 
+
+  
+} //end namespace teb_local_planner
+
+#endif /* COSTMAP_TO_POLYGONS_H_ */

navigations/costmap_converter/include/costmap_converter/costmap_to_polygons_concave.h

@@ -0,0 +1,202 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_POLYGONS_CONCAVE_H_
+#define COSTMAP_TO_POLYGONS_CONCAVE_H_
+
+#include <costmap_converter/costmap_to_polygons.h>
+#include <costmap_converter/misc.h>
+
+// dynamic reconfigure
+#include <costmap_converter/CostmapToPolygonsDBSConcaveHullConfig.h>
+#include <dynamic_reconfigure/server.h>
+
+
+namespace costmap_converter
+{
+  
+/**
+ * @class CostmapToPolygonsDBSConcaveHull
+ * @brief This class converts the costmap_2d into a set of non-convex polygons (and points)
+ *  
+ * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)
+ * @todo change the class hierarchy to a clearer and more generic one
+ */
+class CostmapToPolygonsDBSConcaveHull : public CostmapToPolygonsDBSMCCH
+{
+  public:
+    
+    
+    
+    /**
+     * @brief Constructor
+     */
+    CostmapToPolygonsDBSConcaveHull();
+    
+    /**
+     * @brief Destructor
+     */
+    virtual ~CostmapToPolygonsDBSConcaveHull();
+    
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(ros::NodeHandle nh);
+   
+    
+    /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute();
+    
+  protected:
+ 
+   
+    /**
+     * @brief Compute the concave hull for a single cluster
+     * 
+     * @remarks The last point is the same as the first one
+     * @param cluster list of keypoints that should be converted into a polygon
+     * @param depth Smaller depth: sharper surface, depth -> high value: convex hull
+     * @param[out] polygon the resulting convex polygon
+     */  
+    void concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon);
+        
+    void concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon);
+    
+    template <typename PointLine, typename PointCluster, typename PointHull>
+    std::size_t findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found);
+        
+    
+    template <typename Point1, typename Point2, typename Point3, typename Point4>
+    bool checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end);
+    
+    template <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>
+    bool checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start, 
+                               const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end);
+    
+    double concave_hull_depth_;
+
+  private:
+       
+    /**
+     * @brief Callback for the dynamic_reconfigure node.
+     * 
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+    void reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level);
+    
+    dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime    
+   
+   
+}; 
+
+
+template <typename PointLine, typename PointCluster, typename PointHull>
+std::size_t CostmapToPolygonsDBSConcaveHull::findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found)
+{
+    std::size_t nearsest_idx = 0;
+    double distance = 0;
+    *found = false;
+
+    for (std::size_t i = 0; i < cluster.size(); ++i)
+    {
+        // Skip points that are already in the hull
+        if (std::find_if( hull.begin(), hull.end(), boost::bind(isApprox2d<PointHull, PointCluster>, _1, boost::cref(cluster[i]), 1e-5) ) != hull.end() )
+            continue;
+
+        double dist = computeDistanceToLineSegment(cluster[i], line_start, line_end);
+        bool skip = false;
+        for (int j = 0; !skip && j < (int)hull.size() - 1; ++j) 
+        {
+            double dist_temp = computeDistanceToLineSegment(cluster[i], hull[j], hull[j + 1]);
+            skip |= dist_temp < dist;
+        }
+        if (skip) 
+            continue;
+
+        if (!(*found) || dist < distance) 
+        {
+            nearsest_idx = i;
+            distance = dist;
+            *found = true;
+        }
+    }
+    return nearsest_idx;
+}
+
+
+template <typename Point1, typename Point2, typename Point3, typename Point4>
+bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end)
+{
+    double dx1 = line1_end.x - line1_start.x;
+    double dy1 = line1_end.y - line1_start.y;
+    double dx2 = line2_end.x - line2_start.x;
+    double dy2 = line2_end.y - line2_start.y;
+    double s = (-dy1 * (line1_start.x - line2_start.x) + dx1 * (line1_start.y - line2_start.y)) / (-dx2 * dy1 + dx1 * dy2);
+    double t = ( dx2 * (line1_start.y - line2_start.y) - dy2 * (line1_start.x - line2_start.x)) / (-dx2 * dy1 + dx1 * dy2);
+    return (s > 0 && s < 1 && t > 0 && t < 1);
+}
+
+
+template <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>
+bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start, 
+                                                            const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end)
+{
+    for (int i = 0; i < (int)hull.size() - 2; i++) 
+    {
+        if (isApprox2d(current_line_start, hull[i], 1e-5) && isApprox2d(current_line_end, hull[i+1], 1e-5))
+        {
+            continue;
+        }
+
+        if (checkLineIntersection(test_line_start, test_line_end, hull[i], hull[i+1])) 
+        {
+            return true;
+        }
+    }
+    return false;
+} 
+  
+  
+} //end namespace teb_local_planner
+
+#endif /* COSTMAP_TO_POLYGONS_CONCAVE_H_ */

navigations/costmap_converter/include/costmap_converter/misc.h

@@ -0,0 +1,157 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef MISC_H_
+#define MISC_H_
+
+#include <algorithm>
+#include <cmath>
+
+namespace costmap_converter
+{
+
+/**
+  * @brief Calculate the distance between a point and a straight line (with infinite length)
+  * @param point generic 2D point type defining the reference point
+  * @param line_pt1 generic 2D point as part of the line
+  * @param line_pt2 generic 2D point as part of the line
+  * @tparam Point generic point type that should provide x and y member fiels.
+  * @tparam LinePoint generic point type that should provide x and y member fiels.
+  * @return (minimum) euclidean distance to the line segment
+  */
+template <typename Point, typename LinePoint>
+inline double computeDistanceToLine(const Point& point, const LinePoint& line_pt1, const LinePoint& line_pt2)
+{
+    double dx = line_pt2.x - line_pt1.x;
+    double dy = line_pt2.y - line_pt1.y;
+    
+    double length = std::sqrt(dx*dx + dy*dy);
+    
+    if (length>0)
+      return std::abs(dy * point.x - dx * point.y + line_pt2.x * line_pt1.y - line_pt2.y * line_pt1.x) / length;
+  
+    return std::sqrt(std::pow(point.x - line_pt1.x, 2) + std::pow(point.y - line_pt1.y, 2));  
+}
+
+
+/**
+  * @brief Calculate the squared distance between a point and a straight line segment
+  * @param point generic 2D point type defining the reference point
+  * @param line_start generic 2D point type defining the start of the line
+  * @param line_end generic 2D point type defining the end of the line
+  * @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional]
+  * @tparam Point generic point type that should provide x and y member fiels.
+  * @tparam LinePoint generic point type that should provide x and y member fiels.
+  * @return (minimum) squared euclidean distance to the line segment
+  */
+template <typename Point, typename LinePoint>
+inline double computeSquaredDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
+{
+    double dx = line_end.x - line_start.x;
+    double dy = line_end.y - line_start.y;
+    
+    double length_sqr = dx*dx + dy*dy;
+
+    double u = 0;
+
+    if (length_sqr > 0)
+      u = ((point.x - line_start.x) * dx + (point.y - line_start.y) * dy) / length_sqr;
+
+    if (is_inbetween)
+      *is_inbetween = (u>=0 && u<=1);
+  
+    if (u <= 0)
+      return std::pow(point.x-line_start.x,2) + std::pow(point.y-line_start.y,2);
+    
+    if (u >= 1)
+      return std::pow(point.x-line_end.x,2) + std::pow(point.y-line_end.y,2);
+    
+    return std::pow(point.x - (line_start.x+u*dx) ,2) + std::pow(point.y - (line_start.y+u*dy),2);
+}
+  
+/**
+  * @brief Calculate the distance between a point and a straight line segment
+  * @param point generic 2D point type defining the reference point
+  * @param line_start generic 2D point type defining the start of the line
+  * @param line_end generic 2D point type defining the end of the line
+  * @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional]
+  * @tparam Point generic point type that should provide x and y member fiels.
+  * @tparam LinePoint generic point type that should provide x and y member fiels.
+  * @return (minimum) euclidean distance to the line segment
+  */
+template <typename Point, typename LinePoint>
+inline double computeDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
+{
+  return std::sqrt(computeSquaredDistanceToLineSegment(point, line_start, line_end, is_inbetween));
+}
+  
+
+/**
+  * @brief Calculate the distance between two 2d points
+  * @param pt1 generic 2D point
+  * @param pt2 generic 2D point
+  * @tparam Point1 generic point type that should provide x and y member fiels.
+  * @tparam Point2 generic point type that should provide x and y member fiels.
+  * @return euclidean distance to the line segment
+  */
+template <typename Point1, typename Point2>  
+inline double norm2d(const Point1& pt1, const Point2& pt2)
+{
+  return std::sqrt( std::pow(pt2.x - pt1.x, 2) + std::pow(pt2.y - pt1.y, 2)  );
+}
+
+/**
+  * @brief Check if two points are approximately defining the same one
+  * @param pt1 generic 2D point
+  * @param pt2 generic 2D point
+  * @param threshold define the minimum threshold |pt1.x-pt2.y| < tresh && |pt1.y-pt2.y| < tresh
+  * @tparam Point1 generic point type that should provide x and y member fiels.
+  * @tparam Point2 generic point type that should provide x and y member fiels.
+  * @return \c true, if similar, \c false otherwise
+  */
+template <typename Point1, typename Point2>  
+inline bool isApprox2d(const Point1& pt1, const Point2& pt2, double threshold)
+{
+  return ( std::abs(pt2.x-pt1.x)<threshold && std::abs(pt2.y-pt1.y)<threshold );
+}
+
+
+  
+} //end namespace teb_local_planner
+
+#endif /* MISC_H_ */

navigations/costmap_converter/msg/ObstacleArrayMsg.msg

@@ -0,0 +1,10 @@
+# Message that contains a list of polygon shaped obstacles.
+# Special types:
+# Polygon with 1 vertex: Point obstacle
+# Polygon with 2 vertices: Line obstacle
+# Polygon with more than 2 vertices: First and last points are assumed to be connected
+
+std_msgs/Header header
+
+costmap_converter/ObstacleMsg[] obstacles
+

navigations/costmap_converter/msg/ObstacleMsg.msg

@@ -0,0 +1,24 @@
+# Special types:
+# Polygon with 1 vertex: Point obstacle (you might also specify a non-zero value for radius)
+# Polygon with 2 vertices: Line obstacle
+# Polygon with more than 2 vertices: First and last points are assumed to be connected
+
+std_msgs/Header header
+
+# Obstacle footprint (polygon descriptions)
+geometry_msgs/Polygon polygon
+
+# Specify the radius for circular/point obstacles
+float64 radius
+
+# Obstacle ID
+# Specify IDs in order to provide (temporal) relationships
+# between obstacles among multiple messages.
+int64 id
+
+# Individual orientation (centroid)
+geometry_msgs/Quaternion orientation
+
+# Individual velocities (centroid)
+geometry_msgs/TwistWithCovariance velocities
+

navigations/costmap_converter/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>costmap_converter</name>
+  <version>0.0.13</version>
+  <description>
+    A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types.
+  </description>
+
+  <maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
+
+  <author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
+  <author email="franz.albers@tu-dortmund.de">Franz Albers</author>
+  <author email="otniel.rinaldo@tu-dortmund.de">Otniel Rinaldo</author> 
+
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/costmap_converter</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+
+  <build_export_depend>message_runtime</build_export_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <test_depend>rostest</test_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>costmap_2d</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>pluginlib</depend>
+  <depend>cv_bridge</depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <costmap_converter plugin="${prefix}/plugins.xml" />
+  </export>
+</package>

navigations/costmap_converter/plugins.xml

@@ -0,0 +1,38 @@
+<library path="lib/libcostmap_converter">
+  
+  <class type="costmap_converter::CostmapToPolygonsDBSMCCH" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class converts costmap2d obstacles into a vector of convex polygons. 
+      Clustering is performed with DBScan. Convex polygons are detected using the Monotone Chain Convex Hull algorithm.
+  </description>
+  </class>
+  
+  <class type="costmap_converter::CostmapToLinesDBSMCCH" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). 
+      Clustering is performed with DBScan. Clusters are transformed into convex polygons using the Monotone Chain Convex Hull algorithm.
+      The resulting keypoints are used for possible line candidates.
+      A line is only defined if there exist a specified number of support points between each keypoint pair.
+  </description>
+  </class>
+  
+  <class type="costmap_converter::CostmapToLinesDBSRANSAC" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). 
+      Clustering is performed with DBScan. For each cluster RANSAC is applied sequentally to fit multiple line models.
+  </description>
+  </class>
+  
+  <class type="costmap_converter::CostmapToPolygonsDBSConcaveHull" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class converts costmap2d obstacles into a vector of non-convex (concave) polygons. 
+  </description>
+  </class>
+
+  <class type="costmap_converter::CostmapToDynamicObstacles" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class detects and tracks obstacles from a sequence of costmaps. 
+  </description>
+  </class>
+  
+</library>

navigations/costmap_converter/src/costmap_converter_node.cpp

@@ -0,0 +1,296 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#include <ros/ros.h>
+
+#include <costmap_2d/costmap_2d.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <geometry_msgs/PolygonStamped.h>
+#include <visualization_msgs/Marker.h>
+
+#include <costmap_converter/costmap_converter_interface.h>
+#include <pluginlib/class_loader.h>
+
+
+
+class CostmapStandaloneConversion
+{
+public:
+  CostmapStandaloneConversion() : converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), n_("~")
+  {
+      // load converter plugin from parameter server, otherwise set default
+      std::string converter_plugin = "costmap_converter::CostmapToPolygonsDBSMCCH";
+      n_.param("converter_plugin", converter_plugin, converter_plugin);
+
+      try
+      {
+        converter_ = converter_loader_.createInstance(converter_plugin);
+      }
+      catch(const pluginlib::PluginlibException& ex)
+      {
+        ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
+        ros::shutdown();
+      }
+
+      ROS_INFO_STREAM("Standalone costmap converter:" << converter_plugin << " loaded.");
+
+      std::string costmap_topic = "/move_base/local_costmap/costmap";
+      n_.param("costmap_topic", costmap_topic, costmap_topic);
+
+      std::string costmap_update_topic = "/move_base/local_costmap/costmap_updates";
+      n_.param("costmap_update_topic", costmap_update_topic, costmap_update_topic);
+
+      std::string obstacles_topic = "costmap_obstacles";
+      n_.param("obstacles_topic", obstacles_topic, obstacles_topic);
+
+      std::string polygon_marker_topic = "costmap_polygon_markers";
+      n_.param("polygon_marker_topic", polygon_marker_topic, polygon_marker_topic);
+
+      costmap_sub_ = n_.subscribe(costmap_topic, 1, &CostmapStandaloneConversion::costmapCallback, this);
+      costmap_update_sub_ = n_.subscribe(costmap_update_topic, 1, &CostmapStandaloneConversion::costmapUpdateCallback, this);
+      obstacle_pub_ = n_.advertise<costmap_converter::ObstacleArrayMsg>(obstacles_topic, 1000);
+      marker_pub_ = n_.advertise<visualization_msgs::Marker>(polygon_marker_topic, 10);
+
+      occupied_min_value_ = 100;
+      n_.param("occupied_min_value", occupied_min_value_, occupied_min_value_);
+
+      std::string odom_topic = "/odom";
+      n_.param("odom_topic", odom_topic, odom_topic);
+
+      if (converter_)
+      {
+        converter_->setOdomTopic(odom_topic);
+        converter_->initialize(n_);
+        converter_->setCostmap2D(&map_);
+        //converter_->startWorker(ros::Rate(5), &map, true);
+      }
+   }
+
+
+  void costmapCallback(const nav_msgs::OccupancyGridConstPtr& msg)
+  {
+      ROS_INFO_ONCE("Got first costmap callback. This message will be printed once");
+
+      if (msg->info.width != map_.getSizeInCellsX() || msg->info.height != map_.getSizeInCellsY() || msg->info.resolution != map_.getResolution())
+      {
+        ROS_INFO("New map format, resizing and resetting map...");
+        map_.resizeMap(msg->info.width, msg->info.height, msg->info.resolution, msg->info.origin.position.x, msg->info.origin.position.y);
+      }
+      else
+      {
+        map_.updateOrigin(msg->info.origin.position.x, msg->info.origin.position.y);
+      }
+
+
+      for (std::size_t i=0; i < msg->data.size(); ++i)
+      {
+        unsigned int mx, my;
+        map_.indexToCells((unsigned int)i, mx, my);
+        map_.setCost(mx, my, msg->data[i] >= occupied_min_value_ ? 255 : 0 );
+      }
+
+      // convert
+      converter_->updateCostmap2D();
+      converter_->compute();
+      costmap_converter::ObstacleArrayConstPtr obstacles = converter_->getObstacles();
+
+      if (!obstacles)
+        return;
+
+      obstacle_pub_.publish(obstacles);
+
+      frame_id_ = msg->header.frame_id;
+
+      publishAsMarker(frame_id_, *obstacles, marker_pub_);
+  }
+
+  void costmapUpdateCallback(const map_msgs::OccupancyGridUpdateConstPtr& update)
+  {
+    unsigned int di = 0;
+    for (unsigned int y = 0; y < update->height ; ++y)
+    {
+      for (unsigned int x = 0; x < update->width ; ++x)
+      {
+        map_.setCost(x, y, update->data[di++] >= occupied_min_value_ ? 255 : 0 );
+      }
+    }
+
+    // convert
+    // TODO(roesmann): currently, the converter updates the complete costmap and not the part which is updated in this callback
+    converter_->updateCostmap2D();
+    converter_->compute();
+    costmap_converter::ObstacleArrayConstPtr obstacles = converter_->getObstacles();
+
+    if (!obstacles)
+      return;
+
+    obstacle_pub_.publish(obstacles);
+
+    publishAsMarker(frame_id_, *obstacles, marker_pub_);
+  }
+
+  void publishAsMarker(const std::string& frame_id, const std::vector<geometry_msgs::PolygonStamped>& polygonStamped, ros::Publisher& marker_pub)
+  {
+    visualization_msgs::Marker line_list;
+    line_list.header.frame_id = frame_id;
+    line_list.header.stamp = ros::Time::now();
+    line_list.ns = "Polygons";
+    line_list.action = visualization_msgs::Marker::ADD;
+    line_list.pose.orientation.w = 1.0;
+
+    line_list.id = 0;
+    line_list.type = visualization_msgs::Marker::LINE_LIST;
+
+    line_list.scale.x = 0.1;
+    line_list.color.g = 1.0;
+    line_list.color.a = 1.0;
+
+    for (std::size_t i=0; i<polygonStamped.size(); ++i)
+    {
+      for (int j=0; j< (int)polygonStamped[i].polygon.points.size()-1; ++j)
+      {
+        geometry_msgs::Point line_start;
+        line_start.x = polygonStamped[i].polygon.points[j].x;
+        line_start.y = polygonStamped[i].polygon.points[j].y;
+        line_list.points.push_back(line_start);
+        geometry_msgs::Point line_end;
+        line_end.x = polygonStamped[i].polygon.points[j+1].x;
+        line_end.y = polygonStamped[i].polygon.points[j+1].y;
+        line_list.points.push_back(line_end);
+      }
+      // close loop for current polygon
+      if (!polygonStamped[i].polygon.points.empty() && polygonStamped[i].polygon.points.size() != 2 )
+      {
+        geometry_msgs::Point line_start;
+        line_start.x = polygonStamped[i].polygon.points.back().x;
+        line_start.y = polygonStamped[i].polygon.points.back().y;
+        line_list.points.push_back(line_start);
+        if (line_list.points.size() % 2 != 0)
+        {
+          geometry_msgs::Point line_end;
+          line_end.x = polygonStamped[i].polygon.points.front().x;
+          line_end.y = polygonStamped[i].polygon.points.front().y;
+          line_list.points.push_back(line_end);
+        }
+      }
+
+
+    }
+    marker_pub.publish(line_list);
+  }
+
+  void publishAsMarker(const std::string& frame_id, const costmap_converter::ObstacleArrayMsg& obstacles, ros::Publisher& marker_pub)
+  {
+    visualization_msgs::Marker line_list;
+    line_list.header.frame_id = frame_id;
+    line_list.header.stamp = ros::Time::now();
+    line_list.ns = "Polygons";
+    line_list.action = visualization_msgs::Marker::ADD;
+    line_list.pose.orientation.w = 1.0;
+
+    line_list.id = 0;
+    line_list.type = visualization_msgs::Marker::LINE_LIST;
+
+    line_list.scale.x = 0.1;
+    line_list.color.g = 1.0;
+    line_list.color.a = 1.0;
+
+    for (const costmap_converter::ObstacleMsg& obstacle : obstacles.obstacles)
+    {
+      for (int j=0; j< (int)obstacle.polygon.points.size()-1; ++j)
+      {
+        geometry_msgs::Point line_start;
+        line_start.x = obstacle.polygon.points[j].x;
+        line_start.y = obstacle.polygon.points[j].y;
+        line_list.points.push_back(line_start);
+        geometry_msgs::Point line_end;
+        line_end.x = obstacle.polygon.points[j+1].x;
+        line_end.y = obstacle.polygon.points[j+1].y;
+        line_list.points.push_back(line_end);
+      }
+      // close loop for current polygon
+      if (!obstacle.polygon.points.empty() && obstacle.polygon.points.size() != 2 )
+      {
+        geometry_msgs::Point line_start;
+        line_start.x = obstacle.polygon.points.back().x;
+        line_start.y = obstacle.polygon.points.back().y;
+        line_list.points.push_back(line_start);
+        if (line_list.points.size() % 2 != 0)
+        {
+          geometry_msgs::Point line_end;
+          line_end.x = obstacle.polygon.points.front().x;
+          line_end.y = obstacle.polygon.points.front().y;
+          line_list.points.push_back(line_end);
+        }
+      }
+
+
+    }
+    marker_pub.publish(line_list);
+  }
+
+private:
+  pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> converter_loader_;
+  boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> converter_;
+
+  ros::NodeHandle n_;
+  ros::Subscriber costmap_sub_;
+  ros::Subscriber costmap_update_sub_;
+  ros::Publisher obstacle_pub_;
+  ros::Publisher marker_pub_;
+
+  std::string frame_id_;
+  int occupied_min_value_;
+
+  costmap_2d::Costmap2D map_;
+
+};
+
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "costmap_converter");
+
+  CostmapStandaloneConversion convert_process;
+
+  ros::spin();
+
+  return 0;
+}
+
+

navigations/costmap_converter/src/costmap_to_dynamic_obstacles/background_subtractor.cpp

@@ -0,0 +1,115 @@
+#include <opencv2/highgui/highgui.hpp>
+//#include <opencv2/cvv/cvv.hpp>
+#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>
+
+BackgroundSubtractor::BackgroundSubtractor(const Params &parameters): params_(parameters)
+{
+}
+
+void BackgroundSubtractor::apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x, int shift_y)
+{
+  current_frame_ = image;
+
+  // occupancy grids are empty only once in the beginning -> initialize variables
+  if (occupancy_grid_fast_.empty() && occupancy_grid_slow_.empty())
+  {
+    occupancy_grid_fast_ = current_frame_;
+    occupancy_grid_slow_ = current_frame_;
+    previous_shift_x_ = shift_x;
+    previous_shift_y_ = shift_y;
+    return;
+  }
+
+  // Shift previous occupancy grid to new location (match current_frame_)
+  int shift_relative_to_previous_pos_x_ = shift_x - previous_shift_x_;
+  int shift_relative_to_previous_pos_y_ = shift_y - previous_shift_y_;
+  previous_shift_x_ = shift_x;
+  previous_shift_y_ = shift_y;
+
+  // if(shift_relative_to_previous_pos_x_ != 0 || shift_relative_to_previous_pos_y_ != 0)
+  transformToCurrentFrame(shift_relative_to_previous_pos_x_, shift_relative_to_previous_pos_y_);
+
+  // cvv::debugFilter(occupancy_grid_fast_, currentFrame_, CVVISUAL_LOCATION);
+
+  // Calculate normalized sum (mean) of nearest neighbors
+  int size = 3; // 3, 5, 7, ....
+  cv::Mat nearest_neighbor_mean_fast(occupancy_grid_fast_.size(), CV_8UC1);
+  cv::Mat nearest_neighbor_mean_slow(occupancy_grid_slow_.size(), CV_8UC1);
+  cv::boxFilter(occupancy_grid_fast_, nearest_neighbor_mean_fast, -1, cv::Size(size, size), cv::Point(-1, -1), true,
+                cv::BORDER_REPLICATE);
+  cv::boxFilter(occupancy_grid_slow_, nearest_neighbor_mean_slow, -1, cv::Size(size, size), cv::Point(-1, -1), true,
+                cv::BORDER_REPLICATE);
+  //  cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);
+  //  cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);
+
+  // compute time mean value for each pixel according to learningrate alpha
+  // occupancy_grid_fast_ = beta*(alpha_fast*current_frame_ + (1.0-alpha_fast)*occupancy_grid_fast_) + (1-beta)*nearest_neighbor_mean_fast;
+  cv::addWeighted(current_frame_, params_.alpha_fast, occupancy_grid_fast_, (1 - params_.alpha_fast), 0, occupancy_grid_fast_);
+  cv::addWeighted(occupancy_grid_fast_, params_.beta, nearest_neighbor_mean_fast, (1 - params_.beta), 0, occupancy_grid_fast_);
+  // occupancy_grid_slow_ = beta*(alpha_slow*current_frame_ + (1.0-alpha_slow)*occupancy_grid_slow) + (1-beta)*nearest_neighbor_mean_slow;
+  cv::addWeighted(current_frame_, params_.alpha_slow, occupancy_grid_slow_, (1 - params_.alpha_slow), 0, occupancy_grid_slow_);
+  cv::addWeighted(occupancy_grid_slow_, params_.beta, nearest_neighbor_mean_slow, (1 - params_.beta), 0, occupancy_grid_slow_);
+
+  // 1) Obstacles should be detected after a minimum response of the fast filter
+  //    occupancy_grid_fast_ > min_occupancy_probability
+  cv::threshold(occupancy_grid_fast_, occupancy_grid_fast_, params_.min_occupancy_probability, 0 /*unused*/, cv::THRESH_TOZERO);
+  // 2) Moving obstacles have a minimum difference between the responses of the slow and fast filter
+  //    occupancy_grid_fast_-occupancy_grid_slow_ > min_sep_between_fast_and_slow_filter
+  cv::threshold(occupancy_grid_fast_ - occupancy_grid_slow_, fg_mask, params_.min_sep_between_fast_and_slow_filter, 255,
+                cv::THRESH_BINARY);
+  // 3) Dismiss static obstacles
+  //    nearest_neighbor_mean_slow < max_occupancy_neighbors
+  cv::threshold(nearest_neighbor_mean_slow, nearest_neighbor_mean_slow, params_.max_occupancy_neighbors, 255, cv::THRESH_BINARY_INV);
+  cv::bitwise_and(nearest_neighbor_mean_slow, fg_mask, fg_mask);
+
+  //visualize("Current frame", currentFrame_);
+  cv::Mat setBorderToZero = cv::Mat(current_frame_.size(), CV_8UC1, 0.0);
+  int border = 5;
+  setBorderToZero(cv::Rect(border, border, current_frame_.cols-2*border, current_frame_.rows-2*border)) = 255;
+
+  cv::bitwise_and(setBorderToZero, fg_mask, fg_mask);
+
+  // cv::imwrite("/home/albers/Desktop/currentFrame.png", currentFrame_);
+  // visualize("Foreground mask", fgMask);
+
+  // Closing Operation
+  cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE,
+                                              cv::Size(2*params_.morph_size + 1, 2*params_.morph_size + 1),
+                                              cv::Point(params_.morph_size, params_.morph_size));
+  cv::dilate(fg_mask, fg_mask, element);
+  cv::dilate(fg_mask, fg_mask, element);
+  cv::erode(fg_mask, fg_mask, element);
+}
+
+void BackgroundSubtractor::transformToCurrentFrame(int shift_x, int shift_y)
+{
+  // TODO: initialize with current_frame (first observed image) rather than zeros
+
+  // Translate (shift) image in costmap coordinates
+  // in cv::Mat: shift X -> to the left; shift y -> to the top
+  cv::Mat temp_fast, temp_slow;
+  cv::Mat translation_mat = (cv::Mat_<double>(2, 3, CV_64F) << 1, 0, -shift_x, 0, 1, -shift_y);
+  cv::warpAffine(occupancy_grid_fast_, temp_fast, translation_mat, occupancy_grid_fast_.size()); // can't operate in-place
+  cv::warpAffine(occupancy_grid_slow_, temp_slow, translation_mat, occupancy_grid_slow_.size()); // can't operate in-place
+
+  // cvv::debugFilter(occupancy_grid_fast_, temp_fast);
+
+  occupancy_grid_fast_ = temp_fast;
+  occupancy_grid_slow_ = temp_slow;
+}
+
+void BackgroundSubtractor::visualize(const std::string& name, const cv::Mat& image)
+{
+  if (!image.empty())
+  {
+    cv::Mat im = image.clone();
+    cv::flip(im, im, 0);
+    cv::imshow(name, im);
+    cv::waitKey(1);
+  }
+}
+
+void BackgroundSubtractor::updateParameters(const Params &parameters)
+{
+  params_ = parameters;
+}

navigations/costmap_converter/src/costmap_to_dynamic_obstacles/blob_detector.cpp

@@ -0,0 +1,193 @@
+#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>
+#include <opencv2/opencv.hpp>
+#include <iostream>
+
+BlobDetector::BlobDetector(const SimpleBlobDetector::Params& parameters) : params_(parameters) {}
+
+cv::Ptr<BlobDetector> BlobDetector::create(const cv::SimpleBlobDetector::Params& params)
+{
+  return cv::Ptr<BlobDetector> (new BlobDetector(params)); // compatibility with older versions
+  //return cv::makePtr<BlobDetector>(params);
+}
+
+void BlobDetector::detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat&)
+{
+  // TODO: support mask
+  contours_.clear();
+
+  keypoints.clear();
+  cv::Mat grayscale_image;
+  if (image.channels() == 3)
+    cv::cvtColor(image, grayscale_image, cv::COLOR_BGR2GRAY);
+  else
+    grayscale_image = image;
+
+  if (grayscale_image.type() != CV_8UC1)
+  {
+    //CV_Error(cv::Error::StsUnsupportedFormat, "Blob detector only supports 8-bit images!");
+    std::cerr << "Blob detector only supports 8-bit images!\n";
+  }
+
+  std::vector<std::vector<Center>> centers;
+  std::vector<std::vector<cv::Point>> contours;
+  for (double thresh = params_.minThreshold; thresh < params_.maxThreshold; thresh += params_.thresholdStep)
+  {
+    cv::Mat binarized_image;
+    cv::threshold(grayscale_image, binarized_image, thresh, 255, cv::THRESH_BINARY);
+
+    std::vector<Center> cur_centers;
+    std::vector<std::vector<cv::Point>> cur_contours, new_contours;
+    findBlobs(grayscale_image, binarized_image, cur_centers, cur_contours);
+    std::vector<std::vector<Center>> new_centers;
+    for (std::size_t i = 0; i < cur_centers.size(); ++i)
+    {
+      bool isNew = true;
+      for (std::size_t j = 0; j < centers.size(); ++j)
+      {
+        double dist = cv::norm(centers[j][centers[j].size() / 2].location - cur_centers[i].location);
+        isNew = dist >= params_.minDistBetweenBlobs && dist >= centers[j][centers[j].size() / 2].radius &&
+                dist >= cur_centers[i].radius;
+        if (!isNew)
+        {
+          centers[j].push_back(cur_centers[i]);
+
+          size_t k = centers[j].size() - 1;
+          while (k > 0 && centers[j][k].radius < centers[j][k - 1].radius)
+          {
+            centers[j][k] = centers[j][k - 1];
+            k--;
+          }
+          centers[j][k] = cur_centers[i];
+
+          break;
+        }
+      }
+      if (isNew)
+      {
+        new_centers.push_back(std::vector<Center>(1, cur_centers[i]));
+        new_contours.push_back(cur_contours[i]);
+      }
+    }
+    std::copy(new_centers.begin(), new_centers.end(), std::back_inserter(centers));
+    std::copy(new_contours.begin(), new_contours.end(), std::back_inserter(contours));
+  }
+
+  for (size_t i = 0; i < centers.size(); ++i)
+  {
+    if (centers[i].size() < params_.minRepeatability)
+      continue;
+    cv::Point2d sum_point(0, 0);
+    double normalizer = 0;
+    for (std::size_t j = 0; j < centers[i].size(); ++j)
+    {
+      sum_point += centers[i][j].confidence * centers[i][j].location;
+      normalizer += centers[i][j].confidence;
+    }
+    sum_point *= (1. / normalizer);
+    cv::KeyPoint kpt(sum_point, (float)(centers[i][centers[i].size() / 2].radius));
+    keypoints.push_back(kpt);
+    contours_.push_back(contours[i]);
+  }
+}
+
+void BlobDetector::findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,
+                             std::vector<std::vector<cv::Point>>& cur_contours) const
+{
+  (void)image;
+  centers.clear();
+  cur_contours.clear();
+
+  std::vector<std::vector<cv::Point>> contours;
+  cv::Mat tmp_binary_image = binary_image.clone();
+  cv::findContours(tmp_binary_image, contours, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);
+
+  for (std::size_t contour_idx = 0; contour_idx < contours.size(); ++contour_idx)
+  {
+    Center center;
+    center.confidence = 1;
+    cv::Moments moms = cv::moments(cv::Mat(contours[contour_idx]));
+    if (params_.filterByArea)
+    {
+      double area = moms.m00;
+      if (area < params_.minArea || area >= params_.maxArea)
+        continue;
+    }
+
+    if (params_.filterByCircularity)
+    {
+      double area = moms.m00;
+      double perimeter = cv::arcLength(cv::Mat(contours[contour_idx]), true);
+      double ratio = 4 * CV_PI * area / (perimeter * perimeter);
+      if (ratio < params_.minCircularity || ratio >= params_.maxCircularity)
+        continue;
+    }
+
+    if (params_.filterByInertia)
+    {
+      double denominator = std::sqrt(std::pow(2 * moms.mu11, 2) + std::pow(moms.mu20 - moms.mu02, 2));
+      const double eps = 1e-2;
+      double ratio;
+      if (denominator > eps)
+      {
+        double cosmin = (moms.mu20 - moms.mu02) / denominator;
+        double sinmin = 2 * moms.mu11 / denominator;
+        double cosmax = -cosmin;
+        double sinmax = -sinmin;
+
+        double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin;
+        double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax;
+        ratio = imin / imax;
+      }
+      else
+      {
+        ratio = 1;
+      }
+
+      if (ratio < params_.minInertiaRatio || ratio >= params_.maxInertiaRatio)
+        continue;
+
+      center.confidence = ratio * ratio;
+    }
+
+    if (params_.filterByConvexity)
+    {
+      std::vector<cv::Point> hull;
+      cv::convexHull(cv::Mat(contours[contour_idx]), hull);
+      double area = cv::contourArea(cv::Mat(contours[contour_idx]));
+      double hullArea = cv::contourArea(cv::Mat(hull));
+      double ratio = area / hullArea;
+      if (ratio < params_.minConvexity || ratio >= params_.maxConvexity)
+        continue;
+    }
+
+    if (moms.m00 == 0.0)
+      continue;
+    center.location = cv::Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00);
+
+    if (params_.filterByColor)
+    {
+      if (binary_image.at<uchar>(cvRound(center.location.y), cvRound(center.location.x)) != params_.blobColor)
+        continue;
+    }
+
+    // compute blob radius
+    {
+      std::vector<double> dists;
+      for (std::size_t point_idx = 0; point_idx < contours[contour_idx].size(); ++point_idx)
+      {
+        cv::Point2d pt = contours[contour_idx][point_idx];
+        dists.push_back(cv::norm(center.location - pt));
+      }
+      std::sort(dists.begin(), dists.end());
+      center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.;
+    }
+
+    centers.push_back(center);
+    cur_contours.push_back(contours[contour_idx]);
+  }
+}
+
+void BlobDetector::updateParameters(const Params& parameters)
+{
+  params_ = parameters;
+}

navigations/costmap_converter/src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp

@@ -0,0 +1,476 @@
+#include <costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h>
+
+#include <pluginlib/class_list_macros.h>
+#include <tf/tf.h>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToDynamicObstacles, costmap_converter::BaseCostmapToPolygons)
+
+namespace costmap_converter
+{
+
+CostmapToDynamicObstacles::CostmapToDynamicObstacles() : BaseCostmapToDynamicObstacles()
+{
+  ego_vel_.x = ego_vel_.y = ego_vel_.z = 0;
+  costmap_ = nullptr;
+  dynamic_recfg_ = nullptr;
+}
+
+CostmapToDynamicObstacles::~CostmapToDynamicObstacles()
+{
+  if(dynamic_recfg_ != nullptr)
+    delete dynamic_recfg_;
+}
+
+void CostmapToDynamicObstacles::initialize(ros::NodeHandle nh)
+{
+  costmap_ = nullptr;
+
+  // We need the odometry from the robot to compensate the ego motion
+  ros::NodeHandle gn; // create odom topic w.r.t. global node handle
+  odom_sub_ = gn.subscribe(odom_topic_, 1, &CostmapToDynamicObstacles::odomCallback, this);
+
+  nh.param("publish_static_obstacles", publish_static_obstacles_, publish_static_obstacles_);
+
+  //////////////////////////////////
+  // Foreground detection parameters
+  BackgroundSubtractor::Params bg_sub_params;
+
+  bg_sub_params.alpha_slow = 0.3;
+  nh.param("alpha_slow", bg_sub_params.alpha_slow, bg_sub_params.alpha_slow);
+
+  bg_sub_params.alpha_fast = 0.85;
+  nh.param("alpha_fast", bg_sub_params.alpha_fast, bg_sub_params.alpha_fast);
+
+  bg_sub_params.beta = 0.85;
+  nh.param("beta", bg_sub_params.beta, bg_sub_params.beta);
+
+  bg_sub_params.min_occupancy_probability = 180;
+  nh.param("min_occupancy_probability", bg_sub_params.min_occupancy_probability, bg_sub_params.min_occupancy_probability);
+
+  bg_sub_params.min_sep_between_fast_and_slow_filter = 80;
+  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);
+
+  bg_sub_params.max_occupancy_neighbors = 100;
+  nh.param("max_occupancy_neighbors", bg_sub_params.max_occupancy_neighbors, bg_sub_params.max_occupancy_neighbors);
+
+  bg_sub_params.morph_size = 1;
+  nh.param("morph_size", bg_sub_params.morph_size, bg_sub_params.morph_size);
+
+  bg_sub_ = std::unique_ptr<BackgroundSubtractor>(new BackgroundSubtractor(bg_sub_params));
+
+  ////////////////////////////
+  // Blob detection parameters
+  BlobDetector::Params blob_det_params;
+
+  blob_det_params.filterByColor = true; // actually filterByIntensity, always true
+  blob_det_params.blobColor = 255;      // Extract light blobs
+  blob_det_params.thresholdStep = 256;  // Input for blob detection is already a binary image
+  blob_det_params.minThreshold = 127;
+  blob_det_params.maxThreshold = 255;
+  blob_det_params.minRepeatability = 1;
+
+  blob_det_params.minDistBetweenBlobs = 10;
+  nh.param("min_distance_between_blobs", blob_det_params.minDistBetweenBlobs, blob_det_params.minDistBetweenBlobs);
+
+  blob_det_params.filterByArea = true;
+  nh.param("filter_by_area", blob_det_params.filterByArea, blob_det_params.filterByArea);
+
+  blob_det_params.minArea = 3; // Filter out blobs with less pixels
+  nh.param("min_area", blob_det_params.minArea, blob_det_params.minArea);
+
+  blob_det_params.maxArea = 300;
+  nh.param("max_area", blob_det_params.maxArea, blob_det_params.maxArea);
+
+  blob_det_params.filterByCircularity = true; // circularity = 4*pi*area/perimeter^2
+  nh.param("filter_by_circularity", blob_det_params.filterByCircularity, blob_det_params.filterByCircularity);
+
+  blob_det_params.minCircularity = 0.2;
+  nh.param("min_circularity", blob_det_params.minCircularity, blob_det_params.minCircularity);
+
+  blob_det_params.maxCircularity = 1; // maximal 1 (in case of a circle)
+  nh.param("max_circularity", blob_det_params.maxCircularity, blob_det_params.maxCircularity);
+
+  blob_det_params.filterByInertia = true; // Filter blobs based on their elongation
+  nh.param("filter_by_intertia", blob_det_params.filterByInertia, blob_det_params.filterByInertia);
+
+  blob_det_params.minInertiaRatio = 0.2;  // minimal 0 (in case of a line)
+  nh.param("min_inertia_ratio", blob_det_params.minInertiaRatio, blob_det_params.minInertiaRatio);
+
+  blob_det_params.maxInertiaRatio = 1;    // maximal 1 (in case of a circle)
+  nh.param("max_intertia_ratio", blob_det_params.maxInertiaRatio, blob_det_params.maxInertiaRatio);
+
+  blob_det_params.filterByConvexity = false; // Area of the Blob / Area of its convex hull
+  nh.param("filter_by_convexity", blob_det_params.filterByConvexity, blob_det_params.filterByConvexity);
+
+  blob_det_params.minConvexity = 0;          // minimal 0
+  nh.param("min_convexity", blob_det_params.minConvexity, blob_det_params.minConvexity);
+
+  blob_det_params.maxConvexity = 1;          // maximal 1
+  nh.param("max_convexity", blob_det_params.maxConvexity, blob_det_params.maxConvexity);
+
+  blob_det_ = BlobDetector::create(blob_det_params);
+
+  ////////////////////////////////////
+  // Tracking parameters
+  CTracker::Params tracker_params;
+  tracker_params.dt = 0.2;
+  nh.param("dt", tracker_params.dt, tracker_params.dt);
+
+  tracker_params.dist_thresh = 60.0;
+  nh.param("dist_thresh", tracker_params.dist_thresh, tracker_params.dist_thresh);
+
+  tracker_params.max_allowed_skipped_frames = 3;
+  nh.param("max_allowed_skipped_frames", tracker_params.max_allowed_skipped_frames, tracker_params.max_allowed_skipped_frames);
+
+  tracker_params.max_trace_length = 10;
+  nh.param("max_trace_length", tracker_params.max_trace_length, tracker_params.max_trace_length);
+
+  tracker_ = std::unique_ptr<CTracker>(new CTracker(tracker_params));
+
+
+  ////////////////////////////////////
+  // Static costmap conversion parameters
+  std::string static_converter_plugin = "costmap_converter::CostmapToPolygonsDBSMCCH";
+  nh.param("static_converter_plugin", static_converter_plugin, static_converter_plugin);
+  loadStaticCostmapConverterPlugin(static_converter_plugin, nh);
+
+
+  // setup dynamic reconfigure
+  dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>(nh);
+  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>::CallbackType cb = boost::bind(&CostmapToDynamicObstacles::reconfigureCB, this, _1, _2);
+  dynamic_recfg_->setCallback(cb);
+}
+
+void CostmapToDynamicObstacles::compute()
+{
+  if (costmap_mat_.empty())
+    return;
+
+  /////////////////////////// Foreground detection ////////////////////////////////////
+  // Dynamic obstacles are separated from static obstacles
+  int origin_x = round(costmap_->getOriginX() / costmap_->getResolution());
+  int origin_y = round(costmap_->getOriginY() / costmap_->getResolution());
+  // ROS_INFO("Origin x  [m]: %f    Origin_y  [m]: %f", costmap_->getOriginX(), costmap_->getOriginY());
+  // ROS_INFO("Origin x [px]: %d \t Origin_y [px]: %d", originX, originY);
+
+  bg_sub_->apply(costmap_mat_, fg_mask_, origin_x, origin_y);
+
+  // if no foreground object is detected, no ObstacleMsgs need to be published
+  if (fg_mask_.empty())
+    return;
+
+  cv::Mat bg_mat;
+  if (publish_static_obstacles_)
+  {
+    // Get static obstacles
+    bg_mat = costmap_mat_ - fg_mask_;
+    // visualize("bg_mat", bg_mat);
+  }
+
+
+  /////////////////////////////// Blob detection /////////////////////////////////////
+  // Centers and contours of Blobs are detected
+  blob_det_->detect(fg_mask_, keypoints_);
+  std::vector<std::vector<cv::Point>> contours = blob_det_->getContours();
+
+
+  ////////////////////////////// Tracking ////////////////////////////////////////////
+  // Objects are assigned to objects from previous frame based on Hungarian Algorithm
+  // Object velocities are estimated using a Kalman Filter
+  std::vector<Point_t> detected_centers(keypoints_.size());
+  for (int i = 0; i < keypoints_.size(); i++)
+  {
+    detected_centers.at(i).x = keypoints_.at(i).pt.x;
+    detected_centers.at(i).y = keypoints_.at(i).pt.y;
+    detected_centers.at(i).z = 0; // Currently unused!
+  }
+
+  tracker_->Update(detected_centers, contours);
+
+
+  ///////////////////////////////////// Output ///////////////////////////////////////
+  /*
+  cv::Mat fg_mask_with_keypoints = cv::Mat::zeros(fg_mask.size(), CV_8UC3);
+  cv::cvtColor(fg_mask, fg_mask_with_keypoints, cv::COLOR_GRAY2BGR);
+
+  for (int i = 0; i < (int)tracker_->tracks.size(); ++i)
+    cv::rectangle(fg_mask_with_keypoints, cv::boundingRect(tracker_->tracks[i]->getLastContour()),
+                  cv::Scalar(0, 0, 255), 1);
+
+  //visualize("fgMaskWithKeyPoints", fgMaskWithKeypoints);
+  //cv::imwrite("/home/albers/Desktop/fgMask.png", fgMask);
+  //cv::imwrite("/home/albers/Desktop/fgMaskWithKeyPoints.png", fgMaskWithKeypoints);
+  */
+
+  //////////////////////////// Fill ObstacleContainerPtr /////////////////////////////
+  ObstacleArrayPtr obstacles(new ObstacleArrayMsg);
+  // header.seq is automatically filled
+  obstacles->header.stamp = ros::Time::now();
+  obstacles->header.frame_id = "/map"; //Global frame /map
+
+  // For all tracked objects
+  for (unsigned int i = 0; i < (unsigned int)tracker_->tracks.size(); ++i)
+  {
+    geometry_msgs::Polygon polygon;
+
+    // TODO directly create polygon inside getContour and avoid copy
+    std::vector<Point_t> contour;
+    getContour(i, contour); // this method also transforms map to world coordinates
+
+    // convert contour to polygon
+    for (const Point_t& pt : contour)
+    {
+      polygon.points.emplace_back();
+      polygon.points.back().x = pt.x;
+      polygon.points.back().y = pt.y;
+      polygon.points.back().z = 0;
+    }
+
+    obstacles->obstacles.emplace_back();
+    obstacles->obstacles.back().polygon = polygon;
+
+    // Set obstacle ID
+    obstacles->obstacles.back().id = tracker_->tracks.at(i)->track_id;
+
+    // Set orientation
+    geometry_msgs::QuaternionStamped orientation;
+
+    Point_t vel = getEstimatedVelocityOfObject(i);
+    double yaw = std::atan2(vel.y, vel.x);
+    //ROS_INFO("yaw: %f", yaw);
+    obstacles->obstacles.back().orientation = tf::createQuaternionMsgFromYaw(yaw);
+
+    // Set velocity
+    geometry_msgs::TwistWithCovariance velocities;
+    //velocities.twist.linear.x = std::sqrt(vel.x*vel.x + vel.y*vel.y);
+    //velocities.twist.linear.y = 0;
+    velocities.twist.linear.x = vel.x;
+    velocities.twist.linear.y = vel.y; // TODO(roesmann): don't we need to consider the transformation between opencv's and costmap's coordinate frames?
+    velocities.twist.linear.z = 0;
+    velocities.twist.angular.x = 0;
+    velocities.twist.angular.y = 0;
+    velocities.twist.angular.z = 0;
+
+    // TODO: use correct covariance matrix
+    velocities.covariance = {1, 0, 0, 0, 0, 0,
+                             0, 1, 0, 0, 0, 0,
+                             0, 0, 1, 0, 0, 0,
+                             0, 0, 0, 1, 0, 0,
+                             0, 0, 0, 0, 1, 0,
+                             0, 0, 0, 0, 0, 1};
+
+    obstacles->obstacles.back().velocities = velocities;
+  }
+
+  ////////////////////////// Static obstacles ////////////////////////////
+  if (publish_static_obstacles_)
+  {
+    uchar* img_data = bg_mat.data;
+    int width = bg_mat.cols;
+    int height = bg_mat.rows;
+    int stride = bg_mat.step;
+
+    if (stackedCostmapConversion())
+    {
+      // Create new costmap with static obstacles (background)
+      boost::shared_ptr<costmap_2d::Costmap2D> static_costmap(new costmap_2d::Costmap2D(costmap_->getSizeInCellsX(),
+                                                                                        costmap_->getSizeInCellsY(),
+                                                                                        costmap_->getResolution(),
+                                                                                        costmap_->getOriginX(),
+                                                                                        costmap_->getOriginY()));
+      for(int i = 0; i < height; i++)
+      {
+        for(int j = 0; j < width; j++)
+        {
+          static_costmap->setCost(j, i, img_data[i * stride + j]);
+        }
+      }
+
+      // Apply static obstacle conversion plugin
+      setStaticCostmap(static_costmap);
+      convertStaticObstacles();
+
+      // Store converted static obstacles for publishing
+      auto static_polygons = getStaticPolygons();
+      for (auto it = static_polygons->begin(); it != static_polygons->end(); ++it)
+      {
+        obstacles->obstacles.emplace_back();
+        obstacles->obstacles.back().polygon = *it;
+        obstacles->obstacles.back().velocities.twist.linear.x = 0;
+        obstacles->obstacles.back().velocities.twist.linear.y = 0;
+        obstacles->obstacles.back().id = -1;
+      }
+    }
+    // Otherwise leave static obstacles point-shaped
+    else
+    {
+      for(int i = 0; i < height; i++)
+      {
+        for(int j = 0; j < width; j++)
+        {
+            uchar value = img_data[i * stride + j];
+            if (value > 0)
+            {
+              // obstacle found
+              obstacles->obstacles.emplace_back();
+              geometry_msgs::Point32 pt;
+              pt.x = (double)j*costmap_->getResolution() + costmap_->getOriginX();
+              pt.y = (double)i*costmap_->getResolution() + costmap_->getOriginY();
+              obstacles->obstacles.back().polygon.points.push_back(pt);
+              obstacles->obstacles.back().velocities.twist.linear.x = 0;
+              obstacles->obstacles.back().velocities.twist.linear.y = 0;
+              obstacles->obstacles.back().id = -1;
+            }
+        }
+      }
+    }
+  }
+
+  updateObstacleContainer(obstacles);
+}
+
+void CostmapToDynamicObstacles::setCostmap2D(costmap_2d::Costmap2D* costmap)
+{
+  if (!costmap)
+    return;
+
+  costmap_ = costmap;
+
+  updateCostmap2D();
+}
+
+void CostmapToDynamicObstacles::updateCostmap2D()
+{
+  if (!costmap_->getMutex())
+  {
+    ROS_ERROR("Cannot update costmap since the mutex pointer is null");
+    return;
+  }
+  costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
+
+  // Initialize costmapMat_ directly with the unsigned char array of costmap_
+  //costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
+  //                      costmap_->getCharMap()).clone(); // Deep copy: TODO
+  costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
+                        costmap_->getCharMap());
+}
+
+ObstacleArrayConstPtr CostmapToDynamicObstacles::getObstacles()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+  return obstacles_;
+}
+
+void CostmapToDynamicObstacles::updateObstacleContainer(ObstacleArrayPtr obstacles)
+{
+  boost::mutex::scoped_lock lock(mutex_);
+  obstacles_ = obstacles;
+}
+
+Point_t CostmapToDynamicObstacles::getEstimatedVelocityOfObject(unsigned int idx)
+{
+  // vel [px/s] * costmapResolution [m/px] = vel [m/s]
+  Point_t vel = tracker_->tracks.at(idx)->getEstimatedVelocity() * costmap_->getResolution() + ego_vel_;
+
+  //ROS_INFO("vel x: %f, vel y: %f, vel z: %f", vel.x, vel.y, vel.z);
+  // velocity in /map frame
+  return vel;
+}
+
+void CostmapToDynamicObstacles::odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+  ROS_INFO_ONCE("CostmapToDynamicObstacles: odom received.");
+
+  tf::Quaternion pose;
+  tf::quaternionMsgToTF(msg->pose.pose.orientation, pose);
+
+  tf::Vector3 twistLinear;
+  tf::vector3MsgToTF(msg->twist.twist.linear, twistLinear);
+
+  // velocity of the robot in x, y and z coordinates
+  tf::Vector3 vel = tf::quatRotate(pose, twistLinear);
+  ego_vel_.x = vel.x();
+  ego_vel_.y = vel.y();
+  ego_vel_.z = vel.z();
+}
+
+void CostmapToDynamicObstacles::reconfigureCB(CostmapToDynamicObstaclesConfig& config, uint32_t level)
+{
+  publish_static_obstacles_ = config.publish_static_obstacles;
+
+  // BackgroundSubtraction Parameters
+  BackgroundSubtractor::Params bg_sub_params;
+  bg_sub_params.alpha_slow = config.alpha_slow;
+  bg_sub_params.alpha_fast = config.alpha_fast;
+  bg_sub_params.beta = config.beta;
+  bg_sub_params.min_sep_between_fast_and_slow_filter = config.min_sep_between_slow_and_fast_filter;
+  bg_sub_params.min_occupancy_probability = config.min_occupancy_probability;
+  bg_sub_params.max_occupancy_neighbors = config.max_occupancy_neighbors;
+  bg_sub_params.morph_size = config.morph_size;
+  bg_sub_->updateParameters(bg_sub_params);
+
+  // BlobDetector Parameters
+  BlobDetector::Params blob_det_params;
+  // necessary, because blobDetParams are otherwise initialized with default values for dark blobs
+  blob_det_params.filterByColor = true; // actually filterByIntensity, always true
+  blob_det_params.blobColor = 255;      // Extract light blobs
+  blob_det_params.thresholdStep = 256;  // Input for blobDetection is already a binary image
+  blob_det_params.minThreshold = 127;
+  blob_det_params.maxThreshold = 255;
+  blob_det_params.minRepeatability = 1;
+  blob_det_params.minDistBetweenBlobs = config.min_distance_between_blobs; // TODO: Currently not working as expected
+  blob_det_params.filterByArea = config.filter_by_area;
+  blob_det_params.minArea = config.min_area;
+  blob_det_params.maxArea = config.max_area;
+  blob_det_params.filterByCircularity = config.filter_by_circularity;
+  blob_det_params.minCircularity = config.min_circularity;
+  blob_det_params.maxCircularity = config.max_circularity;
+  blob_det_params.filterByInertia = config.filter_by_inertia;
+  blob_det_params.minInertiaRatio = config.min_inertia_ratio;
+  blob_det_params.maxInertiaRatio = config.max_inertia_ratio;
+  blob_det_params.filterByConvexity = config.filter_by_convexity;
+  blob_det_params.minConvexity = config.min_convexity;
+  blob_det_params.maxConvexity = config.max_convexity;
+  blob_det_->updateParameters(blob_det_params);
+
+  // Tracking Parameters
+  CTracker::Params tracking_params;
+  tracking_params.dt = config.dt;
+  tracking_params.dist_thresh = config.dist_thresh;
+  tracking_params.max_allowed_skipped_frames = config.max_allowed_skipped_frames;
+  tracking_params.max_trace_length = config.max_trace_length;
+  tracker_->updateParameters(tracking_params);
+}
+
+void CostmapToDynamicObstacles::getContour(unsigned int idx, std::vector<Point_t>& contour)
+{
+  assert(!tracker_->tracks.empty() && idx < tracker_->tracks.size());
+
+  contour.clear();
+
+  // contour [px] * costmapResolution [m/px] = contour [m]
+  std::vector<cv::Point> contour2i = tracker_->tracks.at(idx)->getLastContour();
+
+  contour.reserve(contour2i.size());
+
+  Point_t costmap_origin(costmap_->getOriginX(), costmap_->getOriginY(), 0);
+
+  for (std::size_t i = 0; i < contour2i.size(); ++i)
+  {
+    contour.push_back((Point_t(contour2i.at(i).x, contour2i.at(i).y, 0.0)*costmap_->getResolution())
+                        + costmap_origin); // Shift to /map
+  }
+
+}
+
+void CostmapToDynamicObstacles::visualize(const std::string& name, const cv::Mat& image)
+{
+  if (!image.empty())
+  {
+    cv::Mat im = image.clone();
+    cv::flip(im, im, 0);
+    cv::imshow(name, im);
+    cv::waitKey(1);
+  }
+}
+}

navigations/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp

@@ -0,0 +1,130 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>
+
+// ---------------------------------------------------------------------------
+// Tracker. Manage tracks. Create, remove, update.
+// ---------------------------------------------------------------------------
+CTracker::CTracker(const Params &parameters)
+    : params(parameters),
+      NextTrackID(0)
+{
+}
+// ---------------------------------------------------------------------------
+//
+// ---------------------------------------------------------------------------
+void CTracker::Update(const std::vector<Point_t>& detectedCentroid, const std::vector< std::vector<cv::Point> >& contours)
+{
+  // Each contour has a centroid
+  assert(detectedCentroid.size() == contours.size());
+
+  // -----------------------------------
+  // If there is no tracks yet, then every cv::Point begins its own track.
+  // -----------------------------------
+  if (tracks.size() == 0)
+  {
+    // If no tracks yet
+    for (size_t i = 0; i < detectedCentroid.size(); ++i)
+    {
+      tracks.push_back(
+          std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
+    }
+  }
+
+  size_t N = tracks.size();
+  size_t M = detectedCentroid.size();
+
+  assignments_t assignment;
+
+  if (!tracks.empty())
+  {
+    // Distance matrix of N-th Track to the M-th detectedCentroid
+    distMatrix_t Cost(N * M);
+
+    // calculate distance between the blobs centroids
+    for (size_t i = 0; i < tracks.size(); i++)
+    {
+      for (size_t j = 0; j < detectedCentroid.size(); j++)
+      {
+        Cost[i + j * N] = tracks[i]->CalcDist(detectedCentroid[j]);
+      }
+    }
+
+    // -----------------------------------
+    // Solving assignment problem (tracks and predictions of Kalman filter)
+    // -----------------------------------
+    AssignmentProblemSolver APS;
+    APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal);
+
+    // -----------------------------------
+    // clean assignment from pairs with large distance
+    // -----------------------------------
+    for (size_t i = 0; i < assignment.size(); i++)
+    {
+      if (assignment[i] != -1)
+      {
+        if (Cost[i + assignment[i] * N] > params.dist_thresh)
+        {
+          assignment[i] = -1;
+          tracks[i]->skipped_frames = 1;
+        }
+      }
+      else
+      {
+        // If track have no assigned detect, then increment skipped frames counter.
+        tracks[i]->skipped_frames++;
+      }
+    }
+
+    // -----------------------------------
+    // If track didn't get detects long time, remove it.
+    // -----------------------------------
+    for (int i = 0; i < static_cast<int>(tracks.size()); i++)
+    {
+      if (tracks[i]->skipped_frames > params.max_allowed_skipped_frames)
+      {
+        tracks.erase(tracks.begin() + i);
+        assignment.erase(assignment.begin() + i);
+        i--;
+      }
+    }
+  }
+
+  // -----------------------------------
+  // Search for unassigned detects and start new tracks for them.
+  // -----------------------------------
+  for (size_t i = 0; i < detectedCentroid.size(); ++i)
+  {
+    if (find(assignment.begin(), assignment.end(), i) == assignment.end())
+    {
+      tracks.push_back(std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
+    }
+  }
+
+  // Update Kalman Filters state
+
+  for (size_t i = 0; i < assignment.size(); i++)
+  {
+    // If track updated less than one time, than filter state is not correct.
+
+    if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
+    {
+      tracks[i]->skipped_frames = 0;
+      tracks[i]->Update(detectedCentroid[assignment[i]], contours[assignment[i]], true, params.max_trace_length);
+    }
+    else // if not continue using predictions
+    {
+      tracks[i]->Update(Point_t(), std::vector<cv::Point>(), false, params.max_trace_length);
+    }
+  }
+}
+
+void CTracker::updateParameters(const Params &parameters)
+{
+  params = parameters;
+}
+// ---------------------------------------------------------------------------
+//
+// ---------------------------------------------------------------------------
+CTracker::~CTracker(void) {}

navigations/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp

@@ -0,0 +1,732 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h>
+#include <limits>
+
+AssignmentProblemSolver::AssignmentProblemSolver() {}
+
+AssignmentProblemSolver::~AssignmentProblemSolver() {}
+
+track_t AssignmentProblemSolver::Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns,
+                                       std::vector<int>& assignment, TMethod Method)
+{
+  assignment.resize(nOfRows, -1);
+
+  track_t cost = 0;
+
+  switch (Method)
+  {
+  case optimal:
+    assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
+    break;
+
+  case many_forbidden_assignments:
+    assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
+    break;
+
+  case without_forbidden_assignments:
+    assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
+    break;
+  }
+
+  return cost;
+}
+// --------------------------------------------------------------------------
+// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost,
+                                                const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
+{
+  // Generate distance cv::Matrix
+  // and check cv::Matrix elements positiveness :)
+
+  // Total elements number
+  size_t nOfElements = nOfRows * nOfColumns;
+  // Memory allocation
+  track_t* distMatrix = (track_t*)malloc(nOfElements * sizeof(track_t));
+  // Pointer to last element
+  track_t* distMatrixEnd = distMatrix + nOfElements;
+
+  for (size_t row = 0; row < nOfElements; row++)
+  {
+    track_t value = distMatrixIn[row];
+    assert(value >= 0);
+    distMatrix[row] = value;
+  }
+
+  // Memory allocation
+  bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool));
+  bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool));
+  bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool));
+  bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool));
+  bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */
+
+  /* preliminary steps */
+  if (nOfRows <= nOfColumns)
+  {
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      /* find the smallest element in the row */
+      track_t* distMatrixTemp = distMatrix + row;
+      track_t minValue = *distMatrixTemp;
+      distMatrixTemp += nOfRows;
+      while (distMatrixTemp < distMatrixEnd)
+      {
+        track_t value = *distMatrixTemp;
+        if (value < minValue)
+        {
+          minValue = value;
+        }
+        distMatrixTemp += nOfRows;
+      }
+      /* subtract the smallest element from each element of the row */
+      distMatrixTemp = distMatrix + row;
+      while (distMatrixTemp < distMatrixEnd)
+      {
+        *distMatrixTemp -= minValue;
+        distMatrixTemp += nOfRows;
+      }
+    }
+    /* Steps 1 and 2a */
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        if (distMatrix[row + nOfRows * col] == 0)
+        {
+          if (!coveredColumns[col])
+          {
+            starMatrix[row + nOfRows * col] = true;
+            coveredColumns[col] = true;
+            break;
+          }
+        }
+      }
+    }
+  }
+  else /* if(nOfRows > nOfColumns) */
+  {
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      /* find the smallest element in the column */
+      track_t* distMatrixTemp = distMatrix + nOfRows * col;
+      track_t* columnEnd = distMatrixTemp + nOfRows;
+      track_t minValue = *distMatrixTemp++;
+      while (distMatrixTemp < columnEnd)
+      {
+        track_t value = *distMatrixTemp++;
+        if (value < minValue)
+        {
+          minValue = value;
+        }
+      }
+      /* subtract the smallest element from each element of the column */
+      distMatrixTemp = distMatrix + nOfRows * col;
+      while (distMatrixTemp < columnEnd)
+      {
+        *distMatrixTemp++ -= minValue;
+      }
+    }
+    /* Steps 1 and 2a */
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      for (size_t row = 0; row < nOfRows; row++)
+      {
+        if (distMatrix[row + nOfRows * col] == 0)
+        {
+          if (!coveredRows[row])
+          {
+            starMatrix[row + nOfRows * col] = true;
+            coveredColumns[col] = true;
+            coveredRows[row] = true;
+            break;
+          }
+        }
+      }
+    }
+
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      coveredRows[row] = false;
+    }
+  }
+  /* move to step 2b */
+  step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+         nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns);
+  /* compute cost and remove invalid assignments */
+  computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
+  /* free allocated memory */
+  free(distMatrix);
+  free(coveredColumns);
+  free(coveredRows);
+  free(starMatrix);
+  free(primeMatrix);
+  free(newStarMatrix);
+  return;
+}
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows,
+                                                    size_t nOfColumns)
+{
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      if (starMatrix[row + nOfRows * col])
+      {
+        assignment[row] = static_cast<int>(col);
+        break;
+      }
+    }
+  }
+}
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost,
+                                                    const distMatrix_t& distMatrixIn, size_t nOfRows)
+{
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    const int col = assignment[row];
+    if (col >= 0)
+    {
+      cost += distMatrixIn[row + nOfRows * col];
+    }
+  }
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                     size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+  bool* starMatrixTemp, *columnEnd;
+  /* cover every column containing a starred zero */
+  for (size_t col = 0; col < nOfColumns; col++)
+  {
+    starMatrixTemp = starMatrix + nOfRows * col;
+    columnEnd = starMatrixTemp + nOfRows;
+    while (starMatrixTemp < columnEnd)
+    {
+      if (*starMatrixTemp++)
+      {
+        coveredColumns[col] = true;
+        break;
+      }
+    }
+  }
+  /* move to step 3 */
+  step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+         nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                     size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+  /* count covered columns */
+  size_t nOfCoveredColumns = 0;
+  for (size_t col = 0; col < nOfColumns; col++)
+  {
+    if (coveredColumns[col])
+    {
+      nOfCoveredColumns++;
+    }
+  }
+  if (nOfCoveredColumns == minDim)
+  {
+    /* algorithm finished */
+    buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
+  }
+  else
+  {
+    /* move to step 3 */
+    step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+          nOfColumns, minDim);
+  }
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                    size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+  bool zerosFound = true;
+  while (zerosFound)
+  {
+    zerosFound = false;
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      if (!coveredColumns[col])
+      {
+        for (size_t row = 0; row < nOfRows; row++)
+        {
+          if ((!coveredRows[row]) && (distMatrix[row + nOfRows * col] == 0))
+          {
+            /* prime zero */
+            primeMatrix[row + nOfRows * col] = true;
+            /* find starred zero in current row */
+            size_t starCol = 0;
+            for (; starCol < nOfColumns; starCol++)
+            {
+              if (starMatrix[row + nOfRows * starCol])
+              {
+                break;
+              }
+            }
+            if (starCol == nOfColumns) /* no starred zero found */
+            {
+              /* move to step 4 */
+              step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows,
+                    nOfRows, nOfColumns, minDim, row, col);
+              return;
+            }
+            else
+            {
+              coveredRows[row] = true;
+              coveredColumns[starCol] = false;
+              zerosFound = true;
+              break;
+            }
+          }
+        }
+      }
+    }
+  }
+  /* move to step 5 */
+  step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+        nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                    size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col)
+{
+  const size_t nOfElements = nOfRows * nOfColumns;
+  /* generate temporary copy of starMatrix */
+  for (size_t n = 0; n < nOfElements; n++)
+  {
+    newStarMatrix[n] = starMatrix[n];
+  }
+  /* star current zero */
+  newStarMatrix[row + nOfRows * col] = true;
+  /* find starred zero in current column */
+  size_t starCol = col;
+  size_t starRow = 0;
+  for (; starRow < nOfRows; starRow++)
+  {
+    if (starMatrix[starRow + nOfRows * starCol])
+    {
+      break;
+    }
+  }
+  while (starRow < nOfRows)
+  {
+    /* unstar the starred zero */
+    newStarMatrix[starRow + nOfRows * starCol] = false;
+    /* find primed zero in current row */
+    size_t primeRow = starRow;
+    size_t primeCol = 0;
+    for (; primeCol < nOfColumns; primeCol++)
+    {
+      if (primeMatrix[primeRow + nOfRows * primeCol])
+      {
+        break;
+      }
+    }
+    /* star the primed zero */
+    newStarMatrix[primeRow + nOfRows * primeCol] = true;
+    /* find starred zero in current column */
+    starCol = primeCol;
+    for (starRow = 0; starRow < nOfRows; starRow++)
+    {
+      if (starMatrix[starRow + nOfRows * starCol])
+      {
+        break;
+      }
+    }
+  }
+  /* use temporary copy as new starMatrix */
+  /* delete all primes, uncover all rows */
+  for (size_t n = 0; n < nOfElements; n++)
+  {
+    primeMatrix[n] = false;
+    starMatrix[n] = newStarMatrix[n];
+  }
+  for (size_t n = 0; n < nOfRows; n++)
+  {
+    coveredRows[n] = false;
+  }
+  /* move to step 2a */
+  step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+         nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                    size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+  /* find smallest uncovered element h */
+  float h = std::numeric_limits<track_t>::max();
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    if (!coveredRows[row])
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        if (!coveredColumns[col])
+        {
+          const float value = distMatrix[row + nOfRows * col];
+          if (value < h)
+          {
+            h = value;
+          }
+        }
+      }
+    }
+  }
+  /* add h to each covered row */
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    if (coveredRows[row])
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        distMatrix[row + nOfRows * col] += h;
+      }
+    }
+  }
+  /* subtract h from each uncovered column */
+  for (size_t col = 0; col < nOfColumns; col++)
+  {
+    if (!coveredColumns[col])
+    {
+      for (size_t row = 0; row < nOfRows; row++)
+      {
+        distMatrix[row + nOfRows * col] -= h;
+      }
+    }
+  }
+  /* move to step 3 */
+  step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+        nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+// Computes a suboptimal solution. Good for cases without forbidden assignments.
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost,
+                                                    const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
+{
+  /* make working copy of distance Matrix */
+  const size_t nOfElements = nOfRows * nOfColumns;
+  float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
+  for (size_t n = 0; n < nOfElements; n++)
+  {
+    distMatrix[n] = distMatrixIn[n];
+  }
+
+  /* recursively search for the minimum element and do the assignment */
+  for (;;)
+  {
+    /* find minimum distance observation-to-track pair */
+    float minValue = std::numeric_limits<track_t>::max();
+    size_t tmpRow = 0;
+    size_t tmpCol = 0;
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        const float value = distMatrix[row + nOfRows * col];
+        if (value != std::numeric_limits<track_t>::max() && (value < minValue))
+        {
+          minValue = value;
+          tmpRow = row;
+          tmpCol = col;
+        }
+      }
+    }
+
+    if (minValue != std::numeric_limits<track_t>::max())
+    {
+      assignment[tmpRow] = static_cast<int>(tmpCol);
+      cost += minValue;
+      for (size_t n = 0; n < nOfRows; n++)
+      {
+        distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
+      }
+      for (size_t n = 0; n < nOfColumns; n++)
+      {
+        distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
+      }
+    }
+    else
+    {
+      break;
+    }
+  }
+
+  free(distMatrix);
+}
+// --------------------------------------------------------------------------
+// Computes a suboptimal solution. Good for cases with many forbidden assignments.
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost,
+                                                    const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
+{
+  /* make working copy of distance Matrix */
+  const size_t nOfElements = nOfRows * nOfColumns;
+  float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
+  for (size_t n = 0; n < nOfElements; n++)
+  {
+    distMatrix[n] = distMatrixIn[n];
+  }
+
+  /* allocate memory */
+  int* nOfValidObservations = (int*)calloc(nOfRows, sizeof(int));
+  int* nOfValidTracks = (int*)calloc(nOfColumns, sizeof(int));
+
+  /* compute number of validations */
+  bool infiniteValueFound = false;
+  bool finiteValueFound = false;
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
+      {
+        nOfValidTracks[col] += 1;
+        nOfValidObservations[row] += 1;
+        finiteValueFound = true;
+      }
+      else
+      {
+        infiniteValueFound = true;
+      }
+    }
+  }
+
+  if (infiniteValueFound)
+  {
+    if (!finiteValueFound)
+    {
+      free(nOfValidTracks);
+      free(nOfValidObservations);
+      free(distMatrix);
+      return;
+    }
+    bool repeatSteps = true;
+
+    while (repeatSteps)
+    {
+      repeatSteps = false;
+
+      /* step 1: reject assignments of multiply validated tracks to singly validated observations		 */
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        bool singleValidationFound = false;
+        for (size_t row = 0; row < nOfRows; row++)
+        {
+          if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() &&
+              (nOfValidObservations[row] == 1))
+          {
+            singleValidationFound = true;
+            break;
+          }
+
+          if (singleValidationFound)
+          {
+            for (size_t row = 0; row < nOfRows; row++)
+              if ((nOfValidObservations[row] > 1) &&
+                  distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
+              {
+                distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
+                nOfValidObservations[row] -= 1;
+                nOfValidTracks[col] -= 1;
+                repeatSteps = true;
+              }
+          }
+        }
+      }
+
+      /* step 2: reject assignments of multiply validated observations to singly validated tracks */
+      if (nOfColumns > 1)
+      {
+        for (size_t row = 0; row < nOfRows; row++)
+        {
+          bool singleValidationFound = false;
+          for (size_t col = 0; col < nOfColumns; col++)
+          {
+            if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() && (nOfValidTracks[col] == 1))
+            {
+              singleValidationFound = true;
+              break;
+            }
+          }
+
+          if (singleValidationFound)
+          {
+            for (size_t col = 0; col < nOfColumns; col++)
+            {
+              if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
+              {
+                distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
+                nOfValidObservations[row] -= 1;
+                nOfValidTracks[col] -= 1;
+                repeatSteps = true;
+              }
+            }
+          }
+        }
+      }
+    } /* while(repeatSteps) */
+
+    /* for each multiply validated track that validates only with singly validated  */
+    /* observations, choose the observation with minimum distance */
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      if (nOfValidObservations[row] > 1)
+      {
+        bool allSinglyValidated = true;
+        float minValue = std::numeric_limits<track_t>::max();
+        size_t tmpCol = 0;
+        for (size_t col = 0; col < nOfColumns; col++)
+        {
+          const float value = distMatrix[row + nOfRows * col];
+          if (value != std::numeric_limits<track_t>::max())
+          {
+            if (nOfValidTracks[col] > 1)
+            {
+              allSinglyValidated = false;
+              break;
+            }
+            else if ((nOfValidTracks[col] == 1) && (value < minValue))
+            {
+              tmpCol = col;
+              minValue = value;
+            }
+          }
+        }
+
+        if (allSinglyValidated)
+        {
+          assignment[row] = static_cast<int>(tmpCol);
+          cost += minValue;
+          for (size_t n = 0; n < nOfRows; n++)
+          {
+            distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
+          }
+          for (size_t n = 0; n < nOfColumns; n++)
+          {
+            distMatrix[row + nOfRows * n] = std::numeric_limits<track_t>::max();
+          }
+        }
+      }
+    }
+
+    // for each multiply validated observation that validates only with singly validated  track, choose the track with
+    // minimum distance
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      if (nOfValidTracks[col] > 1)
+      {
+        bool allSinglyValidated = true;
+        float minValue = std::numeric_limits<track_t>::max();
+        size_t tmpRow = 0;
+        for (size_t row = 0; row < nOfRows; row++)
+        {
+          const float value = distMatrix[row + nOfRows * col];
+          if (value != std::numeric_limits<track_t>::max())
+          {
+            if (nOfValidObservations[row] > 1)
+            {
+              allSinglyValidated = false;
+              break;
+            }
+            else if ((nOfValidObservations[row] == 1) && (value < minValue))
+            {
+              tmpRow = row;
+              minValue = value;
+            }
+          }
+        }
+
+        if (allSinglyValidated)
+        {
+          assignment[tmpRow] = static_cast<int>(col);
+          cost += minValue;
+          for (size_t n = 0; n < nOfRows; n++)
+          {
+            distMatrix[n + nOfRows * col] = std::numeric_limits<track_t>::max();
+          }
+          for (size_t n = 0; n < nOfColumns; n++)
+          {
+            distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
+          }
+        }
+      }
+    }
+  } /* if(infiniteValueFound) */
+
+  /* now, recursively search for the minimum element and do the assignment */
+  for (;;)
+  {
+    /* find minimum distance observation-to-track pair */
+    float minValue = std::numeric_limits<track_t>::max();
+    size_t tmpRow = 0;
+    size_t tmpCol = 0;
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        const float value = distMatrix[row + nOfRows * col];
+        if (value != std::numeric_limits<track_t>::max() && (value < minValue))
+        {
+          minValue = value;
+          tmpRow = row;
+          tmpCol = col;
+        }
+      }
+    }
+
+    if (minValue != std::numeric_limits<track_t>::max())
+    {
+      assignment[tmpRow] = static_cast<int>(tmpCol);
+      cost += minValue;
+      for (size_t n = 0; n < nOfRows; n++)
+      {
+        distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
+      }
+      for (size_t n = 0; n < nOfColumns; n++)
+      {
+        distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
+      }
+    }
+    else
+    {
+      break;
+    }
+  }
+
+  /* free allocated memory */
+  free(nOfValidObservations);
+  free(nOfValidTracks);
+  free(distMatrix);
+}

navigations/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp

@@ -0,0 +1,99 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h>
+#include "opencv2/opencv.hpp"
+#include <iostream>
+#include <vector>
+
+//---------------------------------------------------------------------------
+//---------------------------------------------------------------------------
+TKalmanFilter::TKalmanFilter(Point_t pt, track_t deltatime)
+{
+  // time increment (lower values makes target more "massive")
+  dt = deltatime;
+
+  // 6 state variables [x y z xdot ydot zdot], 3 measurements [x y z]
+  kalman = new cv::KalmanFilter(6, 3, 0);
+  // Transition cv::Matrix
+  kalman->transitionMatrix = (cv::Mat_<track_t>(6, 6) <<
+                              1, 0, 0, dt,  0,  0,
+                              0, 1, 0,  0, dt,  0,
+                              0, 0, 1,  0,  0, dt,
+                              0, 0, 0,  1,  0,  0,
+                              0, 0, 0,  0,  1,  0,
+                              0, 0, 0,  0,  0,  1);
+  // init...
+  LastPosition = pt;
+  kalman->statePre.at<track_t>(0) = pt.x;
+  kalman->statePre.at<track_t>(1) = pt.y;
+  kalman->statePre.at<track_t>(2) = pt.z;
+
+  kalman->statePre.at<track_t>(3) = 0;
+  kalman->statePre.at<track_t>(4) = 0;
+  kalman->statePre.at<track_t>(5) = 0;
+
+  kalman->statePost.at<track_t>(0) = pt.x;
+  kalman->statePost.at<track_t>(1) = pt.y;
+  kalman->statePost.at<track_t>(2) = pt.z;
+
+  // Nur x, y und z Koordinaten messbar!
+  kalman->measurementMatrix = (cv::Mat_<track_t>(3, 6) <<
+                               1, 0, 0, 0, 0, 0,
+                               0, 1, 0, 0, 0, 0,
+                               0, 0, 1, 0, 0, 0);
+
+  float sigma = 0.5; // Assume standard deviation for acceleration, todo: dynamic reconfigure
+  float c1 = pow(dt,4.0)/4.0;
+  float c2 = pow(dt,2.0);
+  float c3 = pow(dt,3.0)/2.0;
+  kalman->processNoiseCov = sigma*sigma*(cv::Mat_<float>(6, 6) <<
+                             c1,  0,  0, c3,  0,  0,
+                              0, c1,  0,  0, c3,  0,
+                              0,  0, c1,  0,  0, c3,
+                             c3,  0,  0, c2,  0,  0,
+                              0, c3,  0,  0, c2,  0,
+                              0,  0, c3,  0,  0, c2);
+
+  cv::setIdentity(kalman->measurementNoiseCov, cv::Scalar::all(5e-2));
+
+  cv::setIdentity(kalman->errorCovPost, cv::Scalar::all(1000000));
+}
+//---------------------------------------------------------------------------
+TKalmanFilter::~TKalmanFilter() { delete kalman; }
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::Prediction()
+{
+  cv::Mat prediction = kalman->predict();
+  LastPosition = Point_t(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
+  LastVelocity = Point_t(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5));
+}
+
+//---------------------------------------------------------------------------
+Point_t TKalmanFilter::Update(Point_t p, bool DataCorrect)
+{
+  cv::Mat measurement(3, 1, Mat_t(1));
+  if (!DataCorrect)
+  {
+    measurement.at<track_t>(0) = LastPosition.x; // update using prediction
+    measurement.at<track_t>(1) = LastPosition.y;
+    measurement.at<track_t>(2) = LastPosition.z;
+  }
+  else
+  {
+    measurement.at<track_t>(0) = p.x; // update using measurements
+    measurement.at<track_t>(1) = p.y;
+    measurement.at<track_t>(2) = p.z;
+  }
+  // Correction
+  cv::Mat estimated = kalman->correct(measurement);
+  LastPosition.x = estimated.at<track_t>(0); // update using measurements
+  LastPosition.y = estimated.at<track_t>(1);
+  LastPosition.z = estimated.at<track_t>(2);
+  LastVelocity.x = estimated.at<track_t>(3);
+  LastVelocity.y = estimated.at<track_t>(4);
+  LastVelocity.z = estimated.at<track_t>(5);
+  return LastPosition;
+}
+//---------------------------------------------------------------------------

navigations/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md

@@ -0,0 +1,13 @@
+Multitarget Tracker
+===================
+
+This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].
+It is utilized for the *CostmapToDynamicObstacles* plugin.
+
+The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].
+The package itself depends on other third party packages with different licenses (refer to the package repository).
+
+TODO: Include the whole package as external CMake project.
+
+
+

navigations/costmap_converter/src/costmap_to_lines_convex_hull.cpp

@@ -0,0 +1,289 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#include <costmap_converter/costmap_to_lines_convex_hull.h>
+#include <costmap_converter/misc.h>
+#include <boost/thread.hpp>
+#include <boost/thread/mutex.hpp>
+#include <pluginlib/class_list_macros.h>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::BaseCostmapToPolygons)
+
+namespace costmap_converter
+{
+
+CostmapToLinesDBSMCCH::CostmapToLinesDBSMCCH() : CostmapToPolygonsDBSMCCH() 
+{
+    dynamic_recfg_ = NULL;
+}
+  
+CostmapToLinesDBSMCCH::~CostmapToLinesDBSMCCH() 
+{
+  if (dynamic_recfg_ != NULL)
+    delete dynamic_recfg_;
+}
+  
+void CostmapToLinesDBSMCCH::initialize(ros::NodeHandle nh)
+{ 
+    // DB SCAN
+    nh.param("cluster_max_distance", parameter_.max_distance_, 0.4);
+    nh.param("cluster_min_pts", parameter_.min_pts_, 2);
+    nh.param("cluster_max_pts", parameter_.max_pts_, 30);
+    // convex hull
+    nh.param("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, 0.1);
+    parameter_buffered_ = parameter_;
+    
+    // Line extraction
+    nh.param("support_pts_max_dist", support_pts_max_dist_, 0.3);
+    nh.param("support_pts_max_dist_inbetween", support_pts_max_dist_inbetween_, 1.0);
+    nh.param("min_support_pts", min_support_pts_, 2);
+    
+    // setup dynamic reconfigure
+    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>(nh);
+    dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSMCCH::reconfigureCB, this, _1, _2);
+    dynamic_recfg_->setCallback(cb);
+    
+    // deprecated
+    if (nh.hasParam("support_pts_min_dist_") || nh.hasParam("support_pts_min_dist"))
+      ROS_WARN("CostmapToLinesDBSMCCH: Parameter 'support_pts_min_dist' is deprecated and not included anymore.");
+    if (nh.hasParam("min_support_pts_"))
+      ROS_WARN("CostmapToLinesDBSMCCH: Parameter 'min_support_pts_' is not found. Remove the underscore.");
+}  
+  
+void CostmapToLinesDBSMCCH::compute()
+{
+    std::vector< std::vector<KeyPoint> > clusters;
+    dbScan(clusters);
+    
+    // Create new polygon container
+    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());  
+    
+    
+    // add convex hulls to polygon container
+    for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
+    {
+      geometry_msgs::Polygon polygon;
+      convexHull2(clusters[i], polygon );
+      
+      // now extract lines of the polygon (by searching for support points in the cluster)  
+      // and add them to the polygon container
+      extractPointsAndLines(clusters[i], polygon, std::back_inserter(*polygons));
+    }
+    
+    // add our non-cluster points to the polygon container (as single points)
+    if (!clusters.empty())
+    {
+      for (int i=0; i < clusters.front().size(); ++i)
+      {
+        polygons->push_back( geometry_msgs::Polygon() );
+        convertPointToPolygon(clusters.front()[i], polygons->back());
+      }
+    }
+        
+    // replace shared polygon container
+    updatePolygonContainer(polygons);
+}
+
+typedef CostmapToLinesDBSMCCH CL;
+bool sort_keypoint_x(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) 
+{ return (cluster[i].x<cluster[j].x) || (cluster[i].x == cluster[j].x && cluster[i].y < cluster[j].y); }
+bool sort_keypoint_y(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) 
+{ return (cluster[i].y<cluster[j].y) || (cluster[i].y == cluster[j].y && cluster[i].x < cluster[j].x); }
+
+void CostmapToLinesDBSMCCH::extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::Polygon& polygon,
+                                                  std::back_insert_iterator< std::vector<geometry_msgs::Polygon> > lines)
+{
+   if (polygon.points.empty())
+     return;
+  
+   if (polygon.points.size() < 2)
+   {
+     lines = polygon; // our polygon is just a point, push back onto the output sequence
+     return;
+   }
+   int n = (int)polygon.points.size();
+     
+   for (int i=1; i<n; ++i) // this implemenation requires an explicitly closed polygon (start vertex = end vertex)
+   {
+        const geometry_msgs::Point32* vertex1 = &polygon.points[i-1];
+        const geometry_msgs::Point32* vertex2 = &polygon.points[i];
+
+        // check how many vertices belong to the line (sometimes the convex hull algorithm finds multiple vertices on a line,
+        // in case of small coordinate deviations)
+        double dx = vertex2->x - vertex1->x;
+        double dy = vertex2->y - vertex1->y;
+//         double norm = std::sqrt(dx*dx + dy*dy);
+//         dx /= norm;
+//         dy /= norm;
+//         for (int j=i; j<(int)polygon.points.size() - 2; ++j)
+//         {
+//           const geometry_msgs::Point32* vertex_jp2 = &polygon.points[j+2];
+//           double dx2 = vertex_jp2->x - vertex2->x;
+//           double dy2 = vertex_jp2->y - vertex2->y;
+//           double norm2 = std::sqrt(dx2*dx2 + dy2*dy2);
+//           dx2 /= norm2;
+//           dy2 /= norm2;
+//           if (std::abs(dx*dx2 + dy*dy2) < 0.05) //~3 degree
+//           {
+//             vertex2 = &polygon.points[j+2];
+//             i = j; // DO NOT USE "i" afterwards
+//           }
+//           else break;
+//         }
+       
+        //Search support points
+        std::vector<std::size_t> support_points; // store indices of cluster
+        
+        for (std::size_t k = 0; k < cluster.size(); ++k)
+        {
+            bool is_inbetween = false;
+            double dist_line_to_point = computeDistanceToLineSegment( cluster[k], *vertex1, *vertex2, &is_inbetween );
+
+            if(is_inbetween && dist_line_to_point <= support_pts_max_dist_)
+            {
+              support_points.push_back(k);
+              continue;
+            }
+        }
+
+        // now check if the inlier models a line by checking the minimum distance between all support points (avoid lines over gaps)
+        // and by checking if the minium number of points is reached // deactivate by setting support_pts_max_dist_inbetween_==0
+        bool is_line=true;
+        if (support_pts_max_dist_inbetween_!=0)
+        {
+          if ((int)support_points.size() >= min_support_pts_ + 2) // +2 since start and goal are included
+          {          
+            // sort points
+            if (std::abs(dx) >= std::abs(dy))
+              std::sort(support_points.begin(), support_points.end(), boost::bind(sort_keypoint_x, _1, _2, boost::cref(cluster)));
+            else 
+              std::sort(support_points.begin(), support_points.end(), boost::bind(sort_keypoint_y, _1, _2, boost::cref(cluster)));
+            
+            // now calculate distances
+            for (int k = 1; k < int(support_points.size()); ++k)
+            {
+              double dist_x = cluster[support_points[k]].x - cluster[support_points[k-1]].x;
+              double dist_y = cluster[support_points[k]].y - cluster[support_points[k-1]].y;
+              double dist = std::sqrt( dist_x*dist_x + dist_y*dist_y);
+              if (dist > support_pts_max_dist_inbetween_)
+              {
+                is_line = false;
+                break;
+              }
+            }
+            
+          }
+          else
+            is_line = false;
+        }
+        
+        if (is_line)
+        {
+          // line found:
+          geometry_msgs::Polygon line;
+          line.points.push_back(*vertex1);
+          line.points.push_back(*vertex2);
+          lines = line; // back insertion
+          
+          // remove inlier from list
+          // iterate in reverse order, otherwise indices are not valid after erasing elements
+          std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
+          for (; support_it != support_points.rend(); ++support_it)
+          {
+            cluster.erase(cluster.begin() + *support_it);
+          }
+        }
+        else
+        {
+            // remove goal, since it will be added in the subsequent iteration
+            //support_points.pop_back();
+          // old:
+//             // add vertex 1 as single point
+//             geometry_msgs::Polygon point;
+//             point.points.push_back(*vertex1);
+//             lines = point; // back insertion
+
+           // add complete inlier set as points 
+//            for (int k = 0; k < int(support_points.size()); ++k)
+//            {
+//               geometry_msgs::Polygon polygon;
+//               convertPointToPolygon(cluster[support_points[k]], polygon);
+//               lines = polygon; // back insertion
+//            }
+           
+          // remove inlier from list and add them as point obstacles
+          // iterate in reverse order, otherwise indices are not valid after erasing elements
+          std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
+          for (; support_it != support_points.rend(); ++support_it)
+          {
+            geometry_msgs::Polygon polygon;
+            convertPointToPolygon(cluster[*support_it], polygon);
+            lines = polygon; // back insertion
+            
+            cluster.erase(cluster.begin() + *support_it);
+          }
+        }
+    }
+ 
+    // add all remaining cluster points, that do not belong to a line
+    for (int i=0; i<(int)cluster.size();++i)
+    {
+        geometry_msgs::Polygon polygon;
+        convertPointToPolygon(cluster[i], polygon);
+        lines = polygon; // back insertion
+    }
+
+}
+
+void CostmapToLinesDBSMCCH::reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level)
+{
+    boost::mutex::scoped_lock lock(parameter_mutex_);
+    parameter_buffered_.max_distance_ = config.cluster_max_distance;
+    parameter_buffered_.min_pts_ = config.cluster_min_pts;
+    parameter_buffered_.max_pts_ = config.cluster_max_pts;
+    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
+    support_pts_max_dist_ = config.support_pts_max_dist;
+    support_pts_max_dist_inbetween_ = config.support_pts_max_dist_inbetween;
+    min_support_pts_ = config.min_support_pts;
+}
+
+
+
+}//end namespace costmap_converter
+
+

navigations/costmap_converter/src/costmap_to_lines_ransac.cpp

@@ -0,0 +1,342 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include <costmap_converter/costmap_to_lines_ransac.h>
+#include <boost/thread.hpp>
+#include <boost/thread/mutex.hpp>
+#include <pluginlib/class_list_macros.h>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::BaseCostmapToPolygons)
+
+namespace costmap_converter
+{
+
+CostmapToLinesDBSRANSAC::CostmapToLinesDBSRANSAC() : CostmapToPolygonsDBSMCCH() 
+{
+  dynamic_recfg_ = NULL;
+}
+
+CostmapToLinesDBSRANSAC::~CostmapToLinesDBSRANSAC() 
+{
+  if (dynamic_recfg_ != NULL)
+    delete dynamic_recfg_;
+}
+  
+void CostmapToLinesDBSRANSAC::initialize(ros::NodeHandle nh)
+{ 
+    // DB SCAN
+    nh.param("cluster_max_distance", parameter_.max_distance_, 0.4);
+    nh.param("cluster_min_pts", parameter_.min_pts_, 2);
+    nh.param("cluster_max_pts", parameter_.max_pts_, 30);
+    // convex hull (only necessary if outlier filtering is enabled)
+    nh.param("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, 0.1);
+    parameter_buffered_ = parameter_;
+
+    // ransac
+    nh.param("ransac_inlier_distance", ransac_inlier_distance_, 0.2);
+    nh.param("ransac_min_inliers", ransac_min_inliers_, 10);
+    nh.param("ransac_no_iterations", ransac_no_iterations_, 2000);
+    nh.param("ransac_remainig_outliers", ransac_remainig_outliers_, 3);
+    nh.param("ransac_convert_outlier_pts", ransac_convert_outlier_pts_, true);
+    nh.param("ransac_filter_remaining_outlier_pts", ransac_filter_remaining_outlier_pts_, false);
+    
+    // setup dynamic reconfigure
+    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>(nh);
+    dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSRANSAC::reconfigureCB, this, _1, _2);
+    dynamic_recfg_->setCallback(cb);
+}  
+  
+void CostmapToLinesDBSRANSAC::compute()
+{
+    std::vector< std::vector<KeyPoint> > clusters;
+    dbScan(clusters);
+    
+    // Create new polygon container
+    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());  
+    
+    
+    // fit lines using ransac for each cluster
+    for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
+    { 
+      
+      while (clusters[i].size() > ransac_remainig_outliers_)
+      {
+//         std::vector<KeyPoint> inliers;
+        std::vector<KeyPoint> outliers;
+        std::pair<KeyPoint,KeyPoint> model;
+        if (!lineRansac(clusters[i], ransac_inlier_distance_, ransac_no_iterations_, ransac_min_inliers_, model, NULL, &outliers ) )
+          break;
+        
+        // add to polygon container
+        geometry_msgs::Polygon line;
+        line.points.resize(2);
+        model.first.toPointMsg(line.points.front());
+        model.second.toPointMsg(line.points.back());
+        polygons->push_back(line);
+        
+        clusters[i] = outliers;
+      }
+      // create point polygons for remaining outliers
+      if (ransac_convert_outlier_pts_)
+      {
+        if (ransac_filter_remaining_outlier_pts_)
+        {
+          // take edge points of a convex polygon
+          // these points define a cluster and since all lines are extracted,
+          // we remove points from the interior...
+          geometry_msgs::Polygon polygon;
+          convexHull2(clusters[i], polygon);
+          for (int j=0; j < (int)polygon.points.size(); ++j)
+          {
+            polygons->push_back(geometry_msgs::Polygon());
+            convertPointToPolygon(polygon.points[j], polygons->back());
+          }
+        }
+        else
+        {
+          for (int j = 0; j < (int)clusters[i].size(); ++j)
+          {
+            polygons->push_back(geometry_msgs::Polygon());
+            convertPointToPolygon(clusters[i][j], polygons->back());           
+          }
+        }
+
+      }
+      
+      
+    }
+    
+    // add our non-cluster points to the polygon container (as single points)
+    if (!clusters.empty())
+    {
+      for (int i=0; i < clusters.front().size(); ++i)
+      {
+        polygons->push_back( geometry_msgs::Polygon() );
+        convertPointToPolygon(clusters.front()[i], polygons->back());
+      }
+    }
+        
+    // replace shared polygon container
+    updatePolygonContainer(polygons);
+}
+
+
+bool CostmapToLinesDBSRANSAC::lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations,
+                                         int min_inliers, std::pair<KeyPoint, KeyPoint>& best_model, 
+                                         std::vector<KeyPoint>* inliers, std::vector<KeyPoint>* outliers)
+{
+  if (data.size() < 2 ||  data.size() < min_inliers)
+  {
+    return false;
+  }
+  
+  boost::random::uniform_int_distribution<> distribution(0, data.size()-1);
+  
+  std::pair<int, int> best_model_idx;
+  int best_no_inliers = -1;
+  
+  
+  for (int i=0; i < no_iterations; ++i)
+  {
+    // choose random points to define a line candidate
+    int start_idx = distribution(rnd_generator_);
+    int end_idx = start_idx;
+    while (end_idx == start_idx)
+      end_idx = distribution(rnd_generator_);
+    
+    
+    // compute inliers
+    int no_inliers = 0;
+    for (int j=0; j<(int)data.size(); ++j)
+    {
+      if ( isInlier(data[j], data[start_idx], data[end_idx], inlier_distance) )
+        ++no_inliers;
+    }
+    
+    if (no_inliers > best_no_inliers)
+    {
+      best_no_inliers = no_inliers;
+      best_model_idx.first = start_idx;
+      best_model_idx.second = end_idx;
+    }
+  }
+  
+  best_model.first = data[best_model_idx.first];
+  best_model.second = data[best_model_idx.second];
+  
+  if (best_no_inliers < min_inliers)
+    return false;
+  
+  // Now repeat the calculation for the best model in order to obtain the inliers and outliers set
+  // This might be faster if no_iterations is large, but TEST
+  if (inliers || outliers)
+  {
+    if (inliers)
+      inliers->clear();
+    if (outliers)
+      outliers->clear();
+    
+    int no_inliers = 0;
+    for (int i=0; i<(int)data.size(); ++i)
+    {
+        if ( isInlier( data[i], best_model.first, best_model.second, inlier_distance ) )
+        {
+          if (inliers)
+            inliers->push_back( data[i] );
+        }
+        else
+        {
+          if (outliers)
+            outliers->push_back( data[i] );
+        }
+    }
+  }
+  
+  return true;
+}
+
+bool CostmapToLinesDBSRANSAC::linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept, 
+                                               double* mean_x_out, double* mean_y_out)
+{
+  if (data.size() < 2)
+  {
+    ROS_ERROR("CostmapToLinesDBSRANSAC: at least 2 data points required for linear regression");
+    return false;
+  }
+  
+  double mean_x = 0;
+  double mean_y = 0;
+  for (int i=0; i<(int)data.size(); ++i)
+  {
+    mean_x += data[i].x;
+    mean_y += data[i].y;
+  }
+  mean_x /= double(data.size());
+  mean_y /= double(data.size());
+  
+  if (mean_x_out)
+    *mean_x_out = mean_x;
+ 
+  if (mean_y_out)
+    *mean_y_out = mean_y;
+  
+  double numerator = 0.0;
+  double denominator = 0.0;
+
+  for(int i=0; i<(int)data.size(); ++i)
+  {
+      double dx = data[i].x - mean_x;
+      numerator += dx * (data[i].y - mean_y);
+      denominator += dx*dx;
+  }
+  
+  if (denominator == 0)
+  {
+    ROS_ERROR("CostmapToLinesDBSRANSAC: linear regression failed, denominator 0");
+    return false;
+  }
+  else
+    slope = numerator / denominator;
+  
+  intercept = mean_y - slope * mean_x;
+  return true;
+}
+
+
+void CostmapToLinesDBSRANSAC::reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level)
+{
+    boost::mutex::scoped_lock lock(parameter_mutex_);
+    parameter_buffered_.max_distance_ = config.cluster_max_distance;
+    parameter_buffered_.min_pts_ = config.cluster_min_pts;
+    parameter_buffered_.max_pts_ = config.cluster_max_pts;
+    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
+    ransac_inlier_distance_ = config.ransac_inlier_distance;
+    ransac_min_inliers_ = config.ransac_min_inliers;
+    ransac_no_iterations_ = config.ransac_no_iterations;
+    ransac_remainig_outliers_ = config.ransac_remainig_outliers;
+    ransac_convert_outlier_pts_ = config.ransac_convert_outlier_pts;
+    ransac_filter_remaining_outlier_pts_ = config.ransac_filter_remaining_outlier_pts;
+}
+
+
+/*
+void CostmapToLinesDBSRANSAC::adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2, 
+                                               KeyPoint& line_start, KeyPoint& line_end)
+{
+  line_start = linept1; 
+  line_end = linept2;
+
+  // infinite line is defined by linept1 and linept2
+  double dir_x = line_end.x - line_start.x;
+  double dir_y = line_end.y - line_start.y;
+  double norm = std::sqrt(dir_x*dir_x + dir_y*dir_y);
+  dir_x /= norm;
+  dir_y /= norm;
+  
+  // project data onto the line and check if the distance is increased in both directions
+  for (int i=0; i < (int) data.size(); ++i)
+  {
+    double dx = data[i].x - line_start.x;
+    double dy = data[i].y - line_start.y;
+    // check scalar product at start
+    double extension = dx*dir_x + dy*dir_y;
+    if (extension<0)
+    {
+      line_start.x -=  dir_x*extension;
+      line_start.y -=  dir_y*extension;
+    }
+    else
+    {
+      dx = data[i].x - line_end.x;
+      dy = data[i].y - line_end.y;
+      // check scalar product at end
+      double extension = dx*dir_x + dy*dir_y;
+      if (extension>0)
+      {
+        line_end.x +=  dir_x*extension;
+        line_end.y +=  dir_y*extension;
+      }
+    }
+  }
+  
+}*/
+
+
+}//end namespace costmap_converter
+
+

navigations/costmap_converter/src/costmap_to_polygons.cpp

@@ -0,0 +1,503 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#include <costmap_converter/costmap_to_polygons.h>
+#include <costmap_converter/misc.h>
+#include <boost/thread.hpp>
+#include <boost/thread/mutex.hpp>
+#include <pluginlib/class_list_macros.h>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons)
+
+namespace
+{
+
+/**
+ * @brief Douglas-Peucker Algorithm for fitting lines into ordered set of points
+ * 
+ * Douglas-Peucker Algorithm, see https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
+ * 
+ * @param begin iterator pointing to the begin of the range of points
+ * @param end interator pointing to the end of the range of points
+ * @param epsilon distance criteria for removing points if it is closer to the line segment than this
+ * @param result the simplified polygon
+ */
+std::vector<geometry_msgs::Point32> douglasPeucker(std::vector<geometry_msgs::Point32>::iterator begin,
+  std::vector<geometry_msgs::Point32>::iterator end, double epsilon)
+{
+  if (std::distance(begin, end) <= 2)
+  {
+    return std::vector<geometry_msgs::Point32>(begin, end);
+  }
+
+  // Find the point with the maximum distance from the line [begin, end)
+  double dmax = std::numeric_limits<double>::lowest();
+  std::vector<geometry_msgs::Point32>::iterator max_dist_it;
+  std::vector<geometry_msgs::Point32>::iterator last = std::prev(end);
+  for (auto it = std::next(begin); it != last; ++it)
+  {
+    double d = costmap_converter::computeSquaredDistanceToLineSegment(*it, *begin, *last);
+    if (d > dmax)
+    {
+      max_dist_it = it;
+      dmax = d;
+    }
+  }
+
+  if (dmax < epsilon * epsilon)
+  { // termination criterion reached, line is good enough
+    std::vector<geometry_msgs::Point32> result;
+    result.push_back(*begin);
+    result.push_back(*last);
+    return result;
+  }
+
+  // Recursive calls for the two splitted parts
+  auto firstLineSimplified = douglasPeucker(begin, std::next(max_dist_it), epsilon);
+  auto secondLineSimplified = douglasPeucker(max_dist_it, end, epsilon);
+
+  // Combine the two lines into one line and return the merged line.
+  // Note that we have to skip the first point of the second line, as it is duplicated above.
+  firstLineSimplified.insert(firstLineSimplified.end(),
+    std::make_move_iterator(std::next(secondLineSimplified.begin())),
+    std::make_move_iterator(secondLineSimplified.end()));
+  return firstLineSimplified;
+}
+
+} // end namespace
+
+namespace costmap_converter
+{
+    
+CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons()
+{
+  costmap_ = NULL;
+  dynamic_recfg_ = NULL;
+  neighbor_size_x_ = neighbor_size_y_ = -1;
+  offset_x_ = offset_y_ = 0.;
+}
+
+CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH() 
+{
+  if (dynamic_recfg_ != NULL)
+    delete dynamic_recfg_;
+}
+
+void CostmapToPolygonsDBSMCCH::initialize(ros::NodeHandle nh)
+{
+    costmap_ = NULL;
+   
+    nh.param("cluster_max_distance", parameter_.max_distance_, 0.4);
+    nh.param("cluster_min_pts", parameter_.min_pts_, 2);
+    nh.param("cluster_max_pts", parameter_.max_pts_, 30);
+    nh.param("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, 0.1);
+    
+    parameter_buffered_ = parameter_;
+    
+    // setup dynamic reconfigure
+    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>(nh);
+    dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2);
+    dynamic_recfg_->setCallback(cb);
+}
+
+
+void CostmapToPolygonsDBSMCCH::compute()
+{
+    std::vector< std::vector<KeyPoint> > clusters;
+    dbScan(clusters);
+    
+    // Create new polygon container
+    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());
+    
+    
+    // add convex hulls to polygon container
+    for (std::size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
+    {
+      polygons->push_back( geometry_msgs::Polygon() );
+      convexHull2(clusters[i], polygons->back() );
+    }
+    
+    // add our non-cluster points to the polygon container (as single points)
+    if (!clusters.empty())
+    {
+      for (std::size_t i=0; i < clusters.front().size(); ++i)
+      {
+        polygons->push_back( geometry_msgs::Polygon() );
+        convertPointToPolygon(clusters.front()[i], polygons->back());
+      }
+    }
+    
+    // replace shared polygon container
+    updatePolygonContainer(polygons);
+}
+
+void CostmapToPolygonsDBSMCCH::setCostmap2D(costmap_2d::Costmap2D *costmap)
+{
+    if (!costmap)
+      return;
+
+    costmap_ = costmap;     
+    
+    updateCostmap2D();
+}
+
+void CostmapToPolygonsDBSMCCH::updateCostmap2D()
+{
+      occupied_cells_.clear();
+      
+      if (!costmap_->getMutex())
+      {
+        ROS_ERROR("Cannot update costmap since the mutex pointer is null");
+        return;
+      }
+      
+      { // get a copy of our parameters from dynamic reconfigure
+        boost::mutex::scoped_lock lock(parameter_mutex_);
+        parameter_ = parameter_buffered_;
+      }
+      
+      costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
+
+      // allocate neighbor lookup
+      int cells_x = int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1;
+      int cells_y = int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1;
+
+      if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_) {
+        neighbor_size_x_ = cells_x;
+        neighbor_size_y_ = cells_y;
+        neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_);
+      }
+      offset_x_ = costmap_->getOriginX();
+      offset_y_ = costmap_->getOriginY();
+      for (auto& n : neighbor_lookup_)
+        n.clear();
+
+      // get indices of obstacle cells
+      for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
+      {
+        for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
+        {
+          int value = costmap_->getCost(i,j);
+          if(value >= costmap_2d::LETHAL_OBSTACLE)
+          {
+            double x, y;
+            costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
+            addPoint(x, y);
+          }
+        }
+      }
+}
+
+
+void CostmapToPolygonsDBSMCCH::dbScan(std::vector< std::vector<KeyPoint> >& clusters)
+{
+  std::vector<bool> visited(occupied_cells_.size(), false);
+
+  clusters.clear();  
+  
+  //DB Scan Algorithm
+  int cluster_id = 0; // current cluster_id
+  clusters.push_back(std::vector<KeyPoint>());
+  for(int i = 0; i< (int)occupied_cells_.size(); i++)
+  {
+    if(!visited[i]) //keypoint has not been visited before
+    {
+      visited[i] = true; // mark as visited
+      std::vector<int> neighbors;
+      regionQuery(i, neighbors); //Find neighbors around the keypoint
+      if((int)neighbors.size() < parameter_.min_pts_) //If not enough neighbors are found, mark as noise
+      {		
+        clusters[0].push_back(occupied_cells_[i]);
+      }
+      else
+      {
+        ++cluster_id; // increment current cluster_id
+        clusters.push_back(std::vector<KeyPoint>());
+        
+        // Expand the cluster
+        clusters[cluster_id].push_back(occupied_cells_[i]);
+        for(int j = 0; j<(int)neighbors.size(); j++)
+        {
+          if ((int)clusters[cluster_id].size() == parameter_.max_pts_)
+            break;
+          
+          if(!visited[neighbors[j]]) //keypoint has not been visited before
+          {
+            visited[neighbors[j]] = true;  // mark as visited
+            std::vector<int> further_neighbors;
+            regionQuery(neighbors[j], further_neighbors); //Find more neighbors around the new keypoint
+//             if(further_neighbors.size() < min_pts_)
+//             {	  
+//               clusters[0].push_back(occupied_cells[neighbors[j]]);
+//             }
+//             else
+            if ((int)further_neighbors.size() >= parameter_.min_pts_)
+            {
+              // neighbors found
+              neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end());  //Add these newfound P' neighbour to P neighbour vector "nb_indeces"
+              clusters[cluster_id].push_back(occupied_cells_[neighbors[j]]);
+            }
+          }
+        }
+      }	      
+    }
+  } 
+}
+  
+void CostmapToPolygonsDBSMCCH::regionQuery(int curr_index, std::vector<int>& neighbors)
+{
+    neighbors.clear();
+
+    double dist_sqr_threshold = parameter_.max_distance_ * parameter_.max_distance_;
+    const KeyPoint& kp = occupied_cells_[curr_index];
+    int cx, cy;
+    pointToNeighborCells(kp, cx,cy);
+    
+    // loop over the neighboring cells for looking up the points
+    const int offsets[9][2] = {{-1, -1}, {0, -1}, {1, -1},
+                               {-1,  0}, {0,  0}, {1,  0},
+                               {-1,  1}, {0,  1}, {1,  1}};
+    for (int i = 0; i < 9; ++i)
+    {
+      int idx = neighborCellsToIndex(cx + offsets[i][0], cy + offsets[i][1]);
+      if (idx < 0 || idx >= int(neighbor_lookup_.size()))
+        continue;
+      const std::vector<int>& pointIndicesToCheck = neighbor_lookup_[idx];
+      for (int point_idx : pointIndicesToCheck) {
+        if (point_idx == curr_index) // point is not a neighbor to itself
+          continue;
+        const KeyPoint& other = occupied_cells_[point_idx];
+        double dx = other.x - kp.x;
+        double dy = other.y - kp.y;
+        double dist_sqr = dx*dx + dy*dy;
+        if (dist_sqr <= dist_sqr_threshold)
+          neighbors.push_back(point_idx);
+      }
+    }
+}
+
+bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1, const CostmapToPolygonsDBSMCCH::KeyPoint& p2)
+{
+  return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);
+}
+
+void CostmapToPolygonsDBSMCCH::convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)
+{
+    //Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull
+  
+    int k = 0;
+    int n = cluster.size();
+    
+    // sort points according to x coordinate (TODO. is it already sorted due to the map representation?)
+    std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller);
+    
+    polygon.points.resize(2*n);
+      
+    // lower hull
+    for (int i = 0; i < n; ++i)
+    {
+      while (k >= 2 && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0) 
+      {
+        --k;
+      }
+      cluster[i].toPointMsg(polygon.points[k]);
+      ++k;
+    }
+      
+    // upper hull  
+    for (int i = n-2, t = k+1; i >= 0; --i) 
+    {
+      while (k >= t && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
+      {
+        --k;
+      }
+      cluster[i].toPointMsg(polygon.points[k]);
+      ++k;
+    }
+    
+
+    polygon.points.resize(k); // original
+    // TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice.
+//     polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost
+
+    simplifyPolygon(polygon);
+}
+
+
+
+void CostmapToPolygonsDBSMCCH::convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)
+{
+    std::vector<KeyPoint>& P = cluster;
+    std::vector<geometry_msgs::Point32>& points = polygon.points;
+
+    // Sort P by x and y
+    std::sort(P.begin(), P.end(), isXCoordinateSmaller);
+
+    // the output array H[] will be used as the stack
+    int i;                 // array scan index
+
+    // Get the indices of points with min x-coord and min|max y-coord
+    int minmin = 0, minmax;
+    double xmin = P[0].x;
+    for (i = 1; i < (int)P.size(); i++)
+        if (P[i].x != xmin) break;
+    minmax = i - 1;
+    if (minmax == (int)P.size() - 1)
+    {   // degenerate case: all x-coords == xmin
+        points.push_back(geometry_msgs::Point32());
+        P[minmin].toPointMsg(points.back());
+        if (P[minmax].y != P[minmin].y) // a  nontrivial segment
+        {
+            points.push_back(geometry_msgs::Point32());
+            P[minmax].toPointMsg(points.back());
+        }
+        // add polygon endpoint
+        points.push_back(geometry_msgs::Point32());
+        P[minmin].toPointMsg(points.back());
+        return;
+    }
+
+    // Get the indices of points with max x-coord and min|max y-coord
+    int maxmin, maxmax = (int)P.size() - 1;
+    double xmax = P.back().x;
+    for (i = P.size() - 2; i >= 0; i--)
+        if (P[i].x != xmax) break;
+    maxmin = i+1;
+
+    // Compute the lower hull on the stack H
+    // push  minmin point onto stack
+    points.push_back(geometry_msgs::Point32());
+    P[minmin].toPointMsg(points.back());
+    i = minmax;
+    while (++i <= maxmin)
+    {
+        // the lower line joins P[minmin]  with P[maxmin]
+        if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
+            continue;           // ignore P[i] above or on the lower line
+
+        while (points.size() > 1)         // there are at least 2 points on the stack
+        {
+            // test if  P[i] is left of the line at the stack top
+            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
+                break;         // P[i] is a new hull  vertex
+            points.pop_back();         // pop top point off  stack
+        }
+        // push P[i] onto stack
+        points.push_back(geometry_msgs::Point32());
+        P[i].toPointMsg(points.back());
+    }
+
+    // Next, compute the upper hull on the stack H above  the bottom hull
+    if (maxmax != maxmin)      // if  distinct xmax points
+    {
+         // push maxmax point onto stack
+         points.push_back(geometry_msgs::Point32());
+         P[maxmax].toPointMsg(points.back());
+    }
+    int bot = (int)points.size();                  // the bottom point of the upper hull stack
+    i = maxmin;
+    while (--i >= minmax)
+    {
+        // the upper line joins P[maxmax]  with P[minmax]
+        if (cross( P[maxmax], P[minmax], P[i])  >= 0 && i > minmax)
+            continue;           // ignore P[i] below or on the upper line
+
+        while ((int)points.size() > bot)     // at least 2 points on the upper stack
+        {
+            // test if  P[i] is left of the line at the stack top
+            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
+                break;         // P[i] is a new hull  vertex
+            points.pop_back();         // pop top point off stack
+        }
+        // push P[i] onto stack
+        points.push_back(geometry_msgs::Point32());
+        P[i].toPointMsg(points.back());
+    }
+    if (minmax != minmin)
+    {
+        // push  joining endpoint onto stack
+        points.push_back(geometry_msgs::Point32());
+        P[minmin].toPointMsg(points.back());
+    }
+    
+    simplifyPolygon(polygon);
+}
+
+void CostmapToPolygonsDBSMCCH::simplifyPolygon(geometry_msgs::Polygon& polygon)
+{
+  size_t triangleThreshold = 3;
+  // check if first and last point are the same. If yes, a triangle has 4 points
+  if (polygon.points.size() > 1
+      && std::abs(polygon.points.front().x - polygon.points.back().x) < 1e-5
+      && std::abs(polygon.points.front().y - polygon.points.back().y) < 1e-5)
+  {
+    triangleThreshold = 4;
+  }
+  if (polygon.points.size() <= triangleThreshold) // nothing to do for triangles or lines
+    return;
+  // TODO Reason about better start conditions for splitting lines, e.g., by
+  // https://en.wikipedia.org/wiki/Rotating_calipers
+  polygon.points = douglasPeucker(polygon.points.begin(), polygon.points.end(), parameter_.min_keypoint_separation_);;
+}
+
+void CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons)
+{
+  boost::mutex::scoped_lock lock(mutex_);
+  polygons_ = polygons;
+}
+
+
+PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()
+{
+  boost::mutex::scoped_lock lock(mutex_);
+  PolygonContainerConstPtr polygons = polygons_;
+  return polygons;
+}
+
+void CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level)
+{
+  boost::mutex::scoped_lock lock(parameter_mutex_);
+  parameter_buffered_.max_distance_ = config.cluster_max_distance;
+  parameter_buffered_.min_pts_ = config.cluster_min_pts;
+  parameter_buffered_.max_pts_ = config.cluster_max_pts;
+  parameter_buffered_.min_keypoint_separation_ = config.convex_hull_min_pt_separation;
+}
+
+}//end namespace costmap_converter
+
+

navigations/costmap_converter/src/costmap_to_polygons_concave.cpp

@@ -0,0 +1,220 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the institute nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#include <costmap_converter/costmap_to_polygons_concave.h>
+
+#include <pluginlib/class_list_macros.h>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSConcaveHull, costmap_converter::BaseCostmapToPolygons)
+
+namespace costmap_converter
+{
+    
+CostmapToPolygonsDBSConcaveHull::CostmapToPolygonsDBSConcaveHull() : CostmapToPolygonsDBSMCCH()
+{
+  dynamic_recfg_ = NULL;
+}
+
+CostmapToPolygonsDBSConcaveHull::~CostmapToPolygonsDBSConcaveHull() 
+{
+  if (dynamic_recfg_ != NULL)
+    delete dynamic_recfg_;
+}
+
+void CostmapToPolygonsDBSConcaveHull::initialize(ros::NodeHandle nh)
+{
+    nh.param("cluster_max_distance", parameter_.max_distance_, 0.4);
+    nh.param("cluster_min_pts", parameter_.min_pts_, 2);
+    nh.param("cluster_max_pts", parameter_.max_pts_, 30);
+    nh.param("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, 0.1);
+    parameter_buffered_ = parameter_;
+    
+    nh.param("concave_hull_depth", concave_hull_depth_, 2.0);
+    
+    // setup dynamic reconfigure
+    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>(nh);
+    dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSConcaveHull::reconfigureCB, this, _1, _2);
+    dynamic_recfg_->setCallback(cb);
+}
+
+
+void CostmapToPolygonsDBSConcaveHull::compute()
+{
+    std::vector< std::vector<KeyPoint> > clusters;
+    dbScan(clusters);
+    
+    // Create new polygon container
+    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());
+    
+    
+    // add convex hulls to polygon container
+    for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
+    {
+      polygons->push_back( geometry_msgs::Polygon() );
+      concaveHull(clusters[i], concave_hull_depth_, polygons->back() );
+    }
+    
+    // add our non-cluster points to the polygon container (as single points)
+    if (!clusters.empty())
+    {
+      for (int i=0; i < clusters.front().size(); ++i)
+      {
+        polygons->push_back( geometry_msgs::Polygon() );
+        convertPointToPolygon(clusters.front()[i], polygons->back());
+      }
+    }
+    
+    // replace shared polygon container
+    updatePolygonContainer(polygons);
+}
+
+
+void CostmapToPolygonsDBSConcaveHull::concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon)
+{
+    // start with convex hull
+    convexHull2(cluster, polygon);
+
+    std::vector<geometry_msgs::Point32>& concave_list = polygon.points;
+
+    for (int i = 0; i < (int)concave_list.size() - 1; ++i)
+    {
+      
+        // find nearest inner point pk from line (vertex1 -> vertex2)
+        const geometry_msgs::Point32& vertex1 = concave_list[i];
+        const geometry_msgs::Point32& vertex2 = concave_list[i+1];
+
+        bool found;
+        size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
+        if (!found) 
+          continue;  
+        
+        double line_length = norm2d(vertex1, vertex2);
+                
+        double dst1 = norm2d(cluster[nearest_idx], vertex1);
+        double dst2 = norm2d(cluster[nearest_idx], vertex2);
+        double dd = std::min(dst1, dst2);
+        if (dd<1e-8)
+          continue;
+
+        if (line_length / dd > depth)
+        {
+            // Check that new candidate edge will not intersect existing edges.
+            bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
+            intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
+            if (!intersects) 
+            {
+              geometry_msgs::Point32 new_point;
+              cluster[nearest_idx].toPointMsg(new_point);
+              concave_list.insert(concave_list.begin() + i + 1, new_point);
+              i--;
+            }
+        }
+    }
+}
+
+
+void CostmapToPolygonsDBSConcaveHull::concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon)
+{
+    // start with convex hull
+    convexHull2(cluster, polygon);
+
+    std::vector<geometry_msgs::Point32>& concave_list = polygon.points;
+    
+    // get line length
+    double mean_length = 0;
+    for (int i = 0; i < (int)concave_list.size() - 1; ++i)
+    {
+      mean_length += norm2d(concave_list[i],concave_list[i+1]);
+    }
+    mean_length /= double(concave_list.size());
+
+    for (int i = 0; i < (int)concave_list.size() - 1; ++i)
+    {
+      
+        // find nearest inner point pk from line (vertex1 -> vertex2)
+        const geometry_msgs::Point32& vertex1 = concave_list[i];
+        const geometry_msgs::Point32& vertex2 = concave_list[i+1];
+
+        double line_length = norm2d(vertex1, vertex2);
+        
+        bool found;
+        size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
+        if (!found) 
+        {
+          continue;  
+        }
+        
+                
+        double dst1 = norm2d(cluster[nearest_idx], vertex1);
+        double dst2 = norm2d(cluster[nearest_idx], vertex2);
+        double dd = std::min(dst1, dst2);
+        if (dd<1e-8)
+          continue;
+
+        if (line_length / dd > depth)
+        {
+            // Check that new candidate edge will not intersect existing edges.
+            bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
+            intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
+            if (!intersects) 
+            {
+              geometry_msgs::Point32 new_point;
+              cluster[nearest_idx].toPointMsg(new_point);
+              concave_list.insert(concave_list.begin() + i + 1, new_point);
+              i--;
+            }
+        }
+    }
+}
+
+
+
+
+void CostmapToPolygonsDBSConcaveHull::reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level)
+{
+    boost::mutex::scoped_lock lock(parameter_mutex_);
+    parameter_buffered_.max_distance_ = config.cluster_max_distance;
+    parameter_buffered_.min_pts_ = config.cluster_min_pts;
+    parameter_buffered_.max_pts_ = config.cluster_max_pts;
+    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
+    concave_hull_depth_ = config.concave_hull_depth;
+}
+
+}//end namespace costmap_converter
+
+

navigations/costmap_converter/test/costmap_polygons.cpp

@@ -0,0 +1,232 @@
+#include <random>
+#include <memory>
+#include <gtest/gtest.h>
+
+#include <costmap_converter/costmap_to_polygons.h>
+
+namespace {
+geometry_msgs::Point32 create_point(double x, double y)
+{
+  geometry_msgs::Point32 result;
+  result.x = x;
+  result.y = y;
+  result.z = 0.;
+  return result;
+}
+} // end namespace
+
+// make things accesible in the test
+class CostmapToPolygons : public costmap_converter::CostmapToPolygonsDBSMCCH
+{
+  public:
+    const std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint>& points() const {return occupied_cells_;}
+    costmap_converter::CostmapToPolygonsDBSMCCH::Parameters& parameters() {return parameter_;}
+    using costmap_converter::CostmapToPolygonsDBSMCCH::addPoint;
+    using costmap_converter::CostmapToPolygonsDBSMCCH::regionQuery;
+    using costmap_converter::CostmapToPolygonsDBSMCCH::dbScan;
+    using costmap_converter::CostmapToPolygonsDBSMCCH::convexHull2;
+    using costmap_converter::CostmapToPolygonsDBSMCCH::simplifyPolygon;
+};
+
+class CostmapToPolygonsDBSMCCHTest : public ::testing::Test
+{
+  protected:
+    void SetUp() override {
+      // parameters
+      costmap_to_polygons.parameters().max_distance_ = 0.5;
+      costmap_to_polygons.parameters().max_pts_ = 100;
+      costmap_to_polygons.parameters().min_pts_ = 2;
+      costmap_to_polygons.parameters().min_keypoint_separation_ = 0.1;
+
+      costmap.reset(new costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
+      costmap_to_polygons.setCostmap2D(costmap.get());
+
+      std::random_device rand_dev;
+      std::mt19937 generator(rand_dev());
+      std::uniform_real_distribution<> random_angle(-M_PI, M_PI);
+      std::uniform_real_distribution<> random_dist(0.,   costmap_to_polygons.parameters().max_distance_);
+
+      costmap_to_polygons.addPoint(0., 0.);
+      costmap_to_polygons.addPoint(1.3, 1.3);
+
+      // adding random points
+      double center_x = costmap_to_polygons.points()[0].x;
+      double center_y = costmap_to_polygons.points()[0].y;
+      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_ - 1; ++i)
+      {
+        double alpha = random_angle(generator);
+        double dist  = random_dist(generator);
+        double wx = center_x + std::cos(alpha) * dist;
+        double wy = center_y + std::sin(alpha) * dist;
+        costmap_to_polygons.addPoint(wx, wy);
+      }
+
+      // some noisy points not belonging to a cluster
+      costmap_to_polygons.addPoint(-1, -1);
+      costmap_to_polygons.addPoint(-2, -2);
+      
+      // adding random points
+      center_x = costmap_to_polygons.points()[1].x;
+      center_y = costmap_to_polygons.points()[1].y;
+      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_/2; ++i)
+      {
+        double alpha = random_angle(generator);
+        double dist  = random_dist(generator);
+        double wx = center_x + std::cos(alpha) * dist;
+        double wy = center_y + std::sin(alpha) * dist;
+        costmap_to_polygons.addPoint(wx, wy);
+      }
+    }
+
+    void regionQueryTrivial(int curr_index, std::vector<int>& neighbor_indices)
+    {
+      neighbor_indices.clear();
+      const auto& query_point = costmap_to_polygons.points()[curr_index];
+      for (int i = 0; i < int(costmap_to_polygons.points().size()); ++i)
+      {
+        if (i == curr_index)
+          continue;
+        const auto& kp = costmap_to_polygons.points()[i];
+        double dx = query_point.x - kp.x;
+        double dy = query_point.y - kp.y;
+        double dist = sqrt(dx*dx + dy*dy);
+        if (dist < costmap_to_polygons.parameters().max_distance_)
+          neighbor_indices.push_back(i);
+      }
+    }
+
+    CostmapToPolygons costmap_to_polygons;
+    std::shared_ptr<costmap_2d::Costmap2D> costmap;
+};
+
+TEST_F(CostmapToPolygonsDBSMCCHTest, regionQuery)
+{
+  std::vector<int> neighbors, neighbors_trivial;
+  costmap_to_polygons.regionQuery(0, neighbors);
+  regionQueryTrivial(0, neighbors_trivial);
+  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_ - 1, int(neighbors.size()));
+  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
+  std::sort(neighbors.begin(), neighbors.end());
+  ASSERT_EQ(neighbors_trivial, neighbors);
+
+  costmap_to_polygons.regionQuery(1, neighbors);
+  regionQueryTrivial(1, neighbors_trivial);
+  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2, int(neighbors.size()));
+  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
+  std::sort(neighbors.begin(), neighbors.end());
+  ASSERT_EQ(neighbors_trivial, neighbors);
+}
+
+TEST_F(CostmapToPolygonsDBSMCCHTest, dbScan)
+{
+  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
+  costmap_to_polygons.dbScan(clusters);
+  
+  ASSERT_EQ(3, clusters.size());
+  ASSERT_EQ(2, clusters[0].size()); // noisy points not belonging to a cluster
+  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_, clusters[1].size()); // first cluster at (0,0)
+  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2 + 1, clusters[2].size()); // second cluster at (1,1)
+}
+
+TEST(CostmapToPolygonsDBSMCCH, EmptyMap)
+{
+  std::shared_ptr<costmap_2d::Costmap2D> costmap =
+    std::make_shared<costmap_2d::Costmap2D>(costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
+  CostmapToPolygons costmap_to_polygons;
+  costmap_to_polygons.setCostmap2D(costmap.get());
+
+  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
+  costmap_to_polygons.dbScan(clusters);
+  ASSERT_EQ(1, clusters.size());    // noise cluster exists
+  ASSERT_EQ(0, clusters[0].size()); // noise clsuter is empty
+}
+
+TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygon)
+{
+  const double simplification_threshold = 0.1;
+  CostmapToPolygons costmap_to_polygons;
+  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
+  
+  geometry_msgs::Polygon polygon;
+  polygon.points.push_back(create_point(0., 0.));
+  polygon.points.push_back(create_point(1., 0.));
+
+  // degenerate case with just two points
+  geometry_msgs::Polygon original_polygon = polygon;
+  costmap_to_polygons.simplifyPolygon(polygon);
+  ASSERT_EQ(2, polygon.points.size());
+  for (size_t i = 0; i < polygon.points.size(); ++i)
+  {
+    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
+    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  
+  }
+
+  // degenerate case with three points
+  polygon.points.push_back(create_point(1., simplification_threshold / 2.));
+  original_polygon = polygon;
+  costmap_to_polygons.simplifyPolygon(polygon);
+  ASSERT_EQ(3, polygon.points.size());
+  for (size_t i = 0; i < polygon.points.size(); ++i)
+  {
+    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
+    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  
+  }
+
+  polygon.points.push_back(create_point(1., 1.));
+  original_polygon = polygon;
+  // remove the point that has to be removed by the simplification
+  original_polygon.points.erase(original_polygon.points.begin()+2);
+  costmap_to_polygons.simplifyPolygon(polygon);
+  ASSERT_EQ(3, polygon.points.size());
+  for (size_t i = 0; i < polygon.points.size(); ++i)
+  {
+    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
+    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);
+  }
+}
+
+TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygonPerfectLines)
+{
+  const double simplification_threshold = 0.1;
+  CostmapToPolygons costmap_to_polygons;
+  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
+  
+  geometry_msgs::Polygon polygon;
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(i*1., 0.));
+  geometry_msgs::Point32 lastPoint = polygon.points.back();
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
+  lastPoint = polygon.points.back();
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));
+  lastPoint = polygon.points.back();
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
+  lastPoint = polygon.points.back();
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));
+
+  costmap_to_polygons.simplifyPolygon(polygon);
+  ASSERT_EQ(6, polygon.points.size());
+  ASSERT_FLOAT_EQ(  0., polygon.points[0].x);
+  ASSERT_FLOAT_EQ(  0., polygon.points[0].y);
+  ASSERT_FLOAT_EQ(100., polygon.points[1].x);
+  ASSERT_FLOAT_EQ(  0., polygon.points[1].y);
+  ASSERT_FLOAT_EQ(100., polygon.points[2].x);
+  ASSERT_FLOAT_EQ(100., polygon.points[2].y);
+  ASSERT_FLOAT_EQ(200., polygon.points[3].x);
+  ASSERT_FLOAT_EQ(100., polygon.points[3].y);
+  ASSERT_FLOAT_EQ(200., polygon.points[4].x);
+  ASSERT_FLOAT_EQ(200., polygon.points[4].y);
+  ASSERT_FLOAT_EQ(300., polygon.points[5].x);
+  ASSERT_FLOAT_EQ(200., polygon.points[5].y);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "CostmapConverterTester");
+  ros::NodeHandle nh;
+  return RUN_ALL_TESTS();
+}

navigations/costmap_converter/test/costmap_polygons.test

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="costmap_polygons" pkg="costmap_converter" type="test_costmap_polygons" />
+</launch>