Point Cloud Library (PCL) 1.12.0
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>
50
51template<typename PointT> void
52pcl::PlaneClipper3D<PointT>::setPlaneParameters (const Eigen::Vector4f& plane_params)
53{
54 plane_params_ = plane_params;
55}
56
57template<typename PointT> const Eigen::Vector4f&
59{
60 return plane_params_;
61}
62
63template<typename PointT> pcl::Clipper3D<PointT>*
65{
66 return new PlaneClipper3D<PointT> (plane_params_);
67}
68
69template<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
75template<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 */
84template<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 */
113template<typename PointT> void
114pcl::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 */
170template<typename PointT> void
171pcl::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.
179template<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
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 ~PlaneClipper3D() noexcept
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
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.