costmap_to_lines_ransac.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
00037  *********************************************************************/
00038 
00039 #include <costmap_converter/costmap_to_lines_ransac.h>
00040 #include <boost/thread.hpp>
00041 #include <boost/thread/mutex.hpp>
00042 #include <pluginlib/class_list_macros.h>
00043 
00044 PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::BaseCostmapToPolygons)
00045 
00046 namespace costmap_converter
00047 {
00048 
00049 CostmapToLinesDBSRANSAC::CostmapToLinesDBSRANSAC() : CostmapToPolygonsDBSMCCH() 
00050 {
00051   dynamic_recfg_ = NULL;
00052 }
00053 
00054 CostmapToLinesDBSRANSAC::~CostmapToLinesDBSRANSAC() 
00055 {
00056   if (dynamic_recfg_ != NULL)
00057     delete dynamic_recfg_;
00058 }
00059   
00060 void CostmapToLinesDBSRANSAC::initialize(ros::NodeHandle nh)
00061 { 
00062     // DB SCAN
00063     max_distance_ = 0.4; 
00064     nh.param("cluster_max_distance", max_distance_, max_distance_);
00065     
00066     min_pts_ = 2;
00067     nh.param("cluster_min_pts", min_pts_, min_pts_);
00068     
00069     max_pts_ = 30;
00070     nh.param("cluster_max_pts", max_pts_, max_pts_);
00071 
00072     // ransac
00073     ransac_inlier_distance_ = 0.2;
00074     nh.param("ransac_inlier_distance", ransac_inlier_distance_, ransac_inlier_distance_);
00075     
00076     ransac_min_inliers_ = 10;
00077     nh.param("ransac_min_inliers", ransac_min_inliers_, ransac_min_inliers_);
00078     
00079     ransac_no_iterations_ = 2000;
00080     nh.param("ransac_no_iterations", ransac_no_iterations_, ransac_no_iterations_);
00081    
00082     ransac_remainig_outliers_ = 3;
00083     nh.param("ransac_remainig_outliers", ransac_remainig_outliers_, ransac_remainig_outliers_);
00084     
00085     ransac_convert_outlier_pts_ = true;
00086     nh.param("ransac_convert_outlier_pts", ransac_convert_outlier_pts_, ransac_convert_outlier_pts_);
00087     
00088     ransac_filter_remaining_outlier_pts_ = false;
00089     nh.param("ransac_filter_remaining_outlier_pts", ransac_filter_remaining_outlier_pts_, ransac_filter_remaining_outlier_pts_);
00090     
00091     // convex hull (only necessary if outlier filtering is enabled)
00092     min_keypoint_separation_ = 0.1;
00093     nh.param("convex_hull_min_pt_separation", min_keypoint_separation_, min_keypoint_separation_);
00094     
00095     // setup dynamic reconfigure
00096     dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>(nh);
00097     dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSRANSAC::reconfigureCB, this, _1, _2);
00098     dynamic_recfg_->setCallback(cb);
00099 }  
00100   
00101 void CostmapToLinesDBSRANSAC::compute()
00102 {
00103     std::vector< std::vector<KeyPoint> > clusters;
00104     dbScan(occupied_cells_, clusters);
00105     
00106     // Create new polygon container
00107     PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());  
00108     
00109     
00110     // fit lines using ransac for each cluster
00111     for (int i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
00112     { 
00113       
00114       while (clusters[i].size() > ransac_remainig_outliers_)
00115       {
00116 //         std::vector<KeyPoint> inliers;
00117         std::vector<KeyPoint> outliers;
00118         std::pair<KeyPoint,KeyPoint> model;
00119         if (!lineRansac(clusters[i], ransac_inlier_distance_, ransac_no_iterations_, ransac_min_inliers_, model, NULL, &outliers ) )
00120           break;
00121         
00122         // add to polygon container
00123         geometry_msgs::Polygon line;
00124         line.points.resize(2);
00125         model.first.toPointMsg(line.points.front());
00126         model.second.toPointMsg(line.points.back());
00127         polygons->push_back(line);
00128         
00129         clusters[i] = outliers;
00130       }
00131       // create point polygons for remaining outliers
00132       if (ransac_convert_outlier_pts_)
00133       {
00134         if (ransac_filter_remaining_outlier_pts_)
00135         {
00136           // take edge points of a convex polygon
00137           // these points define a cluster and since all lines are extracted,
00138           // we remove points from the interior...
00139           geometry_msgs::Polygon polygon;
00140           convexHull2(clusters[i], polygon);
00141           for (int j=0; j < (int)polygon.points.size(); ++j)
00142           {
00143             polygons->push_back(geometry_msgs::Polygon());
00144             convertPointToPolygon(polygon.points[j], polygons->back());
00145           }
00146         }
00147         else
00148         {
00149           for (int j = 0; j < (int)clusters[i].size(); ++j)
00150           {
00151             polygons->push_back(geometry_msgs::Polygon());
00152             convertPointToPolygon(clusters[i][j], polygons->back());           
00153           }
00154         }
00155 
00156       }
00157       
00158       
00159     }
00160     
00161     // add our non-cluster points to the polygon container (as single points)
00162     if (!clusters.empty())
00163     {
00164       for (int i=0; i < clusters.front().size(); ++i)
00165       {
00166         polygons->push_back( geometry_msgs::Polygon() );
00167         convertPointToPolygon(clusters.front()[i], polygons->back());
00168       }
00169     }
00170         
00171     // replace shared polygon container
00172     updatePolygonContainer(polygons);
00173 }
00174 
00175 
00176 bool CostmapToLinesDBSRANSAC::lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations,
00177                                          int min_inliers, std::pair<KeyPoint, KeyPoint>& best_model, 
00178                                          std::vector<KeyPoint>* inliers, std::vector<KeyPoint>* outliers)
00179 {
00180   if (data.size() < 2 ||  data.size() < min_inliers)
00181   {
00182     return false;
00183   }
00184   
00185   boost::random::uniform_int_distribution<> distribution(0, data.size()-1);
00186   
00187   std::pair<int, int> best_model_idx;
00188   int best_no_inliers = -1;
00189   
00190   
00191   for (int i=0; i < no_iterations; ++i)
00192   {
00193     // choose random points to define a line candidate
00194     int start_idx = distribution(rnd_generator_);
00195     int end_idx = start_idx;
00196     while (end_idx == start_idx)
00197       end_idx = distribution(rnd_generator_);
00198     
00199     
00200     // compute inliers
00201     int no_inliers = 0;
00202     for (int j=0; j<(int)data.size(); ++j)
00203     {
00204       if ( isInlier(data[j], data[start_idx], data[end_idx], inlier_distance) )
00205         ++no_inliers;
00206     }
00207     
00208     if (no_inliers > best_no_inliers)
00209     {
00210       best_no_inliers = no_inliers;
00211       best_model_idx.first = start_idx;
00212       best_model_idx.second = end_idx;
00213     }
00214   }
00215   
00216   best_model.first = data[best_model_idx.first];
00217   best_model.second = data[best_model_idx.second];
00218   
00219   if (best_no_inliers < min_inliers)
00220     return false;
00221   
00222   // Now repeat the calculation for the best model in order to obtain the inliers and outliers set
00223   // This might be faster if no_iterations is large, but TEST
00224   if (inliers || outliers)
00225   {
00226     if (inliers)
00227       inliers->clear();
00228     if (outliers)
00229       outliers->clear();
00230     
00231     int no_inliers = 0;
00232     for (int i=0; i<(int)data.size(); ++i)
00233     {
00234         if ( isInlier( data[i], best_model.first, best_model.second, inlier_distance ) )
00235         {
00236           if (inliers)
00237             inliers->push_back( data[i] );
00238         }
00239         else
00240         {
00241           if (outliers)
00242             outliers->push_back( data[i] );
00243         }
00244     }
00245   }
00246   
00247   return true;
00248 }
00249 
00250 bool CostmapToLinesDBSRANSAC::linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept, 
00251                                                double* mean_x_out, double* mean_y_out)
00252 {
00253   if (data.size() < 2)
00254   {
00255     ROS_ERROR("CostmapToLinesDBSRANSAC: at least 2 data points required for linear regression");
00256     return false;
00257   }
00258   
00259   double mean_x = 0;
00260   double mean_y = 0;
00261   for (int i=0; i<(int)data.size(); ++i)
00262   {
00263     mean_x += data[i].x;
00264     mean_y += data[i].y;
00265   }
00266   mean_x /= double(data.size());
00267   mean_y /= double(data.size());
00268   
00269   if (mean_x_out)
00270     *mean_x_out = mean_x;
00271  
00272   if (mean_y_out)
00273     *mean_y_out = mean_y;
00274   
00275   double numerator = 0.0;
00276   double denominator = 0.0;
00277 
00278   for(int i=0; i<(int)data.size(); ++i)
00279   {
00280       double dx = data[i].x - mean_x;
00281       numerator += dx * (data[i].y - mean_y);
00282       denominator += dx*dx;
00283   }
00284   
00285   if (denominator == 0)
00286   {
00287     ROS_ERROR("CostmapToLinesDBSRANSAC: linear regression failed, denominator 0");
00288     return false;
00289   }
00290   else
00291     slope = numerator / denominator;
00292   
00293   intercept = mean_y - slope * mean_x;
00294   return true;
00295 }
00296 
00297 
00298 void CostmapToLinesDBSRANSAC::reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level)
00299 {
00300     max_distance_ = config.cluster_max_distance;
00301     min_pts_ = config.cluster_min_pts;
00302     max_pts_ = config.cluster_max_pts;
00303     ransac_inlier_distance_ = config.ransac_inlier_distance;
00304     ransac_min_inliers_ = config.ransac_min_inliers;
00305     ransac_no_iterations_ = config.ransac_no_iterations;
00306     ransac_remainig_outliers_ = config.ransac_remainig_outliers;
00307     ransac_convert_outlier_pts_ = config.ransac_convert_outlier_pts;
00308     ransac_filter_remaining_outlier_pts_ = config.ransac_filter_remaining_outlier_pts;
00309     min_keypoint_separation_ = config.cluster_min_pts;
00310 }
00311 
00312 
00313 /*
00314 void CostmapToLinesDBSRANSAC::adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2, 
00315                                                KeyPoint& line_start, KeyPoint& line_end)
00316 {
00317   line_start = linept1; 
00318   line_end = linept2;
00319 
00320   // infinite line is defined by linept1 and linept2
00321   double dir_x = line_end.x - line_start.x;
00322   double dir_y = line_end.y - line_start.y;
00323   double norm = std::sqrt(dir_x*dir_x + dir_y*dir_y);
00324   dir_x /= norm;
00325   dir_y /= norm;
00326   
00327   // project data onto the line and check if the distance is increased in both directions
00328   for (int i=0; i < (int) data.size(); ++i)
00329   {
00330     double dx = data[i].x - line_start.x;
00331     double dy = data[i].y - line_start.y;
00332     // check scalar product at start
00333     double extension = dx*dir_x + dy*dir_y;
00334     if (extension<0)
00335     {
00336       line_start.x -=  dir_x*extension;
00337       line_start.y -=  dir_y*extension;
00338     }
00339     else
00340     {
00341       dx = data[i].x - line_end.x;
00342       dy = data[i].y - line_end.y;
00343       // check scalar product at end
00344       double extension = dx*dir_x + dy*dir_y;
00345       if (extension>0)
00346       {
00347         line_end.x +=  dir_x*extension;
00348         line_end.y +=  dir_y*extension;
00349       }
00350     }
00351   }
00352   
00353 }*/
00354 
00355 
00356 }//end namespace costmap_converter
00357 
00358 


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