This class converts the costmap_2d into a set of lines (and points) More...
#include <costmap_to_lines_convex_hull.h>
Public Member Functions | |
virtual void | compute () |
This method performs the actual work (conversion of the costmap to polygons) | |
CostmapToLinesDBSMCCH () | |
Constructor. | |
virtual void | initialize (ros::NodeHandle nh) |
Initialize the plugin. | |
virtual | ~CostmapToLinesDBSMCCH () |
Destructor. | |
Protected Member Functions | |
void | extractPointsAndLines (std::vector< KeyPoint > &cluster, const geometry_msgs::Polygon &polygon, std::back_insert_iterator< std::vector< geometry_msgs::Polygon > > lines) |
Extract points and lines from a given cluster by checking all keypoint pairs of the convex hull for a minimum number of support points. | |
Protected Attributes | |
int | min_support_pts_ |
double | support_pts_max_dist_ |
double | support_pts_max_dist_inbetween_ |
Private Member Functions | |
void | reconfigureCB (CostmapToLinesDBSMCCHConfig &config, uint32_t level) |
Callback for the dynamic_reconfigure node. | |
Private Attributes | |
dynamic_reconfigure::Server < CostmapToLinesDBSMCCHConfig > * | dynamic_recfg_ |
Dynamic reconfigure server to allow config modifications at runtime. |
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)
Definition at line 69 of file costmap_to_lines_convex_hull.h.
Constructor.
Definition at line 50 of file costmap_to_lines_convex_hull.cpp.
Destructor.
Definition at line 55 of file costmap_to_lines_convex_hull.cpp.
void costmap_converter::CostmapToLinesDBSMCCH::compute | ( | ) | [virtual] |
This method performs the actual work (conversion of the costmap to polygons)
Reimplemented from costmap_converter::CostmapToPolygonsDBSMCCH.
Definition at line 99 of file costmap_to_lines_convex_hull.cpp.
void costmap_converter::CostmapToLinesDBSMCCH::extractPointsAndLines | ( | std::vector< KeyPoint > & | cluster, |
const geometry_msgs::Polygon & | polygon, | ||
std::back_insert_iterator< std::vector< geometry_msgs::Polygon > > | lines | ||
) | [protected] |
Extract points and lines from a given cluster by checking all keypoint pairs of the convex hull for a minimum number of support points.
cluster | list of points in the cluster | |
polygon | convex hull of the cluster cluster | |
[out] | lines | back_inserter object to a sequence of polygon msgs (new lines will be pushed back) |
Definition at line 139 of file costmap_to_lines_convex_hull.cpp.
void costmap_converter::CostmapToLinesDBSMCCH::initialize | ( | ros::NodeHandle | nh | ) | [virtual] |
Initialize the plugin.
nh | Nodehandle that defines the namespace for parameters |
Reimplemented from costmap_converter::CostmapToPolygonsDBSMCCH.
Definition at line 61 of file costmap_to_lines_convex_hull.cpp.
void costmap_converter::CostmapToLinesDBSMCCH::reconfigureCB | ( | CostmapToLinesDBSMCCHConfig & | config, |
uint32_t | level | ||
) | [private] |
Callback for the dynamic_reconfigure node.
This callback allows to modify parameters dynamically at runtime without restarting the node
config | Reference to the dynamic reconfigure config |
level | Dynamic reconfigure level |
Definition at line 284 of file costmap_to_lines_convex_hull.cpp.
dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>* costmap_converter::CostmapToLinesDBSMCCH::dynamic_recfg_ [private] |
Dynamic reconfigure server to allow config modifications at runtime.
Reimplemented from costmap_converter::CostmapToPolygonsDBSMCCH.
Definition at line 124 of file costmap_to_lines_convex_hull.h.
int costmap_converter::CostmapToLinesDBSMCCH::min_support_pts_ [protected] |
Definition at line 111 of file costmap_to_lines_convex_hull.h.
double costmap_converter::CostmapToLinesDBSMCCH::support_pts_max_dist_ [protected] |
Definition at line 110 of file costmap_to_lines_convex_hull.h.
double costmap_converter::CostmapToLinesDBSMCCH::support_pts_max_dist_inbetween_ [protected] |
Definition at line 109 of file costmap_to_lines_convex_hull.h.