laser_processor.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include "laser_processor.h"
00036 
00037 #include <stdexcept>
00038 
00039 using namespace ros;
00040 using namespace std;
00041 using namespace laser_processor;
00042 
00043 Sample* Sample::Extract(int ind, const sensor_msgs::LaserScan& scan)
00044 {
00045   Sample* s = new Sample();
00046 
00047   s->index = ind;
00048   s->range = scan.ranges[ind];
00049   s->x = cos( scan.angle_min + ind*scan.angle_increment ) * s->range;
00050   s->y = sin( scan.angle_min + ind*scan.angle_increment ) * s->range;
00051   if (s->range > scan.range_min && s->range < scan.range_max)
00052     return s;
00053   else
00054   {
00055     delete s;
00056     return NULL;
00057   }
00058 }
00059 
00060 void SampleSet::clear()
00061 {
00062   for (SampleSet::iterator i = begin();
00063        i != end();
00064        i++)
00065   {
00066     delete (*i);
00067   }
00068   set<Sample*, CompareSample>::clear();
00069 }
00070 
00071 void SampleSet::appendToCloud(sensor_msgs::PointCloud& cloud, int r, int g, int b)
00072 {
00073   float color_val = 0;
00074 
00075   int rgb = (r << 16) | (g << 8) | b;
00076   color_val = *(float*)&(rgb);
00077 
00078   for (iterator sample_iter = begin();
00079        sample_iter != end();
00080        sample_iter++)
00081   {
00082     geometry_msgs::Point32 point;
00083     point.x = (*sample_iter)->x;
00084     point.y = (*sample_iter)->y;
00085     point.z = 0;
00086 
00087     cloud.points.push_back(point);
00088 
00089     if (cloud.channels[0].name == "rgb")
00090       cloud.channels[0].values.push_back(color_val);
00091   }
00092 }
00093 
00094 tf::Point SampleSet::center()
00095 {
00096   float x_mean = 0.0;
00097   float y_mean = 0.0;
00098   for (iterator i = begin();
00099        i != end();
00100        i++)
00101 
00102   {
00103     x_mean += ((*i)->x)/size();
00104     y_mean += ((*i)->y)/size();
00105   }
00106 
00107   return tf::Point (x_mean, y_mean, 0.0);
00108 }
00109 
00110 
00111 void Background::addScan(sensor_msgs::LaserScan& scan, float treshhold) // treshhold to know whether to merge them or to start from scrach
00112 {
00113  if (!filled)
00114   {
00115     angle_min = scan.angle_min;
00116     angle_max = scan.angle_max;
00117     size      = scan.ranges.size();
00118     filled    = true;
00119   } else if (angle_min != scan.angle_min     ||  // min and max angles of the new scan have to be the same as previous
00120              angle_max != scan.angle_max     ||
00121              size      != scan.ranges.size())
00122   {
00123     throw std::runtime_error("laser_scan::Background::addScan: inconsistantly sized scans added to background");
00124   }
00125 
00126   for (uint32_t i = 0; i < scan.ranges.size(); i++)
00127   {
00128   
00129    Sample* s = Sample::Extract(i, scan);
00130    if (s != NULL)
00131     {
00132       s->scans = 1; // sets the number of scans 
00133       SampleSet::iterator m = backgr_data.find(s);
00134       if (m != backgr_data.end())  // there is such a scan 
00135       {
00136         if ( s->range > 29.8 || s->range < 0.1 || abs((*m)->range - s->range) > treshhold  )  // if the difference between the new sample and the old one is bigger then will it ignore the new one
00137         {
00138    
00139     //        printf ("laser_scan::Background::addScan: too much movement %f - ignoring the new scan \n",(*m)->range - s->range); 
00140             delete s;
00141     //      backgr_data.erase(m);  // delete the sample from the mask
00142     //      backgr_data.insert(s); // insert the new sample
00143         } else { // if the movement is within the treshhold then just re-adjust the center and the variation of the while preserving the old scan
00144       //    printf ("d:%f ", (*m)->range - s->range); 
00145            (*m)->range = (((*m)->range*(*m)->scans)+ s->range)/ (++((*m)->scans)); // aritmetic average
00146       //     printf ("r:%f ", (*m)->range); 
00147       //     printf ("s:%i ", (*m)->scans);
00148            (*m)->variation = ((*m)->range - s->range)/2+ (*m)->variation/2; // old variations are reduced in wegth
00149       //     printf ("v:%f ", (*m)->variation);
00150            delete s; 
00151         }
00152       }
00153       else if (s->range < 29.8 && s->range > 0.1) // if there is no such sample in the background found and the range makes sence just insert the new sample
00154       {
00155         backgr_data.insert(s);
00156       }  
00157      } 
00158   }
00159 
00160 }
00161 
00162 
00163 void ScanMask::addScan(sensor_msgs::LaserScan& scan)
00164 {
00165   if (!filled)
00166   {
00167     angle_min = scan.angle_min;
00168     angle_max = scan.angle_max;
00169     size      = scan.ranges.size();
00170     filled    = true;
00171   } else if (angle_min != scan.angle_min     ||  // min and max angles of the new scan have to be the same as previous
00172              angle_max != scan.angle_max     ||
00173              size      != scan.ranges.size())
00174   {
00175     throw std::runtime_error("laser_scan::ScanMask::addScan: inconsistantly sized scans added to mask");
00176   }
00177 
00178   for (uint32_t i = 0; i < scan.ranges.size(); i++)
00179   {
00180     Sample* s = Sample::Extract(i, scan);
00181 
00182     if (s != NULL)
00183     {
00184       SampleSet::iterator m = mask_.find(s);
00185 
00186       if (m != mask_.end())  // at least one such a sample in the mask is found
00187       {
00188         if ((*m)->range > s->range)  // if the sample(s) in the mask is further away  than the current sample replaces the sample in the mask to remove the background
00189         {
00190           delete (*m);
00191           mask_.erase(m);  // delete the sample from the mask
00192           mask_.insert(s); // insert the new sample
00193         } else {
00194           delete s; 
00195         }
00196       }
00197       else // if there is no such sample in the mask not found just insert the new sample
00198       {
00199         mask_.insert(s);
00200       }
00201     }
00202   }
00203 }
00204 
00205 
00206 bool Background::isSamplebelongstoBackgrond(Sample* s, float thresh)
00207 {
00208   if (s != NULL)
00209   {
00210     SampleSet::iterator b = backgr_data.find(s); 
00211     if ( b != backgr_data.end())
00212       if ( (s-> range > 29.8) || ( ((*b)->range + abs((*b)->variation) + thresh > s->range) &&  ((*b)->range - abs((*b)->variation) - thresh < s->range) ) ) // in other words s-range between what we have in the background +- allowances
00213         return true;
00214   }
00215 
00216   return false;
00217 }
00218 
00219 bool ScanMask::hasSample(Sample* s, float thresh)
00220 {
00221   if (s != NULL)
00222   {
00223     SampleSet::iterator m = mask_.find(s);
00224     if ( m != mask_.end())
00225       if ( s-> range > 29.8 || (((*m)->range - thresh) < s->range) )
00226         return true;
00227   }
00228   return false;
00229 }
00230 
00231 
00232 
00233 ScanProcessor::ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold) // constructor without background
00234 {
00235   scan_ = scan;
00236 
00237   SampleSet* cluster = new SampleSet;
00238 
00239   for (uint32_t i = 0; i < scan.ranges.size(); i++)
00240   {
00241     Sample* s = Sample::Extract(i, scan);
00242 
00243     if (s != NULL)
00244     {
00245       if (!mask_.hasSample(s, mask_threshold))
00246       {
00247         cluster->insert(s);
00248       } else {
00249         delete s;
00250       }
00251     }
00252   }
00253 
00254   clusters_.push_back(cluster);
00255 
00256 }
00257 
00258 
00259 ScanProcessor::ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, Background& background_ , float mask_threshold , float background_treshhold ) // constructor with background
00260 
00261 {
00262 
00263 
00264 scan_ = scan;
00265 
00266 int ib= 0;
00267 int im = 0;
00268 int iboth = 0;
00269 int ibnotim = 0;
00270 int imnotib = 0;
00271 
00272   SampleSet* cluster = new SampleSet;
00273 
00274   for (uint32_t i = 0; i < scan.ranges.size(); i++)
00275   {
00276     Sample* s = Sample::Extract(i, scan);
00277 
00278     if (s != NULL)
00279     {
00280  
00281      if (mask_.hasSample(s, mask_threshold) )
00282           im++;    // the sample is in the mask
00283      if (background_.isSamplebelongstoBackgrond(s, background_treshhold))
00284           ib++;    // the sample is in the background
00285   
00286       if (mask_.hasSample(s, mask_threshold) && background_.isSamplebelongstoBackgrond(s, background_treshhold))
00287           iboth++; // the sample is in both
00288 
00289       if (mask_.hasSample(s, mask_threshold) && !background_.isSamplebelongstoBackgrond(s, background_treshhold))
00290           imnotib++; // in the mask but not in the background
00291 
00292       if (!mask_.hasSample(s, mask_threshold) && background_.isSamplebelongstoBackgrond(s, background_treshhold))
00293           ibnotim++; // in background but not in the mask
00294      
00295 
00296 
00297       if (  !mask_.hasSample(s, mask_threshold)  && !background_.isSamplebelongstoBackgrond(s, background_treshhold)) 
00298       {
00299         cluster->insert(s);
00300       } else {
00301         delete s;
00302       }
00303     }
00304   
00305  //if ( ibnotim > 0)  //printing the results of the checks
00306  //  printf ("ib=%i, im=%i, iboth=%i, ibnotim=%i, imnotib=%i \n", ib, im, iboth, ibnotim, imnotib); // check for the work of the background and the mask filters
00307  }
00308 
00309   clusters_.push_back(cluster); 
00310 
00311 }
00312 
00313 ScanProcessor::~ScanProcessor()
00314 {
00315   for ( list<SampleSet*>::iterator c = clusters_.begin();
00316         c != clusters_.end();
00317         c++)
00318     delete (*c);
00319 }
00320 
00321 void
00322 ScanProcessor::removeLessThan(uint32_t num)
00323 {
00324   list<SampleSet*>::iterator c_iter = clusters_.begin();
00325   while (c_iter != clusters_.end())
00326   {
00327     if ( (*c_iter)->size() < num )
00328     {
00329       delete (*c_iter);
00330       clusters_.erase(c_iter++);
00331     } else {
00332       ++c_iter;
00333     }
00334   }
00335 }
00336 
00337 
00338 void
00339 ScanProcessor::splitConnected(float thresh)
00340 {
00341   list<SampleSet*> tmp_clusters;
00342 
00343   list<SampleSet*>::iterator c_iter = clusters_.begin();
00344 
00345   // For each cluster
00346   while (c_iter != clusters_.end())
00347   {
00348     // Go through the entire list
00349     while ((*c_iter)->size() > 0 )
00350     {
00351       // Take the first element
00352       SampleSet::iterator s_first = (*c_iter)->begin();
00353 
00354       // Start a new queue
00355       list<Sample*> sample_queue;
00356       sample_queue.push_back(*s_first);
00357 
00358       (*c_iter)->erase(s_first);
00359 
00360       // Grow until we get to the end of the queue
00361       list<Sample*>::iterator s_q = sample_queue.begin();
00362       while (s_q != sample_queue.end())
00363       {
00364         int expand = (int)(asin( thresh / (*s_q)->range ) / scan_.angle_increment);
00365 
00366         SampleSet::iterator s_rest = (*c_iter)->begin();
00367 
00368         while ( (s_rest != (*c_iter)->end() &&
00369                  (*s_rest)->index < (*s_q)->index + expand ) )
00370         {
00371           if ( (*s_rest)->range - (*s_q)->range > thresh)
00372           {
00373             break;
00374           }
00375           else if (sqrt( pow( (*s_q)->x - (*s_rest)->x, 2.0f) + pow( (*s_q)->y - (*s_rest)->y, 2.0f)) < thresh)  // check whether to split
00376           {
00377             sample_queue.push_back(*s_rest);
00378             (*c_iter)->erase(s_rest++);
00379             break;
00380           } else {
00381             ++s_rest;
00382           }
00383         }
00384         s_q++;
00385       }
00386 
00387       // Move all the samples into the new cluster
00388       SampleSet* c = new SampleSet;
00389       for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++)
00390         c->insert(*s_q);
00391 
00392       // Store the temporary clusters
00393       tmp_clusters.push_back(c);
00394     }
00395 
00396     //Now that c_iter is empty, we can delete
00397     delete (*c_iter);
00398 
00399     //And remove from the map
00400     clusters_.erase(c_iter++);
00401   }
00402 
00403   clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end()); // move back from temp clusters back into the clusters_
00404 }


srs_leg_detector
Author(s): Alex Noyvirt
autogenerated on Sun Jan 5 2014 12:20:17