costmap_to_polygons_concave.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016,
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Christoph Rösmann, Otniel Rinaldo
00037  *********************************************************************/
00038 
00039 #include <costmap_converter/costmap_to_polygons_concave.h>
00040 
00041 #include <pluginlib/class_list_macros.h>
00042 
00043 PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSConcaveHull, costmap_converter::BaseCostmapToPolygons)
00044 
00045 namespace costmap_converter
00046 {
00047     
00048 CostmapToPolygonsDBSConcaveHull::CostmapToPolygonsDBSConcaveHull() : CostmapToPolygonsDBSMCCH()
00049 {
00050   dynamic_recfg_ = NULL;
00051 }
00052 
00053 CostmapToPolygonsDBSConcaveHull::~CostmapToPolygonsDBSConcaveHull() 
00054 {
00055   if (dynamic_recfg_ != NULL)
00056     delete dynamic_recfg_;
00057 }
00058 
00059 void CostmapToPolygonsDBSConcaveHull::initialize(ros::NodeHandle nh)
00060 {
00061     max_distance_ = 0.4; 
00062     nh.param("cluster_max_distance", max_distance_, max_distance_);
00063     
00064     min_pts_ = 2;
00065     nh.param("cluster_min_pts", min_pts_, min_pts_);
00066     
00067     max_pts_ = 30;
00068     nh.param("cluster_max_pts", max_pts_, max_pts_);
00069     
00070     min_keypoint_separation_ = 0.1;
00071     nh.param("convex_hull_min_pt_separation", min_keypoint_separation_, min_keypoint_separation_);
00072     
00073     concave_hull_depth_ = 2.0;
00074     nh.param("concave_hull_depth", concave_hull_depth_, concave_hull_depth_);
00075     
00076     // setup dynamic reconfigure
00077     dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>(nh);
00078     dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSConcaveHull::reconfigureCB, this, _1, _2);
00079     dynamic_recfg_->setCallback(cb);
00080 }
00081 
00082 
00083 void CostmapToPolygonsDBSConcaveHull::compute()
00084 {
00085     std::vector< std::vector<KeyPoint> > clusters;
00086     dbScan(occupied_cells_, clusters);
00087     
00088     // Create new polygon container
00089     PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());
00090     
00091     
00092     // add convex hulls to polygon container
00093     for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
00094     {
00095       polygons->push_back( geometry_msgs::Polygon() );
00096       concaveHull(clusters[i], concave_hull_depth_, polygons->back() );
00097     }
00098     
00099     // add our non-cluster points to the polygon container (as single points)
00100     if (!clusters.empty())
00101     {
00102       for (int i=0; i < clusters.front().size(); ++i)
00103       {
00104         polygons->push_back( geometry_msgs::Polygon() );
00105         convertPointToPolygon(clusters.front()[i], polygons->back());
00106       }
00107     }
00108     
00109     // replace shared polygon container
00110     updatePolygonContainer(polygons);
00111 }
00112 
00113 
00114 void CostmapToPolygonsDBSConcaveHull::concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon)
00115 {
00116     // start with convex hull
00117     convexHull2(cluster, polygon);
00118 
00119     std::vector<geometry_msgs::Point32>& concave_list = polygon.points;
00120 
00121     for (int i = 0; i < (int)concave_list.size() - 1; ++i)
00122     {
00123       
00124         // find nearest inner point pk from line (vertex1 -> vertex2)
00125         const geometry_msgs::Point32& vertex1 = concave_list[i];
00126         const geometry_msgs::Point32& vertex2 = concave_list[i+1];
00127 
00128         bool found;
00129         size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
00130         if (!found) 
00131           continue;  
00132         
00133         double line_length = norm2d(vertex1, vertex2);
00134                 
00135         double dst1 = norm2d(cluster[nearest_idx], vertex1);
00136         double dst2 = norm2d(cluster[nearest_idx], vertex2);
00137         double dd = std::min(dst1, dst2);
00138         if (dd<1e-8)
00139           continue;
00140 
00141         if (line_length / dd > depth)
00142         {
00143             // Check that new candidate edge will not intersect existing edges.
00144             bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
00145             intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
00146             if (!intersects) 
00147             {
00148               geometry_msgs::Point32 new_point;
00149               cluster[nearest_idx].toPointMsg(new_point);
00150               concave_list.insert(concave_list.begin() + i + 1, new_point);
00151               i--;
00152             }
00153         }
00154     }
00155 }
00156 
00157 
00158 void CostmapToPolygonsDBSConcaveHull::concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::Polygon& polygon)
00159 {
00160     // start with convex hull
00161     convexHull2(cluster, polygon);
00162 
00163     std::vector<geometry_msgs::Point32>& concave_list = polygon.points;
00164     
00165     // get line length
00166     double mean_length = 0;
00167     for (int i = 0; i < (int)concave_list.size() - 1; ++i)
00168     {
00169       mean_length += norm2d(concave_list[i],concave_list[i+1]);
00170     }
00171     mean_length /= double(concave_list.size());
00172 
00173     for (int i = 0; i < (int)concave_list.size() - 1; ++i)
00174     {
00175       
00176         // find nearest inner point pk from line (vertex1 -> vertex2)
00177         const geometry_msgs::Point32& vertex1 = concave_list[i];
00178         const geometry_msgs::Point32& vertex2 = concave_list[i+1];
00179 
00180         double line_length = norm2d(vertex1, vertex2);
00181         
00182         bool found;
00183         size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
00184         if (!found) 
00185         {
00186           continue;  
00187         }
00188         
00189                 
00190         double dst1 = norm2d(cluster[nearest_idx], vertex1);
00191         double dst2 = norm2d(cluster[nearest_idx], vertex2);
00192         double dd = std::min(dst1, dst2);
00193         if (dd<1e-8)
00194           continue;
00195 
00196         if (line_length / dd > depth)
00197         {
00198             // Check that new candidate edge will not intersect existing edges.
00199             bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
00200             intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
00201             if (!intersects) 
00202             {
00203               geometry_msgs::Point32 new_point;
00204               cluster[nearest_idx].toPointMsg(new_point);
00205               concave_list.insert(concave_list.begin() + i + 1, new_point);
00206               i--;
00207             }
00208         }
00209     }
00210 }
00211 
00212 
00213 
00214 
00215 void CostmapToPolygonsDBSConcaveHull::reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level)
00216 {
00217     max_distance_ = config.cluster_max_distance;
00218     min_pts_ = config.cluster_min_pts;
00219     max_pts_ = config.cluster_max_pts;
00220     min_keypoint_separation_ = config.cluster_min_pts;
00221     concave_hull_depth_ = config.concave_hull_depth;
00222 }
00223 
00224 }//end namespace costmap_converter
00225 
00226 


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat Jun 8 2019 20:53:15