Point Cloud Library (PCL)  1.14.1-dev
plane_clipper3D.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
36 #define PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
37 
38 #include <pcl/filters/plane_clipper3D.h>
39 
40 template<typename PointT>
41 pcl::PlaneClipper3D<PointT>::PlaneClipper3D (const Eigen::Vector4f& plane_params)
42 : plane_params_ (plane_params)
43 {
44 }
45 
46 template<typename PointT> void
47 pcl::PlaneClipper3D<PointT>::setPlaneParameters (const Eigen::Vector4f& plane_params)
48 {
49  plane_params_ = plane_params;
50 }
51 
52 template<typename PointT> const Eigen::Vector4f&
54 {
55  return plane_params_;
56 }
57 
58 template<typename PointT> pcl::Clipper3D<PointT>*
60 {
61  return new PlaneClipper3D<PointT> (plane_params_);
62 }
63 
64 template<typename PointT> float
66 {
67  return (plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z + plane_params_[3]);
68 }
69 
70 template<typename PointT> bool
72 {
73  return ((plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z ) >= -plane_params_[3]);
74 }
75 
76 /**
77  * @attention untested code
78  */
79 template<typename PointT> bool
81 {
82  float dist1 = getDistance (point1);
83  float dist2 = getDistance (point2);
84 
85  if (dist1 * dist2 > 0) // both on same side of the plane -> nothing to clip
86  return (dist1 > 0); // true if both are on positive side, thus visible
87 
88  float lambda = dist2 / (dist2 - dist1);
89 
90  // get the plane intersecion
91  PointT intersection;
92  intersection.x = (point1.x - point2.x) * lambda + point2.x;
93  intersection.y = (point1.y - point2.y) * lambda + point2.y;
94  intersection.z = (point1.z - point2.z) * lambda + point2.z;
95 
96  // point1 is visible, point2 not => point2 needs to be replaced by intersection
97  if (dist1 >= 0)
98  point2 = intersection;
99  else
100  point1 = intersection;
101 
102  return false;
103 }
104 
105 /**
106  * @attention untested code
107  */
108 template<typename PointT> void
109 pcl::PlaneClipper3D<PointT>::clipPlanarPolygon3D (const std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon, std::vector<PointT, Eigen::aligned_allocator<PointT> >& clipped_polygon) const
110 {
111  clipped_polygon.clear ();
112  clipped_polygon.reserve (polygon.size ());
113 
114  // test for degenerated polygons
115  if (polygon.size () < 3)
116  {
117  if (polygon.size () == 1)
118  {
119  // point outside clipping area ?
120  if (clipPoint3D (polygon [0]))
121  clipped_polygon.push_back (polygon [0]);
122  }
123  else if (polygon.size () == 2)
124  {
125  clipped_polygon.push_back (polygon [0]);
126  clipped_polygon.push_back (polygon [1]);
127  if (!clipLineSegment3D (clipped_polygon [0], clipped_polygon [1]))
128  clipped_polygon.clear ();
129  }
130  return;
131  }
132 
133  float previous_distance = getDistance (polygon [0]);
134 
135  if (previous_distance > 0)
136  clipped_polygon.push_back (polygon [0]);
137 
138  typename std::vector<PointT, Eigen::aligned_allocator<PointT> >::const_iterator prev_it = polygon.begin ();
139 
140  for (typename std::vector<PointT, Eigen::aligned_allocator<PointT> >::const_iterator pIt = prev_it + 1; pIt != polygon.end (); prev_it = pIt++)
141  {
142  // if we intersect plane
143  float distance = getDistance (*pIt);
144  if (distance * previous_distance < 0)
145  {
146  float lambda = distance / (distance - previous_distance);
147 
148  PointT intersection;
149  intersection.x = (prev_it->x - pIt->x) * lambda + pIt->x;
150  intersection.y = (prev_it->y - pIt->y) * lambda + pIt->y;
151  intersection.z = (prev_it->z - pIt->z) * lambda + pIt->z;
152 
153  clipped_polygon.push_back (intersection);
154  }
155  if (distance > 0)
156  clipped_polygon.push_back (*pIt);
157 
158  previous_distance = distance;
159  }
160 }
161 
162 /**
163  * @attention untested code
164  */
165 template<typename PointT> void
166 pcl::PlaneClipper3D<PointT>::clipPlanarPolygon3D (std::vector<PointT, Eigen::aligned_allocator<PointT> > &polygon) const
167 {
168  std::vector<PointT, Eigen::aligned_allocator<PointT> > clipped;
169  clipPlanarPolygon3D (polygon, clipped);
170  polygon = clipped;
171 }
172 
173 // /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
174 template<typename PointT> void
176 {
177  if (indices.empty ())
178  {
179  clipped.reserve (cloud_in.size ());
180 
181 // #if 0
182 // Eigen::MatrixXf points = cloud_in.getMatrixXfMap (4, sizeof (PointT) / sizeof (float), offsetof(PointT,x) / sizeof (float));
183 // Eigen::VectorXf distances = plane_params_.transpose () * points;
184 // for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
185 // {
186 // if (distances (rIdx, 0) >= -plane_params_[3])
187 // clipped.push_back (rIdx);
188 // }
189 // #else
190 // Eigen::Matrix4Xf points (4, cloud_in.size ());
191 // for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
192 // {
193 // points (0, rIdx) = cloud_in[rIdx].x;
194 // points (1, rIdx) = cloud_in[rIdx].y;
195 // points (2, rIdx) = cloud_in[rIdx].z;
196 // points (3, rIdx) = 1;
197 // }
198 // Eigen::VectorXf distances = plane_params_.transpose () * points;
199 // for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
200 // {
201 // if (distances (rIdx, 0) >= 0)
202 // clipped.push_back (rIdx);
203 // }
204 //
205 // #endif
206 //
207 // //std::cout << "points : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << std::endl;
208 //
209 // //std::cout << "distances: " << distances.rows () << " x " << distances.cols () << std::endl;
210 
211  for (unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
212  if (clipPoint3D (cloud_in[pIdx]))
213  clipped.push_back (pIdx);
214  }
215  else
216  {
217  for (const auto& index : indices)
218  if (clipPoint3D (cloud_in[index]))
219  clipped.push_back (index);
220  }
221 }
222 #endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
Base class for 3D clipper objects.
Definition: clipper3D.h:55
Implementation of a plane clipper in 3D.
void setPlaneParameters(const Eigen::Vector4f &plane_params)
Set new plane parameters.
const Eigen::Vector4f & getPlaneParameters() const
return the current plane parameters
virtual bool clipPoint3D(const PointT &point) const
interface to clip a single point
virtual Clipper3D< PointT > * clone() const
polymorphic method to clone the underlying clipper with its parameters.
virtual bool clipLineSegment3D(PointT &from, PointT &to) const
virtual void clipPlanarPolygon3D(std::vector< PointT, Eigen::aligned_allocator< PointT > > &polygon) const
virtual void clipPointCloud3D(const pcl::PointCloud< PointT > &cloud_in, Indices &clipped, const Indices &indices=Indices()) const
interface to clip a point cloud
PCL_MAKE_ALIGNED_OPERATOR_NEW PlaneClipper3D(const Eigen::Vector4f &plane_params)
Constructor taking the homogeneous representation of the plane as a Eigen::Vector4f.
float getDistance(const PointT &point) const
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
std::size_t size() const
Definition: point_cloud.h:443
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.