filter.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  */
00030 
00039 #include <filters/filter_base.h>
00040 #include <pluginlib/class_list_macros.h>
00041 #include <sensor_msgs/PointCloud.h>
00042 #include <tf/transform_listener.h>
00043 #include <boost/foreach.hpp>
00044 #include <boost/circular_buffer.hpp>
00045 
00046 namespace isolated_point_filter
00047 {
00048 
00049 namespace sm=sensor_msgs;
00050 namespace gm=geometry_msgs;
00051 using std::string;
00052 using std::vector;
00053 
00054 
00055 
00056 class IsolatedPointFilter : public filters::FilterBase<sm::PointCloud>
00057 {
00058 public:
00059 
00060   IsolatedPointFilter ();
00061   bool configure ();
00062   bool update (const sm::PointCloud& in, sm::PointCloud& out);
00063 
00064 private:
00065 
00066   unsigned getIndex (int x, int y) const;
00067   void resetStamps ();
00068   bool withinBounds (const gm::Point32& p) const;
00069   void getCoords (const gm::Point32& p, int* x, int* y) const;
00070   template <class T>
00071   void readParam (const string& name, T& place);
00072 
00073   ros::NodeHandle nh_;
00074   tf::TransformListener tf_;
00075 
00076   // params
00077   int nx_;
00078   int ny_;
00079   double resolution_;
00080   string frame_;
00081   string fixed_frame_;
00082   double x_min_;
00083   double y_min_;
00084   double x_max_;
00085   double y_max_;
00086   double z_max_;
00087   int r_big_;
00088   int r_small_;
00089   int negate_;
00090   unsigned buffer_size_;
00091   
00092 
00093   // state
00094   unsigned counter_;
00095   vector<unsigned> stamps_;
00096   boost::circular_buffer<sm::PointCloud> cloud_buffer_; // Because the 'clouds' in our case are just lines
00097   
00098 };
00099 
00100 IsolatedPointFilter::IsolatedPointFilter () :
00101   tf_(nh_), fixed_frame_("odom_combined"), z_max_(10), buffer_size_(1), counter_(0), cloud_buffer_(1)
00102 {}
00103 
00104 template <class T>
00105 void IsolatedPointFilter::readParam (const string& name, T& place)
00106 {
00107   bool found = getParam(name, place);
00108   if (!found) {
00109     ROS_FATAL_STREAM ("Did not find parameter " << name);
00110     ROS_BREAK();
00111   }
00112 }
00113 
00114 bool IsolatedPointFilter::configure ()
00115 {
00116   readParam("frame", frame_);
00117   readParam("resolution", resolution_);
00118   readParam("x_min", x_min_);
00119   readParam("x_max", x_max_);
00120   readParam("y_min", y_min_);
00121   readParam("y_max", y_max_);
00122   getParam("z_max", z_max_);
00123   readParam("inner_radius", r_small_);
00124   readParam("outer_radius", r_big_);
00125   getParam("negate", negate_);
00126   getParam("buffer_size", buffer_size_);
00127   getParam("fixed_frame", fixed_frame_);
00128   nx_ = ceil((x_max_-x_min_)/resolution_);
00129   ny_ = ceil((y_max_-y_min_)/resolution_);
00130   stamps_.resize(nx_*ny_);
00131   cloud_buffer_.resize(buffer_size_);
00132   ROS_INFO_STREAM ("Initialized isolated_point_filter.  Frame is " << frame_ << ", x bounds are " << x_min_ <<
00133                    ", " << x_max_ << "; y bounds are " << y_min_ << ", " << y_max_ << " points above " <<
00134                    z_max_ << " will never be considered isolated; box radii are " << r_small_ << ", " <<
00135                    r_big_  << " grid size is " << nx_ << " x " << ny_ << ", bufsize = " << buffer_size_);
00136   ROS_ASSERT_MSG (x_min_ < x_max_ && y_min_ < y_max_ && r_small_ < r_big_ && 0 <= r_small_ && 0 < z_max_,
00137                   "Params don't pass sanity check");
00138   return true;
00139 }
00140 
00141 void IsolatedPointFilter::resetStamps ()
00142 {
00143   fill(stamps_.begin(), stamps_.end(), 0);
00144 }
00145 
00146 unsigned IsolatedPointFilter::getIndex (const int x, const int y) const
00147 {
00148   ROS_ASSERT ((x>=0) && (x<nx_) && (y>=0) && (y<ny_));
00149   return ny_*x + y;
00150 }
00151 
00152 bool IsolatedPointFilter::withinBounds (const gm::Point32& p) const
00153 {
00154   return ((p.x > x_min_) && (p.x < x_max_) && (p.y > y_min_) && (p.y < y_max_) &&
00155           (p.z < z_max_));
00156 }
00157 
00158 void IsolatedPointFilter::getCoords (const gm::Point32& p, int* x, int* y) const
00159 {
00160   *x = floor((p.x-x_min_)/resolution_);
00161   *y = floor((p.y-y_min_)/resolution_);
00162   ROS_ASSERT ((*x>=0) && (*x < nx_) && (*y>=0) && (*y < ny_));
00163 }
00164 
00165 
00166 bool IsolatedPointFilter::update (const sm::PointCloud& in, sm::PointCloud& out)
00167 {
00168   out.points.resize(0);
00169   out.header.frame_id = frame_;
00170   out.header.stamp = in.header.stamp;
00171   if (counter_==0) {
00172     resetStamps();
00173   }
00174   counter_++;
00175   ROS_DEBUG_STREAM_NAMED ("update", "In update loop " << counter_ <<
00176                           " on point cloud with " << in.points.size() << " points");
00177 
00178 
00179   try {
00180     sm::PointCloud transformed;
00181     tf_.transformPointCloud (fixed_frame_, in, transformed);
00182     cloud_buffer_.push_back(transformed);
00183   }
00184   catch (tf::TransformException& e) {
00185     ROS_WARN_STREAM ("Received tf exception " << e.what() << "; skipping cloud");
00186     return false;
00187   }
00188   if (cloud_buffer_.size() < buffer_size_) {
00189     // skip
00190     // out = *(--cloud_buffer_.end());
00191     return true;
00192   }
00193   else {
00194 
00195     sm::PointCloud combined, last;
00196     try {
00197       tf_.transformPointCloud (frame_, *(--cloud_buffer_.end()), last);
00198       BOOST_FOREACH (const sm::PointCloud& c, cloud_buffer_) {
00199         sm::PointCloud transformed;
00200         tf_.transformPointCloud (frame_, c, transformed);
00201         BOOST_FOREACH (const gm::Point32& p, transformed.points) {
00202           combined.points.push_back(p);
00203         }
00204       }
00205     }
00206     catch (tf::TransformException& e) {
00207       if (!negate_) {
00208         ROS_DEBUG_STREAM_NAMED ("transforms", "Received tf exception " << e.what () << "; passing along cloud");
00209         out.points = last.points;
00210       }
00211       return true;
00212     }
00213 
00214     // mark each cell that is occupied by a point in this cloud
00215     BOOST_FOREACH (const gm::Point32& p, combined.points) {
00216       if (withinBounds(p)) {
00217         int x, y;
00218         getCoords(p, &x, &y);
00219         stamps_[getIndex(x, y)] = counter_;
00220       }
00221     }
00222 
00223     BOOST_FOREACH (const gm::Point32& p, last.points) {
00224       bool is_isolated;
00225       if (withinBounds(p)) {
00226         int x0, y0;
00227         bool found=false;
00228         getCoords(p, &x0, &y0);
00229         for (int x = x0-r_big_; x <= x0+r_big_; x++) {
00230           if ((x<0) || (x>=nx_))
00231             continue; // Out of map bounds
00232           for (int y = y0-r_big_; y <= y0+r_big_; y++) {
00233             // OK, (x,y) now represents a point near (x0, y0)
00234           
00235             // Skip it if it's outside map bounds
00236             if ((y<0) || (y>=ny_))
00237               continue; 
00238           
00239             // Skip it if it's within the inner square at (x0, y0)
00240             if ((x>=x0-r_small_) && (x<=x0+r_small_) &&
00241                 (y>=y0-r_small_) && (y<=y0+r_small_))
00242               continue; 
00243 
00244             // Else if it has a point, that means (x0, y0) has a neighbor
00245             if (stamps_[getIndex(x, y)] == counter_) {
00246               found = true;
00247               break;
00248             }
00249           }
00250           if (found)
00251             break;
00252         }
00253         if (found)
00254           is_isolated = false;
00255         else {
00256           is_isolated = true;
00257           ROS_DEBUG_STREAM_NAMED ("isolated", " found isolated point " << p);
00258         }
00259       }
00260     
00261       // Points not in the filter bounds are always kept
00262       else 
00263         is_isolated = false;
00264 
00265       if ((is_isolated && negate_) || (!is_isolated && !negate_))
00266         out.points.push_back(p);
00267     }
00268 
00269     out.header.frame_id = frame_;
00270     out.header.stamp = in.header.stamp;
00271     ROS_DEBUG_STREAM_NAMED ("update", "Update loop finished with " << out.points.size() << " points in "
00272                             << frame_ << " frame");
00273   }
00274   return true;
00275 }
00276  
00277 
00278 } // namespace
00279 
00280 PLUGINLIB_DECLARE_CLASS(isolated_point_filter, IsolatedPointFilter,
00281                         isolated_point_filter::IsolatedPointFilter,
00282                         filters::FilterBase<sensor_msgs::PointCloud>)
00283 


isolated_point_filter
Author(s): Bhaskara Marthi
autogenerated on Tue Jan 7 2014 11:11:54