Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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
40template<typename PointT>
41pcl::PlaneClipper3D<PointT>::PlaneClipper3D (const Eigen::Vector4f& plane_params)
42: plane_params_ (plane_params)
43{
44}
45
46template<typename PointT> void
47pcl::PlaneClipper3D<PointT>::setPlaneParameters (const Eigen::Vector4f& plane_params)
48{
49 plane_params_ = plane_params;
50}
51
52template<typename PointT> const Eigen::Vector4f&
54{
55 return plane_params_;
56}
57
58template<typename PointT> pcl::Clipper3D<PointT>*
60{
61 return new PlaneClipper3D<PointT> (plane_params_);
62}
63
64template<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
70template<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 */
79template<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 */
108template<typename PointT> void
109pcl::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 */
165template<typename PointT> void
166pcl::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.
174template<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.
std::size_t size() const
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.