This class converts the costmap_2d into a set of convex polygons (and points) More...

#include <costmap_to_polygons.h>

Inheritance diagram for costmap_converter::CostmapToPolygonsDBSMCCH:

Classes
struct	KeyPoint
	Defines a keypoint in metric coordinates of the map. More...
Public Member Functions
virtual void	compute ()
	This method performs the actual work (conversion of the costmap to polygons)
	CostmapToPolygonsDBSMCCH ()
	Constructor.
PolygonContainerConstPtr	getPolygons ()
	Get a shared instance of the current polygon container.
virtual void	initialize (ros::NodeHandle nh)
	Initialize the plugin.
virtual void	setCostmap2D (costmap_2d::Costmap2D *costmap)
	Pass a pointer to the costap to the plugin.
virtual void	updateCostmap2D ()
	Get updated data from the previously set Costmap2D.
virtual	~CostmapToPolygonsDBSMCCH ()
	Destructor.
Static Public Member Functions
template<typename Point >
static void	convertPointToPolygon (const Point &point, geometry_msgs::Polygon &polygon)
	Convert a generi point type to a geometry_msgs::Polygon.
Protected Member Functions
void	convexHull (std::vector< KeyPoint > &cluster, geometry_msgs::Polygon &polygon)
	Compute the convex hull for a single cluster (monotone chain algorithm)
void	convexHull2 (std::vector< KeyPoint > &cluster, geometry_msgs::Polygon &polygon)
	Compute the convex hull for a single cluster.
template<typename P1 , typename P2 , typename P3 >
long double	cross (const P1 &O, const P2 &A, const P3 &B)
	2D Cross product of two vectors defined by two points and a common origin
void	dbScan (const std::vector< KeyPoint > &occupied_cells, std::vector< std::vector< KeyPoint > > &clusters)
	DBSCAN algorithm for clustering.
void	regionQuery (const std::vector< KeyPoint > &occupied_cells, int curr_index, std::vector< int > &neighbor_indices)
	Helper function for dbScan to search for neighboring points.
void	updatePolygonContainer (PolygonContainerPtr polygons)
	Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class)
Protected Attributes
double	max_distance_
	Parameter for DB_Scan, maximum distance to neighbors [m].
int	max_pts_
	Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes)
double	min_keypoint_separation_
	Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)
int	min_pts_
	Parameter for DB_Scan: minimum number of points that define a cluster.
std::vector< KeyPoint >	occupied_cells_
	List of occupied cells in the current map (updated by updateCostmap2D())
Private Member Functions
void	reconfigureCB (CostmapToPolygonsDBSMCCHConfig &config, uint32_t level)
	Callback for the dynamic_reconfigure node.
Private Attributes
costmap_2d::Costmap2D *	costmap_
	Pointer to the costmap2d.
dynamic_reconfigure::Server < CostmapToPolygonsDBSMCCHConfig > *	dynamic_recfg_
	Dynamic reconfigure server to allow config modifications at runtime.
boost::mutex	mutex_
	Mutex that keeps track about the ownership of the shared polygon instance.
PolygonContainerPtr	polygons_
	Current shared container of polygons.

Detailed Description

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)

Definition at line 79 of file costmap_to_polygons.h.

Constructor & Destructor Documentation

costmap_converter::CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH ( )

Constructor.

Definition at line 49 of file costmap_to_polygons.cpp.

costmap_converter::CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH ( ) [virtual]

Destructor.

Definition at line 55 of file costmap_to_polygons.cpp.

Member Function Documentation

void costmap_converter::CostmapToPolygonsDBSMCCH::compute ( ) [virtual]

This method performs the actual work (conversion of the costmap to polygons)

Implements costmap_converter::BaseCostmapToPolygons.

Reimplemented in costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::CostmapToLinesDBSMCCH, and costmap_converter::CostmapToPolygonsDBSConcaveHull.

Definition at line 84 of file costmap_to_polygons.cpp.

