00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
00094 unsigned counter_;
00095 vector<unsigned> stamps_;
00096 boost::circular_buffer<sm::PointCloud> cloud_buffer_;
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
00190
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
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;
00232 for (int y = y0-r_big_; y <= y0+r_big_; y++) {
00233
00234
00235
00236 if ((y<0) || (y>=ny_))
00237 continue;
00238
00239
00240 if ((x>=x0-r_small_) && (x<=x0+r_small_) &&
00241 (y>=y0-r_small_) && (y<=y0+r_small_))
00242 continue;
00243
00244
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
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 }
00279
00280 PLUGINLIB_DECLARE_CLASS(isolated_point_filter, IsolatedPointFilter,
00281 isolated_point_filter::IsolatedPointFilter,
00282 filters::FilterBase<sensor_msgs::PointCloud>)
00283