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