Point Cloud Library (PCL)  1.11.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>
48 {
49 }
50 
51 template<typename PointT> void
52 pcl::PlaneClipper3D<PointT>::setPlaneParameters (const Eigen::Vector4f& plane_params)
53 {
54  plane_params_ = plane_params;
55 }
56 
57 template<typename PointT> const Eigen::Vector4f&
59 {
60  return plane_params_;
61 }
62 
63 template<typename PointT> pcl::Clipper3D<PointT>*
65 {
66  return new PlaneClipper3D<PointT> (plane_params_);
67 }
68 
69 template<typename PointT> float
71 {
72  return (plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z + plane_params_[3]);
73 }
74 
75 template<typename PointT> bool
77 {
78  return ((plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z ) >= -plane_params_[3]);
79 }
80 
81 /**
82  * @attention untested code
83  */
84 template<typename PointT> bool
86 {
87  float dist1 = getDistance (point1);
88  float dist2 = getDistance (point2);
89 
90  if (dist1 * dist2 > 0) // both on same side of the plane -> nothing to clip
91  return (dist1 > 0); // true if both are on positive side, thus visible
92 
93  float lambda = dist2 / (dist2 - dist1);
94 
95  // get the plane intersecion
96  PointT intersection;
97  intersection.x = (point1.x - point2.x) * lambda + point2.x;
98  intersection.y = (point1.y - point2.y) * lambda + point2.y;
99  intersection.z = (point1.z - point2.z) * lambda + point2.z;
100 
101  // point1 is visible, point2 not => point2 needs to be replaced by intersection
102  if (dist1 >= 0)
103  point2 = intersection;
104  else
105  point1 = intersection;
106 
107  return false;
108 }
109 
110 /**
111  * @attention untested code
112  */
113 template<typename PointT> void
114 pcl::PlaneClipper3D<PointT>::clipPlanarPolygon3D (const std::vector<PointT, Eigen::aligned_allocator<PointT> >& polygon, std::vector<PointT, Eigen::aligned_allocator<PointT> >& clipped_polygon) const
115 {
116  clipped_polygon.clear ();
117  clipped_polygon.reserve (polygon.size ());
118 
119  // test for degenerated polygons
120  if (polygon.size () < 3)
121  {
122  if (polygon.size () == 1)
123  {
124  // point outside clipping area ?
125  if (clipPoint3D (polygon [0]))
126  clipped_polygon.push_back (polygon [0]);
127  }
128  else if (polygon.size () == 2)
129  {
130  clipped_polygon.push_back (polygon [0]);
131  clipped_polygon.push_back (polygon [1]);
132  if (!clipLineSegment3D (clipped_polygon [0], clipped_polygon [1]))
133  clipped_polygon.clear ();
134  }
135  return;
136  }
137 
138  float previous_distance = getDistance (polygon [0]);
139 
140  if (previous_distance > 0)
141  clipped_polygon.push_back (polygon [0]);
142 
143  typename std::vector<PointT, Eigen::aligned_allocator<PointT> >::const_iterator prev_it = polygon.begin ();
144 
145  for (typename std::vector<PointT, Eigen::aligned_allocator<PointT> >::const_iterator pIt = prev_it + 1; pIt != polygon.end (); prev_it = pIt++)
146  {
147  // if we intersect plane
148  float distance = getDistance (*pIt);
149  if (distance * previous_distance < 0)
150  {
151  float lambda = distance / (distance - previous_distance);
152 
153  PointT intersection;
154  intersection.x = (prev_it->x - pIt->x) * lambda + pIt->x;
155  intersection.y = (prev_it->y - pIt->y) * lambda + pIt->y;
156  intersection.z = (prev_it->z - pIt->z) * lambda + pIt->z;
157 
158  clipped_polygon.push_back (intersection);
159  }
160  if (distance > 0)
161  clipped_polygon.push_back (*pIt);
162 
163  previous_distance = distance;
164  }
165 }
166 
167 /**
168  * @attention untested code
169  */
170 template<typename PointT> void
171 pcl::PlaneClipper3D<PointT>::clipPlanarPolygon3D (std::vector<PointT, Eigen::aligned_allocator<PointT> > &polygon) const
172 {
173  std::vector<PointT, Eigen::aligned_allocator<PointT> > clipped;
174  clipPlanarPolygon3D (polygon, clipped);
175  polygon = clipped;
176 }
177 
178 // /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
179 template<typename PointT> void
181 {
182  if (indices.empty ())
183  {
184  clipped.reserve (cloud_in.size ());
185 
186 // #if 0
187 // Eigen::MatrixXf points = cloud_in.getMatrixXfMap (4, sizeof (PointT) / sizeof (float), offsetof(PointT,x) / sizeof (float));
188 // Eigen::VectorXf distances = plane_params_.transpose () * points;
189 // for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
190 // {
191 // if (distances (rIdx, 0) >= -plane_params_[3])
192 // clipped.push_back (rIdx);
193 // }
194 // #else
195 // Eigen::Matrix4Xf points (4, cloud_in.size ());
196 // for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
197 // {
198 // points (0, rIdx) = cloud_in[rIdx].x;
199 // points (1, rIdx) = cloud_in[rIdx].y;
200 // points (2, rIdx) = cloud_in[rIdx].z;
201 // points (3, rIdx) = 1;
202 // }
203 // Eigen::VectorXf distances = plane_params_.transpose () * points;
204 // for (unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
205 // {
206 // if (distances (rIdx, 0) >= 0)
207 // clipped.push_back (rIdx);
208 // }
209 //
210 // #endif
211 //
212 // //std::cout << "points : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << std::endl;
213 //
214 // //std::cout << "distances: " << distances.rows () << " x " << distances.cols () << std::endl;
215 
216  for (unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
217  if (clipPoint3D (cloud_in[pIdx]))
218  clipped.push_back (pIdx);
219  }
220  else
221  {
222  for (const auto& index : indices)
223  if (clipPoint3D (cloud_in[index]))
224  clipped.push_back (index);
225  }
226 }
227 #endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
pcl::PlaneClipper3D::clipPlanarPolygon3D
virtual void clipPlanarPolygon3D(std::vector< PointT, Eigen::aligned_allocator< PointT > > &polygon) const
Definition: plane_clipper3D.hpp:171
pcl::PlaneClipper3D::clipPoint3D
virtual bool clipPoint3D(const PointT &point) const
interface to clip a single point
Definition: plane_clipper3D.hpp:76
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::Clipper3D
Base class for 3D clipper objects.
Definition: clipper3D.h:54
pcl::PlaneClipper3D::clipLineSegment3D
virtual bool clipLineSegment3D(PointT &from, PointT &to) const
Definition: plane_clipper3D.hpp:85
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::PlaneClipper3D::clone
virtual Clipper3D< PointT > * clone() const
polymorphic method to clone the underlying clipper with its parameters.
Definition: plane_clipper3D.hpp:64
pcl::PlaneClipper3D
Implementation of a plane clipper in 3D.
Definition: plane_clipper3D.h:50
pcl::PlaneClipper3D::clipPointCloud3D
virtual void clipPointCloud3D(const pcl::PointCloud< PointT > &cloud_in, Indices &clipped, const Indices &indices=Indices()) const
interface to clip a point cloud
Definition: plane_clipper3D.hpp:180
pcl::PlaneClipper3D::~PlaneClipper3D
virtual ~PlaneClipper3D() noexcept
Definition: plane_clipper3D.hpp:47
pcl::PlaneClipper3D::PlaneClipper3D
PlaneClipper3D(const Eigen::Vector4f &plane_params)
Constructor taking the homogeneous representation of the plane as a Eigen::Vector4f.
Definition: plane_clipper3D.hpp:41
pcl::PlaneClipper3D::getPlaneParameters
const Eigen::Vector4f & getPlaneParameters() const
return the current plane parameters
Definition: plane_clipper3D.hpp:58
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::PlaneClipper3D::setPlaneParameters
void setPlaneParameters(const Eigen::Vector4f &plane_params)
Set new plane parameters.
Definition: plane_clipper3D.hpp:52
pcl::PlaneClipper3D::getDistance
float getDistance(const PointT &point) const
Definition: plane_clipper3D.hpp:70