Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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$ 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 <pcl/filters/conditional_removal.h> 00043 00044 ////////////////////////////////////////////////////////////////////////// 00045 ////////////////////////////////////////////////////////////////////////// 00046 ////////////////////////////////////////////////////////////////////////// 00047 template <typename PointT> 00048 pcl::FieldComparison<PointT>::FieldComparison ( 00049 std::string field_name, ComparisonOps::CompareOp op, double compare_val) 00050 : ComparisonBase<PointT> () 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<pcl::PCLPointField> 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 00091 ////////////////////////////////////////////////////////////////////////// 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 00102 ////////////////////////////////////////////////////////////////////////// 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 00135 ////////////////////////////////////////////////////////////////////////// 00136 ////////////////////////////////////////////////////////////////////////// 00137 ////////////////////////////////////////////////////////////////////////// 00138 template <typename PointT> 00139 pcl::PackedRGBComparison<PointT>::PackedRGBComparison ( 00140 std::string component_name, ComparisonOps::CompareOp op, double compare_val) : 00141 component_name_ (component_name), component_offset_ (), compare_val_ (compare_val) 00142 { 00143 // get all the fields 00144 std::vector<pcl::PCLPointField> point_fields; 00145 // Use a dummy cloud to get the field types in a clever way 00146 PointCloud<PointT> dummyCloud; 00147 pcl::getFields (dummyCloud, point_fields); 00148 00149 // Locate the "rgb" field 00150 size_t d; 00151 for (d = 0; d < point_fields.size (); ++d) 00152 { 00153 if (point_fields[d].name == "rgb" || point_fields[d].name == "rgba") 00154 break; 00155 } 00156 if (d == point_fields.size ()) 00157 { 00158 PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] rgb field not found!\n"); 00159 capable_ = false; 00160 return; 00161 } 00162 00163 // Verify the datatype 00164 uint8_t datatype = point_fields[d].datatype; 00165 if (datatype != pcl::PCLPointField::FLOAT32 && 00166 datatype != pcl::PCLPointField::UINT32 && 00167 datatype != pcl::PCLPointField::INT32) 00168 { 00169 PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] has unusable type!\n"); 00170 capable_ = false; 00171 return; 00172 } 00173 00174 // Verify the component name 00175 if (component_name == "r") 00176 { 00177 component_offset_ = point_fields[d].offset + 2; 00178 } 00179 else if (component_name == "g") 00180 { 00181 component_offset_ = point_fields[d].offset + 1; 00182 } 00183 else if (component_name == "b") 00184 { 00185 component_offset_ = point_fields[d].offset; 00186 } 00187 else 00188 { 00189 PCL_WARN ("[pcl::PackedRGBComparison::PackedRGBComparison] unrecognized component name!\n"); 00190 capable_ = false; 00191 return; 00192 } 00193 00194 // save the rest of the context 00195 capable_ = true; 00196 op_ = op; 00197 } 00198 00199 00200 ////////////////////////////////////////////////////////////////////////// 00201 template <typename PointT> bool 00202 pcl::PackedRGBComparison<PointT>::evaluate (const PointT &point) const 00203 { 00204 // extract the component value 00205 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&point); 00206 uint8_t my_val = *(pt_data + component_offset_); 00207 00208 // now do the comparison 00209 switch (this->op_) 00210 { 00211 case pcl::ComparisonOps::GT : 00212 return (my_val > this->compare_val_); 00213 case pcl::ComparisonOps::GE : 00214 return (my_val >= this->compare_val_); 00215 case pcl::ComparisonOps::LT : 00216 return (my_val < this->compare_val_); 00217 case pcl::ComparisonOps::LE : 00218 return (my_val <= this->compare_val_); 00219 case pcl::ComparisonOps::EQ : 00220 return (my_val == this->compare_val_); 00221 default: 00222 PCL_WARN ("[pcl::PackedRGBComparison::evaluate] unrecognized op_!\n"); 00223 return (false); 00224 } 00225 } 00226 00227 ////////////////////////////////////////////////////////////////////////// 00228 ////////////////////////////////////////////////////////////////////////// 00229 ////////////////////////////////////////////////////////////////////////// 00230 template <typename PointT> 00231 pcl::PackedHSIComparison<PointT>::PackedHSIComparison ( 00232 std::string component_name, ComparisonOps::CompareOp op, double compare_val) : 00233 component_name_ (component_name), component_id_ (), compare_val_ (compare_val), rgb_offset_ () 00234 { 00235 // Get all the fields 00236 std::vector<pcl::PCLPointField> 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 != pcl::PCLPointField::FLOAT32 && 00256 datatype != pcl::PCLPointField::UINT32 && 00257 datatype != pcl::PCLPointField::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 00294 // Save the context 00295 capable_ = true; 00296 op_ = op; 00297 } 00298 00299 ////////////////////////////////////////////////////////////////////////// 00300 template <typename PointT> bool 00301 pcl::PackedHSIComparison<PointT>::evaluate (const PointT &point) const 00302 { 00303 // Since this is a const function, we can't make these data members because we change them here 00304 static uint32_t rgb_val_ = 0; 00305 static uint8_t r_ = 0; 00306 static uint8_t g_ = 0; 00307 static uint8_t b_ = 0; 00308 static int8_t h_ = 0; 00309 static uint8_t s_ = 0; 00310 static uint8_t i_ = 0; 00311 00312 // We know that rgb data is 32 bit aligned (verified in the ctor) so... 00313 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&point); 00314 const uint32_t* rgb_data = reinterpret_cast<const uint32_t*> (pt_data + rgb_offset_); 00315 uint32_t new_rgb_val = *rgb_data; 00316 00317 if (rgb_val_ != new_rgb_val) 00318 { // avoid having to redo this calc, if possible 00319 rgb_val_ = new_rgb_val; 00320 // extract r,g,b 00321 r_ = static_cast <uint8_t> (rgb_val_ >> 16); 00322 g_ = static_cast <uint8_t> (rgb_val_ >> 8); 00323 b_ = static_cast <uint8_t> (rgb_val_); 00324 00325 // definitions taken from http://en.wikipedia.org/wiki/HSL_and_HSI 00326 float hx = (2.0f * r_ - g_ - b_) / 4.0f; // hue x component -127 to 127 00327 float hy = static_cast<float> (g_ - b_) * 111.0f / 255.0f; // hue y component -111 to 111 00328 h_ = static_cast<int8_t> (atan2(hy, hx) * 128.0f / M_PI); 00329 00330 int32_t i = (r_+g_+b_)/3; // 0 to 255 00331 i_ = static_cast<uint8_t> (i); 00332 00333 int32_t m; // min(r,g,b) 00334 m = (r_ < g_) ? r_ : g_; 00335 m = (m < b_) ? m : b_; 00336 00337 s_ = static_cast<uint8_t> ((i == 0) ? 0 : 255 - (m * 255) / i); // saturation 0 to 255 00338 } 00339 00340 float my_val = 0; 00341 00342 switch (component_id_) 00343 { 00344 case H: 00345 my_val = static_cast <float> (h_); 00346 break; 00347 case S: 00348 my_val = static_cast <float> (s_); 00349 break; 00350 case I: 00351 my_val = static_cast <float> (i_); 00352 break; 00353 default: 00354 assert (false); 00355 } 00356 00357 // now do the comparison 00358 switch (this->op_) 00359 { 00360 case pcl::ComparisonOps::GT : 00361 return (my_val > this->compare_val_); 00362 case pcl::ComparisonOps::GE : 00363 return (my_val >= this->compare_val_); 00364 case pcl::ComparisonOps::LT : 00365 return (my_val < this->compare_val_); 00366 case pcl::ComparisonOps::LE : 00367 return (my_val <= this->compare_val_); 00368 case pcl::ComparisonOps::EQ : 00369 return (my_val == this->compare_val_); 00370 default: 00371 PCL_WARN ("[pcl::PackedHSIComparison::evaluate] unrecognized op_!\n"); 00372 return (false); 00373 } 00374 } 00375 00376 00377 ////////////////////////////////////////////////////////////////////////// 00378 ////////////////////////////////////////////////////////////////////////// 00379 ////////////////////////////////////////////////////////////////////////// 00380 template<typename PointT> 00381 pcl::TfQuadraticXYZComparison<PointT>::TfQuadraticXYZComparison () : 00382 comp_matr_ (), comp_vect_ (), comp_scalar_ (0.0) 00383 { 00384 // get all the fields 00385 std::vector<pcl::PCLPointField> point_fields; 00386 // Use a dummy cloud to get the field types in a clever way 00387 PointCloud<PointT> dummyCloud; 00388 pcl::getFields (dummyCloud, point_fields); 00389 00390 // Locate the "x" field 00391 size_t dX; 00392 for (dX = 0; dX < point_fields.size (); ++dX) 00393 { 00394 if (point_fields[dX].name == "x") 00395 break; 00396 } 00397 if (dX == point_fields.size ()) 00398 { 00399 PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n"); 00400 capable_ = false; 00401 return; 00402 } 00403 00404 // Locate the "y" field 00405 size_t dY; 00406 for (dY = 0; dY < point_fields.size (); ++dY) 00407 { 00408 if (point_fields[dY].name == "y") 00409 break; 00410 } 00411 if (dY == point_fields.size ()) 00412 { 00413 PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n"); 00414 capable_ = false; 00415 return; 00416 } 00417 00418 // Locate the "z" field 00419 size_t dZ; 00420 for (dZ = 0; dZ < point_fields.size (); ++dZ) 00421 { 00422 if (point_fields[dZ].name == "z") 00423 break; 00424 } 00425 if (dZ == point_fields.size ()) 00426 { 00427 PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n"); 00428 capable_ = false; 00429 return; 00430 } 00431 00432 comp_matr_ << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; 00433 comp_vect_ << 0.0, 0.0, 0.0, 1.0; 00434 tf_comp_matr_ = comp_matr_; 00435 tf_comp_vect_ = comp_vect_; 00436 op_ = pcl::ComparisonOps::EQ; 00437 capable_ = true; 00438 } 00439 00440 ////////////////////////////////////////////////////////////////////////// 00441 template<typename PointT> 00442 pcl::TfQuadraticXYZComparison<PointT>::TfQuadraticXYZComparison (const pcl::ComparisonOps::CompareOp op, 00443 const Eigen::Matrix3f &comparison_matrix, 00444 const Eigen::Vector3f &comparison_vector, 00445 const float &comparison_scalar, 00446 const Eigen::Affine3f &comparison_transform) : 00447 comp_matr_ (), comp_vect_ (), comp_scalar_ (comparison_scalar) 00448 { 00449 // get all the fields 00450 std::vector<pcl::PCLPointField> point_fields; 00451 // Use a dummy cloud to get the field types in a clever way 00452 PointCloud<PointT> dummyCloud; 00453 pcl::getFields (dummyCloud, point_fields); 00454 00455 // Locate the "x" field 00456 size_t dX; 00457 for (dX = 0; dX < point_fields.size (); ++dX) 00458 { 00459 if (point_fields[dX].name == "x") 00460 break; 00461 } 00462 if (dX == point_fields.size ()) 00463 { 00464 PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] x field not found!\n"); 00465 capable_ = false; 00466 return; 00467 } 00468 00469 // Locate the "y" field 00470 size_t dY; 00471 for (dY = 0; dY < point_fields.size (); ++dY) 00472 { 00473 if (point_fields[dY].name == "y") 00474 break; 00475 } 00476 if (dY == point_fields.size ()) 00477 { 00478 PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] y field not found!\n"); 00479 capable_ = false; 00480 return; 00481 } 00482 00483 // Locate the "z" field 00484 size_t dZ; 00485 for (dZ = 0; dZ < point_fields.size (); ++dZ) 00486 { 00487 if (point_fields[dZ].name == "z") 00488 break; 00489 } 00490 if (dZ == point_fields.size ()) 00491 { 00492 PCL_WARN ("[pcl::TfQuadraticXYZComparison::TfQuadraticXYZComparison] z field not found!\n"); 00493 capable_ = false; 00494 return; 00495 } 00496 00497 capable_ = true; 00498 op_ = op; 00499 setComparisonMatrix (comparison_matrix); 00500 setComparisonVector (comparison_vector); 00501 if (!comparison_transform.matrix ().isIdentity ()) 00502 transformComparison (comparison_transform); 00503 } 00504 00505 ////////////////////////////////////////////////////////////////////////// 00506 template<typename PointT> 00507 bool 00508 pcl::TfQuadraticXYZComparison<PointT>::evaluate (const PointT &point) const 00509 { 00510 Eigen::Vector4f pointAffine; 00511 pointAffine << point.x, point.y, point.z, 1; 00512 00513 float myVal = static_cast<float>(2.0f * tf_comp_vect_.transpose () * pointAffine) + static_cast<float>(pointAffine.transpose () * tf_comp_matr_ * pointAffine) + comp_scalar_ - 3.0f; 00514 00515 // now do the comparison 00516 switch (this->op_) 00517 { 00518 case pcl::ComparisonOps::GT: 00519 return (myVal > 0); 00520 case pcl::ComparisonOps::GE: 00521 return (myVal >= 0); 00522 case pcl::ComparisonOps::LT: 00523 return (myVal < 0); 00524 case pcl::ComparisonOps::LE: 00525 return (myVal <= 0); 00526 case pcl::ComparisonOps::EQ: 00527 return (myVal == 0); 00528 default: 00529 PCL_WARN ("[pcl::transformableQuadricXYZComparison::evaluate] unrecognized op_!\n"); 00530 return (false); 00531 } 00532 } 00533 00534 ////////////////////////////////////////////////////////////////////////// 00535 ////////////////////////////////////////////////////////////////////////// 00536 ////////////////////////////////////////////////////////////////////////// 00537 template <typename PointT> int 00538 pcl::PointDataAtOffset<PointT>::compare (const PointT& p, const double& val) 00539 { 00540 // if p(data) > val return 1 00541 // if p(data) == val return 0 00542 // if p(data) < val return -1 00543 00544 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&p); 00545 00546 switch (datatype_) 00547 { 00548 case pcl::PCLPointField::INT8 : 00549 { 00550 int8_t pt_val; 00551 memcpy (&pt_val, pt_data + this->offset_, sizeof (int8_t)); 00552 return (pt_val > static_cast<int8_t>(val)) - (pt_val < static_cast<int8_t> (val)); 00553 } 00554 case pcl::PCLPointField::UINT8 : 00555 { 00556 uint8_t pt_val; 00557 memcpy (&pt_val, pt_data + this->offset_, sizeof (uint8_t)); 00558 return (pt_val > static_cast<uint8_t>(val)) - (pt_val < static_cast<uint8_t> (val)); 00559 } 00560 case pcl::PCLPointField::INT16 : 00561 { 00562 int16_t pt_val; 00563 memcpy (&pt_val, pt_data + this->offset_, sizeof (int16_t)); 00564 return (pt_val > static_cast<int16_t>(val)) - (pt_val < static_cast<int16_t> (val)); 00565 } 00566 case pcl::PCLPointField::UINT16 : 00567 { 00568 uint16_t pt_val; 00569 memcpy (&pt_val, pt_data + this->offset_, sizeof (uint16_t)); 00570 return (pt_val > static_cast<uint16_t> (val)) - (pt_val < static_cast<uint16_t> (val)); 00571 } 00572 case pcl::PCLPointField::INT32 : 00573 { 00574 int32_t pt_val; 00575 memcpy (&pt_val, pt_data + this->offset_, sizeof (int32_t)); 00576 return (pt_val > static_cast<int32_t> (val)) - (pt_val < static_cast<int32_t> (val)); 00577 } 00578 case pcl::PCLPointField::UINT32 : 00579 { 00580 uint32_t pt_val; 00581 memcpy (&pt_val, pt_data + this->offset_, sizeof (uint32_t)); 00582 return (pt_val > static_cast<uint32_t> (val)) - (pt_val < static_cast<uint32_t> (val)); 00583 } 00584 case pcl::PCLPointField::FLOAT32 : 00585 { 00586 float pt_val; 00587 memcpy (&pt_val, pt_data + this->offset_, sizeof (float)); 00588 return (pt_val > static_cast<float> (val)) - (pt_val < static_cast<float> (val)); 00589 } 00590 case pcl::PCLPointField::FLOAT64 : 00591 { 00592 double pt_val; 00593 memcpy (&pt_val, pt_data + this->offset_, sizeof (double)); 00594 return (pt_val > val) - (pt_val < val); 00595 } 00596 default : 00597 PCL_WARN ("[pcl::pcl::PointDataAtOffset::compare] unknown data_type!\n"); 00598 return (0); 00599 } 00600 } 00601 00602 ////////////////////////////////////////////////////////////////////////// 00603 ////////////////////////////////////////////////////////////////////////// 00604 ////////////////////////////////////////////////////////////////////////// 00605 template <typename PointT> void 00606 pcl::ConditionBase<PointT>::addComparison (ComparisonBaseConstPtr comparison) 00607 { 00608 if (!comparison->isCapable ()) 00609 capable_ = false; 00610 comparisons_.push_back (comparison); 00611 } 00612 00613 ////////////////////////////////////////////////////////////////////////// 00614 template <typename PointT> void 00615 pcl::ConditionBase<PointT>::addCondition (Ptr condition) 00616 { 00617 if (!condition->isCapable ()) 00618 capable_ = false; 00619 conditions_.push_back (condition); 00620 } 00621 00622 ////////////////////////////////////////////////////////////////////////// 00623 ////////////////////////////////////////////////////////////////////////// 00624 ////////////////////////////////////////////////////////////////////////// 00625 template <typename PointT> bool 00626 pcl::ConditionAnd<PointT>::evaluate (const PointT &point) const 00627 { 00628 for (size_t i = 0; i < comparisons_.size (); ++i) 00629 if (!comparisons_[i]->evaluate (point)) 00630 return (false); 00631 00632 for (size_t i = 0; i < conditions_.size (); ++i) 00633 if (!conditions_[i]->evaluate (point)) 00634 return (false); 00635 00636 return (true); 00637 } 00638 00639 ////////////////////////////////////////////////////////////////////////// 00640 ////////////////////////////////////////////////////////////////////////// 00641 ////////////////////////////////////////////////////////////////////////// 00642 template <typename PointT> bool 00643 pcl::ConditionOr<PointT>::evaluate (const PointT &point) const 00644 { 00645 if (comparisons_.empty () && conditions_.empty ()) 00646 return (true); 00647 for (size_t i = 0; i < comparisons_.size (); ++i) 00648 if (comparisons_[i]->evaluate(point)) 00649 return (true); 00650 00651 for (size_t i = 0; i < conditions_.size (); ++i) 00652 if (conditions_[i]->evaluate (point)) 00653 return (true); 00654 00655 return (false); 00656 } 00657 00658 ////////////////////////////////////////////////////////////////////////// 00659 ////////////////////////////////////////////////////////////////////////// 00660 ////////////////////////////////////////////////////////////////////////// 00661 template <typename PointT> void 00662 pcl::ConditionalRemoval<PointT>::setCondition (ConditionBasePtr condition) 00663 { 00664 condition_ = condition; 00665 capable_ = condition_->isCapable (); 00666 } 00667 00668 ////////////////////////////////////////////////////////////////////////// 00669 template <typename PointT> void 00670 pcl::ConditionalRemoval<PointT>::applyFilter (PointCloud &output) 00671 { 00672 if (capable_ == false) 00673 { 00674 PCL_WARN ("[pcl::%s::applyFilter] not capable!\n", getClassName ().c_str ()); 00675 return; 00676 } 00677 // Has the input dataset been set already? 00678 if (!input_) 00679 { 00680 PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); 00681 return; 00682 } 00683 00684 if (condition_.get () == NULL) 00685 { 00686 PCL_WARN ("[pcl::%s::applyFilter] No filtering condition given!\n", getClassName ().c_str ()); 00687 return; 00688 } 00689 00690 // Copy the header (and thus the frame_id) + allocate enough space for points 00691 output.header = input_->header; 00692 if (!keep_organized_) 00693 { 00694 output.height = 1; // filtering breaks the organized structure 00695 output.is_dense = true; 00696 } 00697 else 00698 { 00699 output.height = this->input_->height; 00700 output.width = this->input_->width; 00701 output.is_dense = this->input_->is_dense; 00702 } 00703 output.points.resize (input_->points.size ()); 00704 removed_indices_->resize (input_->points.size ()); 00705 00706 int nr_p = 0; 00707 int nr_removed_p = 0; 00708 00709 if (!keep_organized_) 00710 { 00711 for (size_t cp = 0; cp < Filter<PointT>::indices_->size (); ++cp) 00712 { 00713 // Check if the point is invalid 00714 if (!pcl_isfinite (input_->points[(*Filter < PointT > ::indices_)[cp]].x) 00715 || !pcl_isfinite (input_->points[(*Filter < PointT > ::indices_)[cp]].y) 00716 || !pcl_isfinite (input_->points[(*Filter < PointT > ::indices_)[cp]].z)) 00717 { 00718 if (extract_removed_indices_) 00719 { 00720 (*removed_indices_)[nr_removed_p] = (*Filter<PointT>::indices_)[cp]; 00721 nr_removed_p++; 00722 } 00723 continue; 00724 } 00725 00726 if (condition_->evaluate (input_->points[(*Filter < PointT > ::indices_)[cp]])) 00727 { 00728 pcl::for_each_type<FieldList> ( 00729 pcl::NdConcatenateFunctor<PointT, PointT> ( 00730 input_->points[(*Filter < PointT > ::indices_)[cp]], 00731 output.points[nr_p])); 00732 nr_p++; 00733 } 00734 else 00735 { 00736 if (extract_removed_indices_) 00737 { 00738 (*removed_indices_)[nr_removed_p] = (*Filter<PointT>::indices_)[cp]; 00739 nr_removed_p++; 00740 } 00741 } 00742 } 00743 00744 output.width = nr_p; 00745 output.points.resize (nr_p); 00746 } 00747 else 00748 { 00749 std::vector<int> indices = *Filter<PointT>::indices_; 00750 std::sort (indices.begin (), indices.end ()); //TODO: is this necessary or can we assume the indices to be sorted? 00751 bool removed_p = false; 00752 size_t ci = 0; 00753 for (size_t cp = 0; cp < input_->points.size (); ++cp) 00754 { 00755 if (cp == static_cast<size_t> (indices[ci])) 00756 { 00757 if (ci < indices.size () - 1) 00758 { 00759 ci++; 00760 if (cp == static_cast<size_t> (indices[ci])) //check whether the next index will have the same value. TODO: necessary? 00761 continue; 00762 } 00763 00764 // copy all the fields 00765 pcl::for_each_type<FieldList> (pcl::NdConcatenateFunctor<PointT, PointT> (input_->points[cp], 00766 output.points[cp])); 00767 if (!condition_->evaluate (input_->points[cp])) 00768 { 00769 output.points[cp].getVector4fMap ().setConstant (user_filter_value_); 00770 removed_p = true; 00771 00772 if (extract_removed_indices_) 00773 { 00774 (*removed_indices_)[nr_removed_p] = static_cast<int> (cp); 00775 nr_removed_p++; 00776 } 00777 } 00778 } 00779 else 00780 { 00781 output.points[cp].getVector4fMap ().setConstant (user_filter_value_); 00782 removed_p = true; 00783 //as for !keep_organized_: removed points due to setIndices are not considered as removed_indices_ 00784 } 00785 } 00786 00787 if (removed_p && !pcl_isfinite (user_filter_value_)) 00788 output.is_dense = false; 00789 } 00790 removed_indices_->resize (nr_removed_p); 00791 } 00792 00793 #define PCL_INSTANTIATE_PointDataAtOffset(T) template class PCL_EXPORTS pcl::PointDataAtOffset<T>; 00794 #define PCL_INSTANTIATE_ComparisonBase(T) template class PCL_EXPORTS pcl::ComparisonBase<T>; 00795 #define PCL_INSTANTIATE_FieldComparison(T) template class PCL_EXPORTS pcl::FieldComparison<T>; 00796 #define PCL_INSTANTIATE_PackedRGBComparison(T) template class PCL_EXPORTS pcl::PackedRGBComparison<T>; 00797 #define PCL_INSTANTIATE_PackedHSIComparison(T) template class PCL_EXPORTS pcl::PackedHSIComparison<T>; 00798 #define PCL_INSTANTIATE_TfQuadraticXYZComparison(T) template class PCL_EXPORTS pcl::TfQuadraticXYZComparison<T>; 00799 #define PCL_INSTANTIATE_ConditionBase(T) template class PCL_EXPORTS pcl::ConditionBase<T>; 00800 #define PCL_INSTANTIATE_ConditionAnd(T) template class PCL_EXPORTS pcl::ConditionAnd<T>; 00801 #define PCL_INSTANTIATE_ConditionOr(T) template class PCL_EXPORTS pcl::ConditionOr<T>; 00802 #define PCL_INSTANTIATE_ConditionalRemoval(T) template class PCL_EXPORTS pcl::ConditionalRemoval<T>; 00803 00804 #endif