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

#include <costmap_to_lines_ransac.h>

Inheritance diagram for costmap_converter::CostmapToLinesDBSRANSAC:

Public Member Functions
virtual void	compute ()
	This method performs the actual work (conversion of the costmap to polygons) More...

	CostmapToLinesDBSRANSAC ()
	Constructor. More...

virtual void	initialize (ros::NodeHandle nh)
	Initialize the plugin. More...

virtual	~CostmapToLinesDBSRANSAC ()
	Destructor. More...

Public Member Functions inherited from costmap_converter::CostmapToPolygonsDBSMCCH
	CostmapToPolygonsDBSMCCH ()
	Constructor. More...

PolygonContainerConstPtr	getPolygons ()
	Get a shared instance of the current polygon container. More...

virtual void	setCostmap2D (costmap_2d::Costmap2D *costmap)
	Pass a pointer to the costap to the plugin. More...

virtual void	updateCostmap2D ()
	Get updated data from the previously set Costmap2D. More...

virtual	~CostmapToPolygonsDBSMCCH ()
	Destructor. More...

Public Member Functions inherited from costmap_converter::BaseCostmapToPolygons
virtual ObstacleArrayConstPtr	getObstacles ()
	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(). More...

virtual void	setOdomTopic (const std::string &odom_topic)
	Set name of robot's odometry topic. More...

virtual bool	stackedCostmapConversion ()
	Determines whether an additional plugin for subsequent costmap conversion is specified. More...

