$search
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