Point Cloud Library (PCL)  1.3.1
passthrough.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: passthrough.hpp 3317 2011-12-02 11:17:47Z mdixon $
00035  *
00036  */
00037 
00038 #ifndef PCL_FILTERS_IMPL_PASSTHROUGH_H_
00039 #define PCL_FILTERS_IMPL_PASSTHROUGH_H_
00040 
00041 #include "pcl/filters/passthrough.h"
00042 #include "pcl/common/io.h"
00043 
00045 template <typename PointT> void
00046 pcl::PassThrough<PointT>::applyFilter (PointCloud &output)
00047 {
00048   // Has the input dataset been set already?
00049   if (!input_)
00050   {
00051     PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
00052     output.width = output.height = 0;
00053     output.points.clear ();
00054     return;
00055   }
00056 
00057   // Check if we're going to keep the organized structure of the cloud or not
00058   if (keep_organized_)
00059   {
00060     if (filter_field_name_.empty ())
00061     {
00062       // Silly - if no filtering is actually done, and we want to keep the data organized, 
00063       // just copy everything. Any optimizations that can be done here???
00064       output = *input_;
00065       return;
00066     }
00067 
00068     output.width  = input_->width;
00069     output.height = input_->height;
00070     // Check what the user value is: if !finite, set is_dense to false, true otherwise
00071     if (!pcl_isfinite (user_filter_value_))
00072       output.is_dense = false;
00073     else
00074       output.is_dense = input_->is_dense;
00075   }
00076   else
00077   {
00078     output.height = 1;                    // filtering breaks the organized structure
00079     // Because we're doing explit checks for isfinite, is_dense = true
00080     output.is_dense = true;
00081   }
00082   output.points.resize (input_->points.size ());
00083   removed_indices_->resize (input_->points.size ());
00084 
00085   int nr_p = 0;
00086   int nr_removed_p = 0;
00087 
00088   // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
00089   if (!filter_field_name_.empty ())
00090   {
00091     // Get the distance field index
00092     std::vector<sensor_msgs::PointField> fields;
00093     int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields);
00094     if (distance_idx == -1)
00095     {
00096       PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
00097       output.width = output.height = 0;
00098       output.points.clear ();
00099       return;
00100     }
00101 
00102     if (keep_organized_)
00103     {
00104       for (size_t cp = 0; cp < input_->points.size (); ++cp)
00105       {
00106         if (pcl_isnan (input_->points[cp].x) ||
00107             pcl_isnan (input_->points[cp].y) ||
00108             pcl_isnan (input_->points[cp].z))
00109         {
00110           output.points[cp].x = output.points[cp].y = output.points[cp].z = user_filter_value_;
00111           continue;
00112         }
00113 
00114         // Copy the point
00115         pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[cp]));
00116 
00117         // Filter it. Get the distance value
00118         uint8_t* pt_data = (uint8_t*)&input_->points[cp];
00119         float distance_value = 0;
00120         memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
00121 
00122         if (filter_limit_negative_)
00123         {
00124           // Use a threshold for cutting out points which inside the interval
00125           if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
00126           {
00127             output.points[cp].x = output.points[cp].y = output.points[cp].z = user_filter_value_;
00128             continue;
00129           }
00130           else 
00131           {
00132             if (extract_removed_indices_)
00133             {
00134               (*removed_indices_)[nr_removed_p]=cp;
00135               nr_removed_p++;
00136             }
00137           }
00138         }
00139         else
00140         {
00141           // Use a threshold for cutting out points which are too close/far away
00142           if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
00143           {
00144             output.points[cp].x = output.points[cp].y = output.points[cp].z = user_filter_value_;
00145             continue;
00146           }
00147           else
00148           {
00149             if (extract_removed_indices_)
00150             {
00151               (*removed_indices_)[nr_removed_p] = cp;
00152               nr_removed_p++;
00153             }
00154           }
00155         }
00156       }
00157       nr_p = input_->points.size ();
00158     }
00159     // Remove filtered points
00160     else
00161     {
00162       // Go over all points
00163       for (size_t cp = 0; cp < input_->points.size (); ++cp)
00164       {
00165         // Check if the point is invalid
00166         if (!pcl_isfinite (input_->points[cp].x) || !pcl_isfinite (input_->points[cp].y) || !pcl_isfinite (input_->points[cp].z))
00167         {
00168           if (extract_removed_indices_)
00169           {
00170             (*removed_indices_)[nr_removed_p] = cp;
00171             nr_removed_p++;
00172           }
00173           continue;
00174         }
00175 
00176         // Get the distance value
00177         uint8_t* pt_data = (uint8_t*)&input_->points[cp];
00178         float distance_value = 0;
00179         memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
00180 
00181         if (filter_limit_negative_)
00182         {
00183           // Use a threshold for cutting out points which inside the interval
00184           if (distance_value < filter_limit_max_ && distance_value > filter_limit_min_)
00185           {
00186             if (extract_removed_indices_)
00187             {
00188               (*removed_indices_)[nr_removed_p] = cp;
00189               nr_removed_p++;
00190             }
00191             continue;
00192           }
00193         }
00194         else
00195         {
00196           // Use a threshold for cutting out points which are too close/far away
00197           if (distance_value > filter_limit_max_ || distance_value < filter_limit_min_)
00198           {
00199             if (extract_removed_indices_)
00200             {
00201               (*removed_indices_)[nr_removed_p] = cp;
00202               nr_removed_p++;
00203             }
00204             continue;
00205           }
00206         }
00207 
00208         pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[nr_p]));
00209         nr_p++;
00210       }
00211       output.width = nr_p;
00212     } // !keep_organized_
00213   }
00214   // No distance filtering, process all data. No need to check for is_organized here as we did it above
00215   else
00216   {
00217     // First pass: go over all points and insert them into the right leaf
00218     for (size_t cp = 0; cp < input_->points.size (); ++cp)
00219     {
00220       // Check if the point is invalid
00221       if (!pcl_isfinite (input_->points[cp].x) || !pcl_isfinite (input_->points[cp].y) || !pcl_isfinite (input_->points[cp].z))
00222       {
00223         if (extract_removed_indices_)
00224         {
00225           (*removed_indices_)[nr_removed_p] = cp;
00226           nr_removed_p++;
00227         }
00228         continue;
00229       }
00230 
00231       pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointT> (input_->points[cp], output.points[nr_p]));
00232       nr_p++;
00233     }
00234     output.width = nr_p;
00235   }
00236 
00237   output.points.resize (output.width * output.height);
00238   removed_indices_->resize(nr_removed_p);
00239 }
00240 
00241 #define PCL_INSTANTIATE_PassThrough(T) template class PCL_EXPORTS pcl::PassThrough<T>;
00242 
00243 #endif    // PCL_FILTERS_IMPL_PASSTHROUGH_H_
00244 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines