Point Cloud Library (PCL)  1.7.1
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>& polygon, std::vector<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>::const_iterator prev_it = polygon.begin ();
144 
145  for (typename std::vector<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>& polygon) const
172 {
173  std::vector<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
180 pcl::PlaneClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, std::vector<int>& clipped, const std::vector<int>& indices) const
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 (register 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 (register 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 (register 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  //cout << "points : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << endl;
213 
214  //cout << "distances: " << distances.rows () << " x " << distances.cols () << endl;
215  /*/
216  for (register unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
217  if (clipPoint3D (cloud_in[pIdx]))
218  clipped.push_back (pIdx);
219  //*/
220  }
221  else
222  {
223  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
224  if (clipPoint3D (cloud_in[*iIt]))
225  clipped.push_back (*iIt);
226  }
227 }
228 #endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP