Point Cloud Library (PCL)  1.7.2
crop_box.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id: extract_indices.hpp 1897 2011-07-26 20:35:49Z rusu $
37  *
38  */
39 
40 #ifndef PCL_FILTERS_IMPL_CROP_BOX_H_
41 #define PCL_FILTERS_IMPL_CROP_BOX_H_
42 
43 #include <pcl/filters/crop_box.h>
44 #include <pcl/common/io.h>
45 
46 ///////////////////////////////////////////////////////////////////////////////
47 template<typename PointT> void
49 {
50  std::vector<int> indices;
51  if (keep_organized_)
52  {
53  bool temp = extract_removed_indices_;
54  extract_removed_indices_ = true;
55  applyFilter (indices);
56  extract_removed_indices_ = temp;
57 
58  output = *input_;
59  for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
60  output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
61  if (!pcl_isfinite (user_filter_value_))
62  output.is_dense = false;
63  }
64  else
65  {
66  output.is_dense = true;
67  applyFilter (indices);
68  pcl::copyPointCloud (*input_, indices, output);
69  }
70 }
71 
72 ///////////////////////////////////////////////////////////////////////////////
73 template<typename PointT> void
74 pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
75 {
76  indices.resize (input_->points.size ());
77  removed_indices_->resize (input_->points.size ());
78  int indices_count = 0;
79  int removed_indices_count = 0;
80 
81  Eigen::Affine3f transform = Eigen::Affine3f::Identity ();
82  Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity ();
83 
84  if (rotation_ != Eigen::Vector3f::Zero ())
85  {
86  pcl::getTransformation (0, 0, 0,
87  rotation_ (0), rotation_ (1), rotation_ (2),
88  transform);
89  inverse_transform = transform.inverse ();
90  }
91 
92  for (size_t index = 0; index < indices_->size (); ++index)
93  {
94  if (!input_->is_dense)
95  // Check if the point is invalid
96  if (!isFinite (input_->points[index]))
97  continue;
98 
99  // Get local point
100  PointT local_pt = input_->points[(*indices_)[index]];
101 
102  // Transform point to world space
103  if (!(transform_.matrix ().isIdentity ()))
104  local_pt = pcl::transformPoint<PointT> (local_pt, transform_);
105 
106  if (translation_ != Eigen::Vector3f::Zero ())
107  {
108  local_pt.x -= translation_ (0);
109  local_pt.y -= translation_ (1);
110  local_pt.z -= translation_ (2);
111  }
112 
113  // Transform point to local space of crop box
114  if (!(inverse_transform.matrix ().isIdentity ()))
115  local_pt = pcl::transformPoint<PointT> (local_pt, inverse_transform);
116 
117  // If outside the cropbox
118  if ( (local_pt.x < min_pt_[0] || local_pt.y < min_pt_[1] || local_pt.z < min_pt_[2]) ||
119  (local_pt.x > max_pt_[0] || local_pt.y > max_pt_[1] || local_pt.z > max_pt_[2]))
120  {
121  if (negative_)
122  indices[indices_count++] = (*indices_)[index];
123  else if (extract_removed_indices_)
124  (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
125  }
126  // If inside the cropbox
127  else
128  {
129  if (negative_ && extract_removed_indices_)
130  (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
131  else if (!negative_)
132  indices[indices_count++] = (*indices_)[index];
133  }
134  }
135  indices.resize (indices_count);
136  removed_indices_->resize (removed_indices_count);
137 }
138 
139 #define PCL_INSTANTIATE_CropBox(T) template class PCL_EXPORTS pcl::CropBox<T>;
140 
141 #endif // PCL_FILTERS_IMPL_CROP_BOX_H_
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
Definition: point_tests.h:53
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void getTransformation(Scalar x, Scalar y, Scalar z, Scalar roll, Scalar pitch, Scalar yaw, Eigen::Transform< Scalar, 3, Eigen::Affine > &t)
Create a transformation from the given translation and Euler angles (XYZ-convention) ...
Definition: eigen.hpp:687
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
A point structure representing Euclidean xyz coordinates, and the RGB color.
void applyFilter(PointCloud &output)
Sample of point indices into a separate PointCloud.
Definition: crop_box.hpp:48