template<typename Point >

static void costmap_converter::CostmapToPolygonsDBSMCCH::convertPointToPolygon	(	const Point &	point,
		geometry_msgs::Polygon &	polygon
	)		`[inline, static]`

Convert a generi point type to a geometry_msgs::Polygon.

Parameters:

	point	generic 2D point type
[out]	polygon	already instantiated polygon that will be filled with a single point

Template Parameters:

Point generic point type that should provide (writable) x and y member fiels.

Definition at line 145 of file costmap_to_polygons.h.

void costmap_converter::CostmapToPolygonsDBSMCCH::convexHull	(	std::vector< KeyPoint > &	cluster,
		geometry_msgs::Polygon &	polygon
	)		`[protected]`

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

Parameters:

	cluster	list of keypoints that should be converted into a polygon
[out]	polygon	the resulting convex polygon

Definition at line 232 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::convexHull2	(	std::vector< KeyPoint > &	cluster,
		geometry_msgs::Polygon &	polygon
	)		`[protected]`

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

Parameters:

	cluster	list of keypoints that should be converted into a polygon
[out]	polygon	the resulting convex polygon

Todo:: Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)

Definition at line 285 of file costmap_to_polygons.cpp.

template<typename P1 , typename P2 , typename P3 >

long double costmap_converter::CostmapToPolygonsDBSMCCH::cross	(	const P1 &	O,
		const P2 &	A,
		const P3 &	B
	)		`[inline, protected]`

2D Cross product of two vectors defined by two points and a common origin

Parameters:

O	Origin
A	First point
B	Second point

Template Parameters:

P1	2D Point type with x and y member fields
P2	2D Point type with x and y member fields
P3	2D Point type with x and y member fields

Definition at line 227 of file costmap_to_polygons.h.

void costmap_converter::CostmapToPolygonsDBSMCCH::dbScan	(	const std::vector< KeyPoint > &	occupied_cells,
		std::vector< std::vector< KeyPoint > > &	clusters
	)		`[protected]`

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.

Parameters:

	occupied_cells	a list of occupied cells of the costmap in metric coordinates
[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

Definition at line 156 of file costmap_to_polygons.cpp.

PolygonContainerConstPtr costmap_converter::CostmapToPolygonsDBSMCCH::getPolygons ( ) [virtual]

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!

Returns:: Shared instance of the current polygon container

Reimplemented from costmap_converter::BaseCostmapToPolygons.

Definition at line 408 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::initialize ( ros::NodeHandle nh ) [virtual]

Initialize the plugin.

Parameters:

nh	Nodehandle that defines the namespace for parameters

Implements costmap_converter::BaseCostmapToPolygons.

Reimplemented in costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::CostmapToLinesDBSMCCH, and costmap_converter::CostmapToPolygonsDBSConcaveHull.

Definition at line 61 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::reconfigureCB	(	CostmapToPolygonsDBSMCCHConfig &	config,
		uint32_t	level
	)		`[private]`

Callback for the dynamic_reconfigure node.

This callback allows to modify parameters dynamically at runtime without restarting the node

Parameters:

config	Reference to the dynamic reconfigure config
level	Dynamic reconfigure level

Definition at line 415 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::regionQuery	(	const std::vector< KeyPoint > &	occupied_cells,
		int	curr_index,
		std::vector< int > &	neighbor_indices
	)		`[protected]`

Helper function for dbScan to search for neighboring points.

Parameters:

	occupied_cells	a list of occupied cells of the costmap in metric coordinates
	curr_index	index to the current item in `occupied_cells`
[out]	neighbor_indices	list of neighbors (indices of `occupied` cells)

Definition at line 211 of file costmap_to_polygons.cpp.

void costmap_converter::CostmapToPolygonsDBSMCCH::setCostmap2D ( costmap_2d::Costmap2D * costmap ) [virtual]

Pass a pointer to the costap to the plugin.