Showing preview only (254K chars total). Download the full file or copy to clipboard to get everything.
Repository: rst-tu-dortmund/costmap_converter
Branch: master
Commit: d1d57b6e6be3
Files: 42
Total size: 239.0 KB
Directory structure:
gitextract_fm737r5r/
├── .gitlab-ci.yml
├── .travis.yml
├── CHANGELOG.rst
├── CMakeLists.txt
├── README.md
├── cfg/
│ └── dynamic_reconfigure/
│ ├── CostmapToDynamicObstacles.cfg
│ ├── CostmapToLinesDBSMCCH.cfg
│ ├── CostmapToLinesDBSRANSAC.cfg
│ ├── CostmapToPolygonsDBSConcaveHull.cfg
│ └── CostmapToPolygonsDBSMCCH.cfg
├── include/
│ └── costmap_converter/
│ ├── costmap_converter_interface.h
│ ├── costmap_to_dynamic_obstacles/
│ │ ├── background_subtractor.h
│ │ ├── blob_detector.h
│ │ ├── costmap_to_dynamic_obstacles.h
│ │ └── multitarget_tracker/
│ │ ├── Ctracker.h
│ │ ├── HungarianAlg.h
│ │ ├── Kalman.h
│ │ ├── README.md
│ │ └── defines.h
│ ├── costmap_to_lines_convex_hull.h
│ ├── costmap_to_lines_ransac.h
│ ├── costmap_to_polygons.h
│ ├── costmap_to_polygons_concave.h
│ └── misc.h
├── msg/
│ ├── ObstacleArrayMsg.msg
│ └── ObstacleMsg.msg
├── package.xml
├── plugins.xml
├── src/
│ ├── costmap_converter_node.cpp
│ ├── costmap_to_dynamic_obstacles/
│ │ ├── background_subtractor.cpp
│ │ ├── blob_detector.cpp
│ │ ├── costmap_to_dynamic_obstacles.cpp
│ │ └── multitarget_tracker/
│ │ ├── Ctracker.cpp
│ │ ├── HungarianAlg.cpp
│ │ ├── Kalman.cpp
│ │ └── README.md
│ ├── costmap_to_lines_convex_hull.cpp
│ ├── costmap_to_lines_ransac.cpp
│ ├── costmap_to_polygons.cpp
│ └── costmap_to_polygons_concave.cpp
└── test/
├── costmap_polygons.cpp
└── costmap_polygons.test
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitlab-ci.yml
================================================
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
================================================
FILE: .travis.yml
================================================
# 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.
================================================
FILE: CHANGELOG.rst
================================================
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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.
================================================
FILE: CMakeLists.txt
================================================
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)
================================================
FILE: README.md
================================================
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: [](http://build.ros.org/job/Ndev__costmap_converter__ubuntu_focal_amd64/)
- ROS Buildfarm Melodic: [](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.
================================================
FILE: cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg
================================================
#!/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"))
================================================
FILE: cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg
================================================
#!/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"))
================================================
FILE: cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg
================================================
#!/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"))
================================================
FILE: cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg
================================================
#!/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"))
================================================
FILE: cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg
================================================
#!/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"))
================================================
FILE: include/costmap_converter/costmap_converter_interface.h
================================================
/*********************************************************************
*
* 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_
================================================
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h
================================================
/*********************************************************************
*
* 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_
================================================
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h
================================================
/*********************************************************************
*
* 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_
================================================
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h
================================================
/*********************************************************************
*
* 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_ */
================================================
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h
================================================
// 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 ¶meters);
private:
// Aggregated parameters for the tracker object
Params params;
// ID for the upcoming CTrack object
size_t NextTrackID;
};
================================================
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h
================================================
// 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);
};
================================================
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h
================================================
// 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]
};
================================================
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md
================================================
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.
================================================
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h
================================================
// 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
================================================
FILE: include/costmap_converter/costmap_to_lines_convex_hull.h
================================================
/*********************************************************************
*
* 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_ */
================================================
FILE: include/costmap_converter/costmap_to_lines_ransac.h
================================================
/*********************************************************************
*
* 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_ */
================================================
FILE: include/costmap_converter/costmap_to_polygons.h
================================================
/*********************************************************************
*
* 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_ */
================================================
FILE: include/costmap_converter/costmap_to_polygons_concave.h
================================================
/*********************************************************************
*
* 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_ */
================================================
FILE: include/costmap_converter/misc.h
================================================
/*********************************************************************
*
* 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_ */
================================================
FILE: msg/ObstacleArrayMsg.msg
================================================
# 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
================================================
FILE: msg/ObstacleMsg.msg
================================================
# 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
================================================
FILE: package.xml
================================================
<?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>
================================================
FILE: plugins.xml
================================================
<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>
================================================
FILE: src/costmap_converter_node.cpp
================================================
/*********************************************************************
*
* 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);
publishAsMarker(msg->header.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(update->header.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_;
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;
}
================================================
FILE: src/costmap_to_dynamic_obstacles/background_subtractor.cpp
================================================
#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/cvv/cvv.hpp>
#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>
BackgroundSubtractor::BackgroundSubtractor(const Params ¶meters): 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 ¶meters)
{
params_ = parameters;
}
================================================
FILE: src/costmap_to_dynamic_obstacles/blob_detector.cpp
================================================
#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;
}
================================================
FILE: src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp
================================================
#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);
}
}
}
================================================
FILE: src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp
================================================
// 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 ¶meters)
: 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 ¶meters)
{
params = parameters;
}
// ---------------------------------------------------------------------------
//
// ---------------------------------------------------------------------------
CTracker::~CTracker(void) {}
================================================
FILE: src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp
================================================
// 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);
}
================================================
FILE: src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp
================================================
// 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;
}
//---------------------------------------------------------------------------
================================================
FILE: src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md
================================================
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.
================================================
FILE: src/costmap_to_lines_convex_hull.cpp
================================================
/*********************************************************************
*
* 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;
gitextract_fm737r5r/
├── .gitlab-ci.yml
├── .travis.yml
├── CHANGELOG.rst
├── CMakeLists.txt
├── README.md
├── cfg/
│ └── dynamic_reconfigure/
│ ├── CostmapToDynamicObstacles.cfg
│ ├── CostmapToLinesDBSMCCH.cfg
│ ├── CostmapToLinesDBSRANSAC.cfg
│ ├── CostmapToPolygonsDBSConcaveHull.cfg
│ └── CostmapToPolygonsDBSMCCH.cfg
├── include/
│ └── costmap_converter/
│ ├── costmap_converter_interface.h
│ ├── costmap_to_dynamic_obstacles/
│ │ ├── background_subtractor.h
│ │ ├── blob_detector.h
│ │ ├── costmap_to_dynamic_obstacles.h
│ │ └── multitarget_tracker/
│ │ ├── Ctracker.h
│ │ ├── HungarianAlg.h
│ │ ├── Kalman.h
│ │ ├── README.md
│ │ └── defines.h
│ ├── costmap_to_lines_convex_hull.h
│ ├── costmap_to_lines_ransac.h
│ ├── costmap_to_polygons.h
│ ├── costmap_to_polygons_concave.h
│ └── misc.h
├── msg/
│ ├── ObstacleArrayMsg.msg
│ └── ObstacleMsg.msg
├── package.xml
├── plugins.xml
├── src/
│ ├── costmap_converter_node.cpp
│ ├── costmap_to_dynamic_obstacles/
│ │ ├── background_subtractor.cpp
│ │ ├── blob_detector.cpp
│ │ ├── costmap_to_dynamic_obstacles.cpp
│ │ └── multitarget_tracker/
│ │ ├── Ctracker.cpp
│ │ ├── HungarianAlg.cpp
│ │ ├── Kalman.cpp
│ │ └── README.md
│ ├── costmap_to_lines_convex_hull.cpp
│ ├── costmap_to_lines_ransac.cpp
│ ├── costmap_to_polygons.cpp
│ └── costmap_to_polygons_concave.cpp
└── test/
├── costmap_polygons.cpp
└── costmap_polygons.test
SYMBOL INDEX (56 symbols across 22 files)
FILE: include/costmap_converter/costmap_converter_interface.h
function namespace (line 52) | namespace costmap_converter
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h
function class (line 57) | class BackgroundSubtractor
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h
function class (line 60) | class BlobDetector : public cv::SimpleBlobDetector
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h
function namespace (line 69) | namespace costmap_converter {
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h
function class (line 14) | class CTrack
function class (line 73) | class CTracker
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h
type std (line 12) | typedef std::vector<int> assignments_t;
type std (line 13) | typedef std::vector<track_t> distMatrix_t;
function class (line 15) | class AssignmentProblemSolver
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h
function class (line 9) | class TKalmanFilter
FILE: include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h
type track_t (line 7) | typedef float track_t;
type cv (line 8) | typedef cv::Point3_<track_t> Point_t;
FILE: include/costmap_converter/costmap_to_lines_convex_hull.h
function namespace (line 48) | namespace costmap_converter
FILE: include/costmap_converter/costmap_to_lines_ransac.h
function namespace (line 49) | namespace costmap_converter
FILE: include/costmap_converter/costmap_to_polygons.h
function namespace (line 60) | namespace costmap_converter
type Parameters (line 106) | struct Parameters
function addPoint (line 203) | void addPoint(double x, double y)
function cross (line 262) | double cross(const P1& O, const P2& A, const P3& B)
function neighborCellsToIndex (line 288) | int neighborCellsToIndex(int cx, int cy)
function pointToNeighborCells (line 301) | void pointToNeighborCells(const KeyPoint& kp, int& cx, int& cy)
FILE: include/costmap_converter/costmap_to_polygons_concave.h
function namespace (line 50) | namespace costmap_converter
FILE: include/costmap_converter/misc.h
function namespace (line 45) | namespace costmap_converter
function norm2d (line 133) | double norm2d(const Point1& pt1, const Point2& pt2)
function isApprox2d (line 148) | bool isApprox2d(const Point1& pt1, const Point2& pt2, double threshold)
FILE: src/costmap_converter_node.cpp
class CostmapStandaloneConversion (line 51) | class CostmapStandaloneConversion
method CostmapStandaloneConversion (line 54) | CostmapStandaloneConversion() : converter_loader_("costmap_converter",...
method costmapCallback (line 105) | void costmapCallback(const nav_msgs::OccupancyGridConstPtr& msg)
method costmapUpdateCallback (line 140) | void costmapUpdateCallback(const map_msgs::OccupancyGridUpdateConstPtr...
method publishAsMarker (line 165) | void publishAsMarker(const std::string& frame_id, const std::vector<ge...
method publishAsMarker (line 215) | void publishAsMarker(const std::string& frame_id, const costmap_conver...
function main (line 282) | int main(int argc, char** argv)
FILE: src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp
type costmap_converter (line 8) | namespace costmap_converter
function ObstacleArrayConstPtr (line 358) | ObstacleArrayConstPtr CostmapToDynamicObstacles::getObstacles()
function Point_t (line 370) | Point_t CostmapToDynamicObstacles::getEstimatedVelocityOfObject(unsign...
FILE: src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp
function track_t (line 11) | track_t AssignmentProblemSolver::Solve(const distMatrix_t& distMatrixIn,...
FILE: src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp
function Point_t (line 74) | Point_t TKalmanFilter::Update(Point_t p, bool DataCorrect)
FILE: src/costmap_to_lines_convex_hull.cpp
type costmap_converter (line 47) | namespace costmap_converter
function sort_keypoint_x (line 123) | bool sort_keypoint_x(const std::size_t& i, const std::size_t& j, const...
function sort_keypoint_y (line 125) | bool sort_keypoint_y(const std::size_t& i, const std::size_t& j, const...
FILE: src/costmap_to_lines_ransac.cpp
type costmap_converter (line 46) | namespace costmap_converter
FILE: src/costmap_to_polygons.cpp
function douglasPeucker (line 60) | std::vector<geometry_msgs::Point32> douglasPeucker(std::vector<geometry_...
type costmap_converter (line 104) | namespace costmap_converter
function isXCoordinateSmaller (line 314) | bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1...
function PolygonContainerConstPtr (line 485) | PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()
FILE: src/costmap_to_polygons_concave.cpp
type costmap_converter (line 45) | namespace costmap_converter
FILE: test/costmap_polygons.cpp
function create_point (line 8) | geometry_msgs::Point32 create_point(double x, double y)
class CostmapToPolygons (line 19) | class CostmapToPolygons : public costmap_converter::CostmapToPolygonsDBS...
class CostmapToPolygonsDBSMCCHTest (line 31) | class CostmapToPolygonsDBSMCCHTest : public ::testing::Test
method SetUp (line 34) | void SetUp() override {
method regionQueryTrivial (line 81) | void regionQueryTrivial(int curr_index, std::vector<int>& neighbor_ind...
function TEST_F (line 102) | TEST_F(CostmapToPolygonsDBSMCCHTest, regionQuery)
function TEST_F (line 120) | TEST_F(CostmapToPolygonsDBSMCCHTest, dbScan)
function TEST (line 131) | TEST(CostmapToPolygonsDBSMCCH, EmptyMap)
function TEST (line 144) | TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygon)
function TEST (line 188) | TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygonPerfectLines)
function main (line 226) | int main(int argc, char** argv)
Condensed preview — 42 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (255K chars).
[
{
"path": ".gitlab-ci.yml",
"chars": 597,
"preview": "build-kinetic:\n variables:\n ROSDISTRO: \"kinetic\"\n CATKIN_WS: \"/root/catkin_ws\"\n stage: build\n image: osrf/ros:$"
},
{
"path": ".travis.yml",
"chars": 4148,
"preview": "# Generic .travis.yml file for running continuous integration on Travis-CI with\n# any ROS package.\n#\n# This installs ROS"
},
{
"path": "CHANGELOG.rst",
"chars": 4444,
"preview": "^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\nChangelog for package costmap_converter\n^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^\n"
},
{
"path": "CMakeLists.txt",
"chars": 8283,
"preview": "cmake_minimum_required(VERSION 3.1)\nproject(costmap_converter)\n\n# Set to Release in order to speed up the program signif"
},
{
"path": "README.md",
"chars": 1375,
"preview": "costmap_converter ROS Package\n=============================\n\nA ros package that includes plugins and nodes to convert oc"
},
{
"path": "cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg",
"chars": 4638,
"preview": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For i"
},
{
"path": "cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg",
"chars": 1440,
"preview": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For i"
},
{
"path": "cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg",
"chars": 1678,
"preview": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For i"
},
{
"path": "cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg",
"chars": 1102,
"preview": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For i"
},
{
"path": "cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg",
"chars": 957,
"preview": "#!/usr/bin/env python\n\nfrom dynamic_reconfigure.parameter_generator_catkin import *\n\ngen = ParameterGenerator()\n\n# For i"
},
{
"path": "include/costmap_converter/costmap_converter_interface.h",
"chars": 13581,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h",
"chars": 4464,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h",
"chars": 4481,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h",
"chars": 7049,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h",
"chars": 2551,
"preview": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this dir"
},
{
"path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h",
"chars": 3432,
"preview": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this dir"
},
{
"path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h",
"chars": 605,
"preview": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this dir"
},
{
"path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md",
"chars": 485,
"preview": "Multitarget Tracker\n===================\n\nThis code is mostly copied from the (MultitargetTracker)[https://github.com/Smo"
},
{
"path": "include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h",
"chars": 256,
"preview": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this dir"
},
{
"path": "include/costmap_converter/costmap_to_lines_convex_hull.h",
"chars": 5461,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "include/costmap_converter/costmap_to_lines_ransac.h",
"chars": 8740,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "include/costmap_converter/costmap_to_polygons.h",
"chars": 14235,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "include/costmap_converter/costmap_to_polygons_concave.h",
"chars": 7966,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "include/costmap_converter/misc.h",
"chars": 6677,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "msg/ObstacleArrayMsg.msg",
"chars": 313,
"preview": "# Message that contains a list of polygon shaped obstacles.\n# Special types:\n# Polygon with 1 vertex: Point obstacle\n# P"
},
{
"path": "msg/ObstacleMsg.msg",
"chars": 687,
"preview": "# Special types:\n# Polygon with 1 vertex: Point obstacle (you might also specify a non-zero value for radius)\n# Polygon "
},
{
"path": "package.xml",
"chars": 1257,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>costmap_converter</name>\n <version>0.0.13</version>\n <description>\n"
},
{
"path": "plugins.xml",
"chars": 1899,
"preview": "<library path=\"lib/libcostmap_converter\">\n \n <class type=\"costmap_converter::CostmapToPolygonsDBSMCCH\" base_class_type"
},
{
"path": "src/costmap_converter_node.cpp",
"chars": 10570,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "src/costmap_to_dynamic_obstacles/background_subtractor.cpp",
"chars": 5773,
"preview": "#include <opencv2/highgui/highgui.hpp>\n//#include <opencv2/cvv/cvv.hpp>\n#include <costmap_converter/costmap_to_dynamic_o"
},
{
"path": "src/costmap_to_dynamic_obstacles/blob_detector.cpp",
"chars": 6495,
"preview": "#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>\n#include <opencv2/opencv.hpp>\n#include <iostre"
},
{
"path": "src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp",
"chars": 18641,
"preview": "#include <costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h>\n\n#include <pluginlib/class_list"
},
{
"path": "src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp",
"chars": 4308,
"preview": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this dir"
},
{
"path": "src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp",
"chars": 23269,
"preview": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this dir"
},
{
"path": "src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp",
"chars": 3962,
"preview": "// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3\n// Refer to README.md in this dir"
},
{
"path": "src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md",
"chars": 485,
"preview": "Multitarget Tracker\n===================\n\nThis code is mostly copied from the (MultitargetTracker)[https://github.com/Smo"
},
{
"path": "src/costmap_to_lines_convex_hull.cpp",
"chars": 11869,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "src/costmap_to_lines_ransac.cpp",
"chars": 11272,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "src/costmap_to_polygons.cpp",
"chars": 18004,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "src/costmap_to_polygons_concave.cpp",
"chars": 8204,
"preview": "/*********************************************************************\n *\n * Software License Agreement (BSD License)\n *"
},
{
"path": "test/costmap_polygons.cpp",
"chars": 8979,
"preview": "#include <random>\n#include <memory>\n#include <gtest/gtest.h>\n\n#include <costmap_converter/costmap_to_polygons.h>\n\nnamesp"
},
{
"path": "test/costmap_polygons.test",
"chars": 112,
"preview": "<launch>\n <test test-name=\"costmap_polygons\" pkg=\"costmap_converter\" type=\"test_costmap_polygons\" />\n</launch>\n"
}
]
About this extraction
This page contains the full source code of the rst-tu-dortmund/costmap_converter GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 42 files (239.0 KB), approximately 61.8k tokens, and a symbol index with 56 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.