costmap_to_lines_convex_hull.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_lines_convex_hull.h>
00040 #include <costmap_converter/misc.h>
00041 #include <boost/thread.hpp>
00042 #include <boost/thread/mutex.hpp>
00043 #include <pluginlib/class_list_macros.h>
00044 
00045 PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::BaseCostmapToPolygons)
00046 
00047 namespace costmap_converter
00048 {
00049 
00050 CostmapToLinesDBSMCCH::CostmapToLinesDBSMCCH() : CostmapToPolygonsDBSMCCH() 
00051 {
00052     dynamic_recfg_ = NULL;
00053 }
00054   
00055 CostmapToLinesDBSMCCH::~CostmapToLinesDBSMCCH() 
00056 {
00057   if (dynamic_recfg_ != NULL)
00058     delete dynamic_recfg_;
00059 }
00060   
00061 void CostmapToLinesDBSMCCH::initialize(ros::NodeHandle nh)
00062 { 
00063     // DB SCAN
00064     max_distance_ = 0.4; 
00065     nh.param("cluster_max_distance", max_distance_, max_distance_);
00066     
00067     min_pts_ = 2;
00068     nh.param("cluster_min_pts", min_pts_, min_pts_);
00069     
00070     max_pts_ = 30;
00071     nh.param("cluster_max_pts", max_pts_, max_pts_);
00072     
00073     // convex hull
00074     min_keypoint_separation_ = 0.1;
00075     nh.param("convex_hull_min_pt_separation", min_keypoint_separation_, min_keypoint_separation_);
00076     
00077     // Line extraction
00078     support_pts_max_dist_ = 0.3;
00079     nh.param("support_pts_max_dist", support_pts_max_dist_, support_pts_max_dist_);
00080     
00081     support_pts_max_dist_inbetween_ = 1.0;
00082     nh.param("support_pts_max_dist_inbetween", support_pts_max_dist_inbetween_, support_pts_max_dist_inbetween_);
00083     
00084     min_support_pts_ = 2;
00085     nh.param("min_support_pts", min_support_pts_, min_support_pts_);
00086     
00087     // setup dynamic reconfigure
00088     dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>(nh);
00089     dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSMCCH::reconfigureCB, this, _1, _2);
00090     dynamic_recfg_->setCallback(cb);
00091     
00092     // deprecated
00093     if (nh.hasParam("support_pts_min_dist_") || nh.hasParam("support_pts_min_dist"))
00094       ROS_WARN("CostmapToLinesDBSMCCH: Parameter 'support_pts_min_dist' is deprecated and not included anymore.");
00095     if (nh.hasParam("min_support_pts_"))
00096       ROS_WARN("CostmapToLinesDBSMCCH: Parameter 'min_support_pts_' is not found. Remove the underscore.");
00097 }  
00098   
00099 void CostmapToLinesDBSMCCH::compute()
00100 {
00101     std::vector< std::vector<KeyPoint> > clusters;
00102     dbScan(occupied_cells_, clusters);
00103     
00104     // Create new polygon container
00105     PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());  
00106     
00107     
00108     // add convex hulls to polygon container
00109     for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
00110     {
00111       geometry_msgs::Polygon polygon;
00112       convexHull2(clusters[i], polygon );
00113       
00114       // now extract lines of the polygon (by searching for support points in the cluster)  
00115       // and add them to the polygon container
00116       extractPointsAndLines(clusters[i], polygon, std::back_inserter(*polygons));
00117     }
00118     
00119     // add our non-cluster points to the polygon container (as single points)
00120     if (!clusters.empty())
00121     {
00122       for (int i=0; i < clusters.front().size(); ++i)
00123       {
00124         polygons->push_back( geometry_msgs::Polygon() );
00125         convertPointToPolygon(clusters.front()[i], polygons->back());
00126       }
00127     }
00128         
00129     // replace shared polygon container
00130     updatePolygonContainer(polygons);
00131 }
00132 
00133 typedef CostmapToLinesDBSMCCH CL;
00134 bool sort_keypoint_x(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) 
00135 { return (cluster[i].x<cluster[j].x) || (cluster[i].x == cluster[j].x && cluster[i].y < cluster[j].y); }
00136 bool sort_keypoint_y(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) 
00137 { return (cluster[i].y<cluster[j].y) || (cluster[i].y == cluster[j].y && cluster[i].x < cluster[j].x); }
00138 
00139 void CostmapToLinesDBSMCCH::extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::Polygon& polygon,
00140                                                   std::back_insert_iterator< std::vector<geometry_msgs::Polygon> > lines)
00141 {
00142    if (polygon.points.empty())
00143      return;
00144   
00145    if (polygon.points.size() < 2)
00146    {
00147      lines = polygon; // our polygon is just a point, push back onto the output sequence
00148      return;
00149    }
00150    int n = (int)polygon.points.size();
00151      
00152    for (int i=1; i<n; ++i) // this implemenation requires an explicitly closed polygon (start vertex = end vertex)
00153    {
00154         const geometry_msgs::Point32* vertex1 = &polygon.points[i-1];
00155         const geometry_msgs::Point32* vertex2 = &polygon.points[i];
00156 
00157         // check how many vertices belong to the line (sometimes the convex hull algorithm finds multiple vertices on a line,
00158         // in case of small coordinate deviations)
00159         double dx = vertex2->x - vertex1->x;
00160         double dy = vertex2->y - vertex1->y;
00161 //         double norm = std::sqrt(dx*dx + dy*dy);
00162 //         dx /= norm;
00163 //         dy /= norm;
00164 //         for (int j=i; j<(int)polygon.points.size() - 2; ++j)
00165 //         {
00166 //           const geometry_msgs::Point32* vertex_jp2 = &polygon.points[j+2];
00167 //           double dx2 = vertex_jp2->x - vertex2->x;
00168 //           double dy2 = vertex_jp2->y - vertex2->y;
00169 //           double norm2 = std::sqrt(dx2*dx2 + dy2*dy2);
00170 //           dx2 /= norm2;
00171 //           dy2 /= norm2;
00172 //           if (std::abs(dx*dx2 + dy*dy2) < 0.05) //~3 degree
00173 //           {
00174 //             vertex2 = &polygon.points[j+2];
00175 //             i = j; // DO NOT USE "i" afterwards
00176 //           }
00177 //           else break;
00178 //         }
00179        
00180         //Search support points
00181         std::vector<std::size_t> support_points; // store indices of cluster
00182         
00183         for (std::size_t k = 0; k < cluster.size(); ++k)
00184         {
00185             bool is_inbetween = false;
00186             double dist_line_to_point = computeDistanceToLineSegment( cluster[k], *vertex1, *vertex2, &is_inbetween );
00187 
00188             if(is_inbetween && dist_line_to_point <= support_pts_max_dist_)
00189             {
00190               support_points.push_back(k);
00191               continue;
00192             }
00193         }
00194 
00195         // now check if the inlier models a line by checking the minimum distance between all support points (avoid lines over gaps)
00196         // and by checking if the minium number of points is reached // deactivate by setting support_pts_max_dist_inbetween_==0
00197         bool is_line=true;
00198         if (support_pts_max_dist_inbetween_!=0)
00199         {
00200           if ((int)support_points.size() >= min_support_pts_ + 2) // +2 since start and goal are included
00201           {          
00202             // sort points
00203             if (std::abs(dx) >= std::abs(dy))
00204               std::sort(support_points.begin(), support_points.end(), boost::bind(sort_keypoint_x, _1, _2, boost::cref(cluster)));
00205             else 
00206               std::sort(support_points.begin(), support_points.end(), boost::bind(sort_keypoint_y, _1, _2, boost::cref(cluster)));
00207             
00208             // now calculate distances
00209             for (int k = 1; k < int(support_points.size()); ++k)
00210             {
00211               double dist_x = cluster[support_points[k]].x - cluster[support_points[k-1]].x;
00212               double dist_y = cluster[support_points[k]].y - cluster[support_points[k-1]].y;
00213               double dist = std::sqrt( dist_x*dist_x + dist_y*dist_y);
00214               if (dist > support_pts_max_dist_inbetween_)
00215               {
00216                 is_line = false;
00217                 break;
00218               }
00219             }
00220             
00221           }
00222           else
00223             is_line = false;
00224         }
00225         
00226         if (is_line)
00227         {
00228           // line found:
00229           geometry_msgs::Polygon line;
00230           line.points.push_back(*vertex1);
00231           line.points.push_back(*vertex2);
00232           lines = line; // back insertion
00233           
00234           // remove inlier from list
00235           // iterate in reverse order, otherwise indices are not valid after erasing elements
00236           std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
00237           for (; support_it != support_points.rend(); ++support_it)
00238           {
00239             cluster.erase(cluster.begin() + *support_it);
00240           }
00241         }
00242         else
00243         {
00244             // remove goal, since it will be added in the subsequent iteration
00245             //support_points.pop_back();
00246           // old:
00247 //             // add vertex 1 as single point
00248 //             geometry_msgs::Polygon point;
00249 //             point.points.push_back(*vertex1);
00250 //             lines = point; // back insertion
00251 
00252            // add complete inlier set as points 
00253 //            for (int k = 0; k < int(support_points.size()); ++k)
00254 //            {
00255 //               geometry_msgs::Polygon polygon;
00256 //               convertPointToPolygon(cluster[support_points[k]], polygon);
00257 //               lines = polygon; // back insertion
00258 //            }
00259            
00260           // remove inlier from list and add them as point obstacles
00261           // iterate in reverse order, otherwise indices are not valid after erasing elements
00262           std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
00263           for (; support_it != support_points.rend(); ++support_it)
00264           {
00265             geometry_msgs::Polygon polygon;
00266             convertPointToPolygon(cluster[*support_it], polygon);
00267             lines = polygon; // back insertion
00268             
00269             cluster.erase(cluster.begin() + *support_it);
00270           }
00271         }
00272     }
00273  
00274     // add all remaining cluster points, that do not belong to a line
00275     for (int i=0; i<(int)cluster.size();++i)
00276     {
00277         geometry_msgs::Polygon polygon;
00278         convertPointToPolygon(cluster[i], polygon);
00279         lines = polygon; // back insertion
00280     }
00281 
00282 }
00283 
00284 void CostmapToLinesDBSMCCH::reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level)
00285 {
00286     max_distance_ = config.cluster_max_distance;
00287     min_pts_ = config.cluster_min_pts;
00288     max_pts_ = config.cluster_max_pts;
00289     min_keypoint_separation_ = config.cluster_min_pts;
00290     support_pts_max_dist_ = config.support_pts_max_dist;
00291     support_pts_max_dist_inbetween_ = config.support_pts_max_dist_inbetween;
00292     min_support_pts_ = config.min_support_pts;
00293 }
00294 
00295 
00296 
00297 }//end namespace costmap_converter
00298 
00299 


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