void	startWorker (ros::Rate rate, costmap_2d::Costmap2D *costmap, bool spin_thread=false)
	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 `rate`. The passed `costmap` pointer must be valid at the complete time and must be lockable. By specifying the argument `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). More...

void	stopWorker ()
	Stop the worker that repeatedly converts the costmap to polygons. More...

virtual	~BaseCostmapToPolygons ()
	Destructor. More...

Static Public Member Functions
template<typename Point , typename LinePoint >
static bool	isInlier (const Point &point, const LinePoint &line_start, const LinePoint &line_end, double min_distance)
	Check if the candidate point is an inlier. More...

Static Public Member Functions inherited from costmap_converter::CostmapToPolygonsDBSMCCH
template<typename Point >
static void	convertPointToPolygon (const Point &point, geometry_msgs::Polygon &polygon)
	Convert a generi point type to a geometry_msgs::Polygon. More...

Protected Member Functions
bool	linearRegression (const std::vector< KeyPoint > &data, double &slope, double &intercept, double mean_x_out=NULL, double mean_y_out=NULL)
	Perform a simple linear regression in order to fit a straight line 'y = slope*x + intercept'. More...

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)
	Find a straight line segment in a point cloud with RANSAC (without linear regression). More...

Protected Member Functions inherited from costmap_converter::CostmapToPolygonsDBSMCCH
void	addPoint (double x, double y)
	helper function for adding a point to the lookup data structures More...

void	convexHull (std::vector< KeyPoint > &cluster, geometry_msgs::Polygon &polygon)
	Compute the convex hull for a single cluster (monotone chain algorithm) More...

void	convexHull2 (std::vector< KeyPoint > &cluster, geometry_msgs::Polygon &polygon)
	Compute the convex hull for a single cluster. More...

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 More...

void	dbScan (std::vector< std::vector< KeyPoint > > &clusters)
	DBSCAN algorithm for clustering. More...

int	neighborCellsToIndex (int cx, int cy)
	offset [meters] in y for the lookup grid More...

void	pointToNeighborCells (const KeyPoint &kp, int &cx, int &cy)
	compute the cell indices of a keypoint More...

void	regionQuery (int curr_index, std::vector< int > &neighbor_indices)
	Helper function for dbScan to search for neighboring points. More...

void	simplifyPolygon (geometry_msgs::Polygon &polygon)
	Simplify a polygon by removing points. More...

void	updatePolygonContainer (PolygonContainerPtr polygons)
	Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class) More...

Protected Member Functions inherited from costmap_converter::BaseCostmapToPolygons
	BaseCostmapToPolygons ()
	Protected constructor that should be called by subclasses. More...

void	spinThread ()
	Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) More...

void	workerCallback (const ros::TimerEvent &)
	The callback of the worker that performs the actual work (updating the costmap and converting it to polygons) More...

Protected Attributes
bool	ransac_convert_outlier_pts_
	If `true`, convert remaining outliers to single points. More...

bool	ransac_filter_remaining_outlier_pts_
	If `true`, filter the interior of remaining outliers and keep only keypoints of their convex hull. More...

double	ransac_inlier_distance_
	Maximum distance to the line segment for inliers. More...

int	ransac_min_inliers_
	Minimum numer of inliers required to form a line. More...

int	ransac_no_iterations_
	Number of ransac iterations. More...

int	ransac_remainig_outliers_
	Repeat ransac until the number of outliers is as specified here. More...

boost::random::mt19937	rnd_generator_
	Random number generator for ransac with default seed. More...

Protected Attributes inherited from costmap_converter::CostmapToPolygonsDBSMCCH
std::vector< std::vector< int > >	neighbor_lookup_

int	neighbor_size_x_
	array of cells for neighbor lookup More...

int	neighbor_size_y_
	size of the neighbour lookup in x (number of cells) More...

std::vector< KeyPoint >	occupied_cells_
	List of occupied cells in the current map (updated by updateCostmap2D()) More...

double	offset_x_
	size of the neighbour lookup in y (number of cells) More...

double	offset_y_
	offset [meters] in x for the lookup grid More...

Parameters	parameter_

Parameters	parameter_buffered_

boost::mutex	parameter_mutex_
	Mutex that keeps track about the ownership of the shared polygon instance. More...

Private Member Functions
void	reconfigureCB (CostmapToLinesDBSRANSACConfig &config, uint32_t level)
	Callback for the dynamic_reconfigure node. More...

Private Attributes
dynamic_reconfigure::Server< CostmapToLinesDBSRANSACConfig > *	dynamic_recfg_
	Dynamic reconfigure server to allow config modifications at runtime. More...

Detailed Description

This class converts the costmap_2d into a set of lines (and points)

The conversion is performed in two stages:

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.
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)

Definition at line 71 of file costmap_to_lines_ransac.h.

Constructor & Destructor Documentation

costmap_converter::CostmapToLinesDBSRANSAC::CostmapToLinesDBSRANSAC ( )

Constructor.

Definition at line 49 of file costmap_to_lines_ransac.cpp.

costmap_converter::CostmapToLinesDBSRANSAC::~CostmapToLinesDBSRANSAC ( )

virtual

Destructor.

Definition at line 54 of file costmap_to_lines_ransac.cpp.

Member Function Documentation

void costmap_converter::CostmapToLinesDBSRANSAC::compute ( )

virtual

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

Reimplemented from costmap_converter::CostmapToPolygonsDBSMCCH.

Definition at line 84 of file costmap_to_lines_ransac.cpp.

void costmap_converter::CostmapToLinesDBSRANSAC::initialize ( ros::NodeHandle nh )

virtual

Initialize the plugin.

Parameters

nh	Nodehandle that defines the namespace for parameters

Reimplemented from costmap_converter::CostmapToPolygonsDBSMCCH.

Definition at line 60 of file costmap_to_lines_ransac.cpp.

template<typename Point , typename LinePoint >

bool costmap_converter::CostmapToLinesDBSRANSAC::isInlier	(	const Point &	point,
		const LinePoint &	line_start,
		const LinePoint &	line_end,
		double	min_distance
	)

static

Check if the candidate point is an inlier.

Parameters

point	generic 2D point type defining the reference point
line_start	generic 2D point type defining the start of the line
line_end	generic 2D point type defining the end of the line
min_distance	minimum distance allowed

Template Parameters

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

Returns: true if inlier, false otherwise

Definition at line 173 of file costmap_to_lines_ransac.h.

bool costmap_converter::CostmapToLinesDBSRANSAC::linearRegression	(	const std::vector< KeyPoint > &	data,
		double &	slope,
		double &	intercept,
		double *	mean_x_out = `NULL`,
		double *	mean_y_out = `NULL`
	)

protected

Perform a simple linear regression in order to fit a straight line 'y = slope*x + intercept'.

Parameters

	data	set of 2D data points
[out]	slope	The slope of the fitted line
[out]	intercept	The intercept / offset of the line
[out]	mean_x_out	mean of the x-values of the data [optional]
[out]	mean_y_out	mean of the y-values of the data [optional]

Returns: true, if a valid line has been fitted, false otherwise.

Definition at line 233 of file costmap_to_lines_ransac.cpp.

bool costmap_converter::CostmapToLinesDBSRANSAC::lineRansac	(	const std::vector< KeyPoint > &	data,
		double	inlier_distance,
		int	no_iterations,
		int	min_inliers,
		std::pair< KeyPoint, KeyPoint > &	best_model,
		std::vector< KeyPoint > *	inliers = `NULL`,
		std::vector< KeyPoint > *	outliers = `NULL`
	)

protected

Find a straight line segment in a point cloud with RANSAC (without linear regression).

Parameters

	data	set of 2D data points
	inlier_distance	maximum distance that inliers must satisfy
	no_iterations	number of RANSAC iterations
	min_inliers	minimum number of inliers to return true
[out]	best_model	start and end of the best straight line segment
[out]	inliers	inlier keypoints are written to this container [optional]
[out]	outliers	outlier keypoints are written to this container [optional]

Returns: false, if min_inliers is not satisfied, true otherwise

Definition at line 159 of file costmap_to_lines_ransac.cpp.

void costmap_converter::CostmapToLinesDBSRANSAC::reconfigureCB	(	CostmapToLinesDBSRANSACConfig &	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 281 of file costmap_to_lines_ransac.cpp.

Member Data Documentation

dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>* costmap_converter::CostmapToLinesDBSRANSAC::dynamic_recfg_

private

Dynamic reconfigure server to allow config modifications at runtime.

Definition at line 166 of file costmap_to_lines_ransac.h.

bool costmap_converter::CostmapToLinesDBSRANSAC::ransac_convert_outlier_pts_

protected

If true, convert remaining outliers to single points.

Definition at line 116 of file costmap_to_lines_ransac.h.

bool costmap_converter::CostmapToLinesDBSRANSAC::ransac_filter_remaining_outlier_pts_

protected

If true, filter the interior of remaining outliers and keep only keypoints of their convex hull.

Definition at line 117 of file costmap_to_lines_ransac.h.

double costmap_converter::CostmapToLinesDBSRANSAC::ransac_inlier_distance_

protected

Maximum distance to the line segment for inliers.

Definition at line 112 of file costmap_to_lines_ransac.h.

int costmap_converter::CostmapToLinesDBSRANSAC::ransac_min_inliers_

protected

Minimum numer of inliers required to form a line.

Definition at line 113 of file costmap_to_lines_ransac.h.

int costmap_converter::CostmapToLinesDBSRANSAC::ransac_no_iterations_

protected

Number of ransac iterations.

Definition at line 114 of file costmap_to_lines_ransac.h.

int costmap_converter::CostmapToLinesDBSRANSAC::ransac_remainig_outliers_

protected

Repeat ransac until the number of outliers is as specified here.

Definition at line 115 of file costmap_to_lines_ransac.h.

boost::random::mt19937 costmap_converter::CostmapToLinesDBSRANSAC::rnd_generator_

protected

Random number generator for ransac with default seed.

Definition at line 120 of file costmap_to_lines_ransac.h.

The documentation for this class was generated from the following files:

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation