Point Cloud Library (PCL)
1.3.1
|
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