Point Cloud Library (PCL)  1.3.1
conditional_removal.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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 Willow Garage, Inc. 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  * $Id: conditional_removal.hpp 2433 2011-09-07 23:04:40Z mdixon $
00035  *
00036  */
00037 
00038 #ifndef PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_
00039 #define PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_
00040 
00041 #include <pcl/common/io.h>
00042 #include <boost/shared_ptr.hpp>
00043 #include <vector>
00044 
00048 template <typename PointT>
00049 pcl::FieldComparison<PointT>::FieldComparison (
00050     std::string field_name, ComparisonOps::CompareOp op, double compare_val) :
00051       compare_val_(compare_val), point_data_(NULL)
00052 {
00053   field_name_ = field_name;
00054   op_ = op;
00055 
00056   // Get all fields
00057   std::vector<sensor_msgs::PointField> point_fields; 
00058   // Use a dummy cloud to get the field types in a clever way
00059   PointCloud<PointT> dummyCloud;
00060   pcl::getFields (dummyCloud, point_fields);
00061 
00062   // Find field_name
00063   if (point_fields.empty ())
00064   {
00065     PCL_WARN ("[pcl::FieldComparison::FieldComparison] no fields found!\n");
00066     capable_ = false;
00067     return;
00068   }
00069 
00070   // Get the field index
00071   size_t d;
00072   for (d = 0; d < point_fields.size (); ++d)
00073   {
00074     if (point_fields[d].name == field_name) 
00075       break;
00076   }
00077   
00078   if (d == point_fields.size ())
00079   {
00080     PCL_WARN ("[pcl::FieldComparison::FieldComparison] field not found!\n");
00081     capable_ = false;
00082     return;
00083   }
00084   uint8_t datatype = point_fields[d].datatype;
00085   uint32_t offset = point_fields[d].offset;
00086 
00087   point_data_ = new PointDataAtOffset<PointT>(datatype, offset);
00088   capable_ = true;
00089 }
00090 
00092 template <typename PointT>
00093 pcl::FieldComparison<PointT>::~FieldComparison () 
00094 {
00095   if (point_data_ != NULL)
00096   {
00097     delete point_data_;
00098     point_data_ = NULL;
00099   }
00100 }
00101 
00103 template <typename PointT> bool
00104 pcl::FieldComparison<PointT>::evaluate (const PointT &point) const
00105 {
00106   if (!this->capable_)
00107   {
00108     PCL_WARN ("[pcl::FieldComparison::evaluate] invalid compariosn!\n");
00109     return (false);
00110   }
00111 
00112   // if p(data) > val then compare_result = 1
00113   // if p(data) == val then compare_result = 0
00114   // if p(data) <  ival then compare_result = -1
00115   int compare_result = point_data_->compare (point, compare_val_);
00116   
00117   switch (this->op_)
00118   {
00119     case pcl::ComparisonOps::GT :
00120       return (compare_result > 0);
00121     case pcl::ComparisonOps::GE :
00122       return (compare_result >= 0);
00123     case pcl::ComparisonOps::LT :
00124       return (compare_result < 0);
00125     case pcl::ComparisonOps::LE :
00126       return (compare_result <= 0);
00127     case pcl::ComparisonOps::EQ :
00128       return (compare_result == 0);
00129     default:
00130       PCL_WARN ("[pcl::FieldComparison::evaluate] unrecognized op_!\n");
00131       return (false);
00132   }
00133 }
00134 
00138 template <typename PointT>
00139 pcl::PackedRGBComparison<PointT>::PackedRGBComparison (
00140     std::string component_name, ComparisonOps::CompareOp op, double compare_val)
00141 {
00142   // get all the fields
00143   std::vector<sensor_msgs::PointField> point_fields;
00144   // Use a dummy cloud to get the field types in a clever way
00145   PointCloud<PointT> dummyCloud;
00146   pcl::getFields (dummyCloud, point_fields);
00147 
00148   // Locate the "rgb" field
00149   size_t d;
00150   for (d = 0; d < point_fields.size (); ++d)
00151   {
00152     if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba")
00153       break;
00154   }
00155   if (d == point_fields.size ())
00156   {
00157     PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n");
00158     capable_ = false;
00159     return;
00160   }
00161 
00162   // Verify the datatype
00163   uint8_t datatype = point_fields[d].datatype;
00164   if (datatype != sensor_msgs::PointField::FLOAT32 &&
00165       datatype != sensor_msgs::PointField::UINT32 &&
00166       datatype != sensor_msgs::PointField::INT32)
00167   {
00168     PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n");
00169     capable_ = false;
00170     return;
00171   }
00172 
00173   // Verify the component name
00174   if (component_name == "r")
00175   {
00176     component_offset_ = point_fields[d].offset + 2;
00177   }
00178   else if (component_name == "g")
00179   {
00180     component_offset_ = point_fields[d].offset + 1;
00181   }
00182   else if (component_name == "b")
00183   {
00184     component_offset_ = point_fields[d].offset;
00185   }
00186   else
00187   {
00188     PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
00189     capable_ = false;
00190     return;
00191   }
00192   component_name_ = component_name;
00193 
00194   // save the rest of the context
00195   capable_ = true;
00196   op_ = op;
00197   compare_val_ = compare_val;
00198 }
00199 
00200 
00202 template <typename PointT> bool
00203 pcl::PackedRGBComparison<PointT>::evaluate (const PointT &point) const
00204 {
00205   // extract the component value
00206   uint8_t* pt_data = (uint8_t*)&point;
00207   uint8_t my_val = *(pt_data + component_offset_);
00208 
00209   // now do the comparison
00210   switch (this->op_) 
00211   {
00212     case pcl::ComparisonOps::GT :
00213       return (my_val > this->compare_val_);
00214     case pcl::ComparisonOps::GE :
00215       return (my_val >= this->compare_val_);
00216     case pcl::ComparisonOps::LT :
00217       return (my_val < this->compare_val_);
00218     case pcl::ComparisonOps::LE :
00219       return (my_val <= this->compare_val_);
00220     case pcl::ComparisonOps::EQ :
00221       return (my_val == this->compare_val_);
00222     default:
00223       PCL_WARN ("[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n");
00224       return (false);
00225   }
00226 }
00227 
00231 template <typename PointT>
00232 pcl::PackedHSIComparison<PointT>::PackedHSIComparison (
00233     std::string component_name, ComparisonOps::CompareOp op, double compare_val)
00234 {
00235   // Get all the fields
00236   std::vector<sensor_msgs::PointField> point_fields; 
00237   // Use a dummy cloud to get the field types in a clever way
00238   PointCloud<PointT> dummyCloud;
00239   pcl::getFields (dummyCloud, point_fields);
00240 
00241   // Locate the "rgb" field
00242   size_t d;
00243   for (d = 0; d < point_fields.size (); ++d)
00244     if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba") 
00245       break;
00246   if (d == point_fields.size ())
00247   {
00248     PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field not found!\n");
00249     capable_ = false;
00250     return;
00251   }
00252 
00253   // Verify the datatype
00254   uint8_t datatype = point_fields[d].datatype;
00255   if (datatype != sensor_msgs::PointField::FLOAT32 && 
00256       datatype != sensor_msgs::PointField::UINT32 && 
00257       datatype != sensor_msgs::PointField::INT32) 
00258   {
00259     PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] has unusable type!\n");
00260     capable_ = false;
00261     return;
00262   }
00263 
00264   // verify the offset
00265   uint32_t offset = point_fields[d].offset;
00266   if (offset % 4 != 0)
00267   {
00268     PCL_WARN ("[pcl::PackedHSIComparison::PackedHSIComparison] rgb field is not 32 bit aligned!\n");
00269     capable_ = false;
00270     return;
00271   }
00272   rgb_offset_ = point_fields[d].offset;
00273 
00274   // verify the component name
00275   if (component_name == "h" ) 
00276   {
00277     component_id_ = H;
00278   } 
00279   else if (component_name == "s") 
00280   {
00281     component_id_ = S;
00282   } 
00283   else if (component_name == "i") 
00284   { 
00285     component_id_ = I;
00286   } 
00287   else 
00288   {
00289     PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n");
00290     capable_ = false;
00291     return;
00292   }
00293   component_name_ = component_name;
00294 
00295   // Save the context
00296   capable_ = true;
00297   op_ = op;
00298   compare_val_ = compare_val;
00299 }
00300 
00302 template <typename PointT> bool
00303 pcl::PackedHSIComparison<PointT>::evaluate (const PointT &point) const
00304 {
00305   // Since this is a const function, we can't make these data members because we change them here
00306   static uint32_t rgb_val_ = 0;
00307   static uint8_t r_ = 0;
00308   static uint8_t g_ = 0;
00309   static uint8_t b_ = 0;
00310   static int8_t h_ = 0;
00311   static uint8_t s_ = 0;
00312   static uint8_t i_ = 0;
00313 
00314   // We know that rgb data is 32 bit aligned (verified in the ctor) so...
00315   uint8_t* pt_data = (uint8_t*)&point;
00316   uint32_t* rgb_data = (uint32_t*)(pt_data + rgb_offset_);
00317   uint32_t new_rgb_val = *rgb_data;
00318 
00319   if (rgb_val_ != new_rgb_val) 
00320   { // avoid having to redo this calc, if possible
00321     rgb_val_ = new_rgb_val;
00322     // extract r,g,b
00323     r_ = (uint8_t)(rgb_val_ >> 16); 
00324     g_ = (uint8_t)(rgb_val_ >> 8);
00325     b_ = (uint8_t)(rgb_val_);
00326 
00327     // definitions taken from http://en.wikipedia.org/wiki/HSL_and_HSI
00328     float hx = (2*r_ - g_ - b_)/4.0;  // hue x component -127 to 127
00329     float hy = (g_ - b_) * 111.0 / 255.0; // hue y component -111 to 111
00330     h_ = (int8_t) (atan2(hy, hx) * 128.0 / M_PI);
00331 
00332     int32_t i = (r_+g_+b_)/3; // 0 to 255
00333     i_ = i;
00334 
00335     int32_t m;  // min(r,g,b)
00336     m = (r_ < g_) ? r_ : g_;
00337     m = (m < b_) ? m : b_;
00338 
00339     s_ = (i == 0) ? 0 : 255 - (m*255)/i; // saturation 0 to 255
00340   }
00341 
00342   float my_val = 0;
00343 
00344   switch (component_id_) 
00345   {
00346     case H:
00347       my_val = (float)h_;
00348       break;
00349     case S:
00350       my_val = (float)s_;
00351       break;
00352     case I:
00353       my_val = (float)i_;
00354       break;
00355     default:
00356       assert (false);
00357   }
00358 
00359   // now do the comparison
00360   switch (this->op_) 
00361   {
00362     case pcl::ComparisonOps::GT :
00363       return (my_val > this->compare_val_);
00364     case pcl::ComparisonOps::GE :
00365       return (my_val >= this->compare_val_);
00366     case pcl::ComparisonOps::LT :
00367       return (my_val < this->compare_val_);
00368     case pcl::ComparisonOps::LE :
00369       return (my_val <= this->compare_val_);
00370     case pcl::ComparisonOps::EQ :
00371       return (my_val == this->compare_val_);
00372     default:
00373       PCL_WARN ("[pcl::PackedHSIComparison::evaluate] unrecognized op_!\n");
00374       return (false);
00375   }
00376 }
00377 
00378 
00382 template <typename PointT> int
00383 pcl::PointDataAtOffset<PointT>::compare (const PointT& p, const double& val) 
00384 {
00385   // if p(data) > val return 1
00386   // if p(data) == val return 0
00387   // if p(data) < val return -1 
00388   
00389   uint8_t* pt_data = (uint8_t*)&p;
00390 
00391   switch (datatype_) 
00392   {
00393     case sensor_msgs::PointField::INT8 : 
00394     {
00395       int8_t pt_val;
00396       memcpy (&pt_val, pt_data + this->offset_, sizeof(int8_t));
00397       return ( pt_val > (int8_t)val ) - ( pt_val < (int8_t)val );
00398     }
00399     case sensor_msgs::PointField::UINT8 : 
00400     {
00401       uint8_t pt_val;
00402       memcpy (&pt_val, pt_data + this->offset_, sizeof(uint8_t));
00403       return ( pt_val > (uint8_t)val ) - ( pt_val < (uint8_t)val );
00404     }
00405     case sensor_msgs::PointField::INT16 : 
00406     {
00407       int16_t pt_val;
00408       memcpy (&pt_val, pt_data + this->offset_, sizeof(int16_t));
00409       return ( pt_val > (int16_t)val ) - ( pt_val < (int16_t)val );
00410     }
00411     case sensor_msgs::PointField::UINT16 : 
00412     {
00413       uint16_t pt_val;
00414       memcpy (&pt_val, pt_data + this->offset_, sizeof(uint16_t));
00415       return ( pt_val > (uint16_t)val ) - ( pt_val < (uint16_t)val );
00416     }
00417     case sensor_msgs::PointField::INT32 : 
00418     {
00419       int32_t pt_val;
00420       memcpy (&pt_val, pt_data + this->offset_, sizeof(int32_t));
00421       return ( pt_val > (int32_t)val ) - ( pt_val < (int32_t)val );
00422     }
00423     case sensor_msgs::PointField::UINT32 : 
00424     {
00425       uint32_t pt_val;
00426       memcpy (&pt_val, pt_data + this->offset_, sizeof(uint32_t));
00427       return ( pt_val > (uint32_t)val ) - ( pt_val < (uint32_t)val );
00428     }
00429     case sensor_msgs::PointField::FLOAT32 : 
00430     {
00431       float pt_val;
00432       memcpy (&pt_val, pt_data + this->offset_, sizeof(float));
00433       return ( pt_val > (float)val ) - ( pt_val < (float)val );
00434     }
00435     case sensor_msgs::PointField::FLOAT64 : 
00436     {
00437       double pt_val;
00438       memcpy (&pt_val, pt_data + this->offset_, sizeof(double));
00439       return ( pt_val > val ) - ( pt_val < val );
00440     }
00441     default : 
00442       PCL_WARN ("[pcl::pcl::PointDataAtOffset::compare] unknown data_type!\n");
00443       return (0);
00444   }
00445 }
00446 
00450 template <typename PointT> void 
00451 pcl::ConditionBase<PointT>::addComparison (ComparisonBaseConstPtr comparison)
00452 {
00453   if (!comparison->isCapable ())
00454     capable_ = false;
00455   comparisons_.push_back (comparison);
00456 }
00457 
00459 template <typename PointT> void 
00460 pcl::ConditionBase<PointT>::addCondition (Ptr condition)
00461 {
00462   if (!condition->isCapable ())
00463     capable_ = false;
00464   conditions_.push_back (condition);
00465 }
00466 
00470 template <typename PointT> bool
00471 pcl::ConditionAnd<PointT>::evaluate (const PointT &point) const
00472 {
00473   for (size_t i = 0; i < comparisons_.size (); ++i)
00474     if (!comparisons_[i]->evaluate (point))
00475       return (false);
00476 
00477   for (size_t i = 0; i < conditions_.size (); ++i)
00478     if (!conditions_[i]->evaluate (point))
00479       return (false);
00480 
00481   return (true);
00482 }
00483 
00487 template <typename PointT> bool 
00488 pcl::ConditionOr<PointT>::evaluate (const PointT &point) const
00489 {
00490   if (comparisons_.empty () && conditions_.empty ()) 
00491     return (true);
00492   for (size_t i = 0; i < comparisons_.size (); ++i)
00493     if (comparisons_[i]->evaluate(point))
00494       return (true);
00495 
00496   for (size_t i = 0; i < conditions_.size (); ++i)
00497     if (conditions_[i]->evaluate (point))
00498       return (true);
00499 
00500   return (false);
00501 }
00502 
00506 template <typename PointT> void 
00507 pcl::ConditionalRemoval<PointT>::setCondition (ConditionBasePtr condition)
00508 {
00509   condition_ = condition;
00510   capable_ = condition_->isCapable ();
00511 }
00512 
00514 template <typename PointT> void
00515 pcl::ConditionalRemoval<PointT>::applyFilter (PointCloud &output)
00516 {
00517   if (capable_ == false)
00518   {
00519     PCL_WARN ("[pcl::%s::applyFilter] not capable!\n", getClassName ().c_str ());
00520     return;
00521   }
00522   // Has the input dataset been set already?
00523   if (!input_)
00524   {
00525     PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
00526     return;
00527   }
00528 
00529   if (condition_.get () == NULL) 
00530   {
00531     PCL_WARN ("[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().c_str ());
00532     return;
00533   }
00534 
00535   // Copy the header (and thus the frame_id) + allocate enough space for points
00536   output.header       = input_->header;
00537   if (!keep_organized_)
00538   {
00539     output.height    = 1;   // filtering breaks the organized structure
00540     output.is_dense  = true;
00541   } 
00542   else 
00543   {
00544     output.height   = this->input_->height;
00545     output.width    = this->input_->width;
00546     output.is_dense = this->input_->is_dense;
00547   }
00548   output.points.resize (input_->points.size ());
00549   removed_indices_->resize (input_->points.size ());
00550 
00551   int nr_p = 0;
00552   int nr_removed_p = 0;
00553 
00554   if (!keep_organized_)
00555   {
00556     for (size_t cp = 0; cp < input_->points.size (); ++cp)
00557     {
00558       // Check if the point is invalid
00559       if (!pcl_isfinite (input_->points[cp].x) || 
00560           !pcl_isfinite (input_->points[cp].y) || 
00561           !pcl_isfinite (input_->points[cp].z))
00562       {
00563         if (extract_removed_indices_)
00564         {
00565           (*removed_indices_)[nr_removed_p] = cp;
00566           nr_removed_p++;
00567         }
00568         continue;
00569       } 
00570 
00571       if (condition_->evaluate (input_->points[cp]))
00572       {
00573         pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[nr_p]));
00574         nr_p++;
00575       }
00576       else
00577       {
00578         if (extract_removed_indices_)
00579         {
00580           (*removed_indices_)[nr_removed_p] = cp;
00581           nr_removed_p++;
00582         }
00583       }
00584     }
00585 
00586     output.width = nr_p;
00587     output.points.resize (nr_p);
00588   } 
00589   else 
00590   {
00591     for (size_t cp = 0; cp < input_->points.size (); ++cp)
00592     {
00593       // copy all the fields
00594       pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[cp]));
00595       if (!condition_->evaluate (input_->points[cp]))
00596       {
00597         output.points[cp].getVector4fMap ().setConstant (user_filter_value_);
00598 
00599         if (extract_removed_indices_)
00600         {
00601           (*removed_indices_)[nr_removed_p]=cp;
00602           nr_removed_p++;
00603         }
00604       }
00605     }
00606   }
00607   removed_indices_->resize (nr_removed_p);
00608 }
00609 
00610 #define PCL_INSTANTIATE_PointDataAtOffset(T) template class PCL_EXPORTS pcl::PointDataAtOffset<T>;
00611 #define PCL_INSTANTIATE_ComparisonBase(T) template class PCL_EXPORTS pcl::ComparisonBase<T>;
00612 #define PCL_INSTANTIATE_FieldComparison(T) template class PCL_EXPORTS pcl::FieldComparison<T>;
00613 #define PCL_INSTANTIATE_PackedRGBComparison(T) template class PCL_EXPORTS pcl::PackedRGBComparison<T>;
00614 #define PCL_INSTANTIATE_PackedHSIComparison(T) template class PCL_EXPORTS pcl::PackedHSIComparison<T>;
00615 #define PCL_INSTANTIATE_ConditionBase(T) template class PCL_EXPORTS pcl::ConditionBase<T>;
00616 #define PCL_INSTANTIATE_ConditionAnd(T) template class PCL_EXPORTS pcl::ConditionAnd<T>;
00617 #define PCL_INSTANTIATE_ConditionOr(T) template class PCL_EXPORTS pcl::ConditionOr<T>;
00618 #define PCL_INSTANTIATE_ConditionalRemoval(T) template class PCL_EXPORTS pcl::ConditionalRemoval<T>;
00619 
00620 #endif 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines