crop_box.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: extract_indices.hpp 1897 2011-07-26 20:35:49Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_FILTERS_IMPL_CROP_BOX_H_
00041 #define PCL_FILTERS_IMPL_CROP_BOX_H_
00042 
00043 #include <pcl/filters/crop_box.h>
00044 #include <pcl/common/io.h>
00045 
00047 template<typename PointT> void
00048 pcl::CropBox<PointT>::applyFilter (PointCloud &output)
00049 {
00050   std::vector<int> indices;
00051   if (keep_organized_)
00052   {
00053     bool temp = extract_removed_indices_;
00054     extract_removed_indices_ = true;
00055     applyFilter (indices);
00056     extract_removed_indices_ = temp;
00057 
00058     output = *input_;
00059     for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
00060       output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
00061     if (!pcl_isfinite (user_filter_value_))
00062       output.is_dense = false;
00063   }
00064   else
00065   {
00066     output.is_dense = true;
00067     applyFilter (indices);
00068     pcl::copyPointCloud (*input_, indices, output);
00069   }
00070 }
00071 
00073 template<typename PointT> void
00074 pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
00075 {
00076   indices.resize (input_->points.size ());
00077   removed_indices_->resize (input_->points.size ());
00078   int indices_count = 0;
00079   int removed_indices_count = 0;
00080 
00081   Eigen::Affine3f transform = Eigen::Affine3f::Identity ();
00082   Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity ();
00083 
00084   if (rotation_ != Eigen::Vector3f::Zero ())
00085   {
00086     pcl::getTransformation (0, 0, 0,
00087                             rotation_ (0), rotation_ (1), rotation_ (2),
00088                             transform);
00089     inverse_transform = transform.inverse ();
00090   }
00091 
00092   for (size_t index = 0; index < indices_->size (); ++index)
00093   {
00094     if (!input_->is_dense)
00095       // Check if the point is invalid
00096       if (!isFinite (input_->points[index]))
00097         continue;
00098 
00099     // Get local point
00100     PointT local_pt = input_->points[(*indices_)[index]];
00101 
00102     // Transform point to world space
00103     if (!(transform_.matrix ().isIdentity ()))
00104       local_pt = pcl::transformPoint<PointT> (local_pt, transform_);
00105 
00106     if (translation_ != Eigen::Vector3f::Zero ())
00107     {
00108       local_pt.x -= translation_ (0);
00109       local_pt.y -= translation_ (1);
00110       local_pt.z -= translation_ (2);
00111     }
00112 
00113     // Transform point to local space of crop box
00114     if (!(inverse_transform.matrix ().isIdentity ()))
00115       local_pt = pcl::transformPoint<PointT> (local_pt, inverse_transform);
00116 
00117     // If outside the cropbox
00118     if ( (local_pt.x < min_pt_[0] || local_pt.y < min_pt_[1] || local_pt.z < min_pt_[2]) ||
00119          (local_pt.x > max_pt_[0] || local_pt.y > max_pt_[1] || local_pt.z > max_pt_[2]))
00120     {
00121       if (negative_)
00122         indices[indices_count++] = (*indices_)[index];
00123       else if (extract_removed_indices_)
00124         (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
00125     }
00126     // If inside the cropbox
00127     else
00128     {
00129       if (negative_ && extract_removed_indices_)
00130         (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
00131       else if (!negative_) 
00132         indices[indices_count++] = (*indices_)[index];
00133     }
00134   }
00135   indices.resize (indices_count);
00136   removed_indices_->resize (removed_indices_count);
00137 }
00138 
00139 #define PCL_INSTANTIATE_CropBox(T) template class PCL_EXPORTS pcl::CropBox<T>;
00140 
00141 #endif    // PCL_FILTERS_IMPL_CROP_BOX_H_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:18