Public Member Functions | Static Public Member Functions | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes | List of all members
costmap_converter::CostmapToLinesDBSRANSAC Class Reference

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:
Inheritance graph
[legend]

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< KeyPointoccupied_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:

  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)

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
nhNodehandle 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
pointgeneric 2D point type defining the reference point
line_startgeneric 2D point type defining the start of the line
line_endgeneric 2D point type defining the end of the line
min_distanceminimum distance allowed
Template Parameters
Pointgeneric point type that should provide (writable) x and y member fiels.
LinePointgeneric 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
dataset of 2D data points
[out]slopeThe slope of the fitted line
[out]interceptThe intercept / offset of the line
[out]mean_x_outmean of the x-values of the data [optional]
[out]mean_y_outmean 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
dataset of 2D data points
inlier_distancemaximum distance that inliers must satisfy
no_iterationsnumber of RANSAC iterations
min_inliersminimum number of inliers to return true
[out]best_modelstart and end of the best straight line segment
[out]inliersinlier keypoints are written to this container [optional]
[out]outliersoutlier 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
configReference to the dynamic reconfigure config
levelDynamic 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:


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat May 16 2020 03:19:18