35 #ifndef PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
36 #define PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP
38 #include <pcl/filters/plane_clipper3D.h>
40 template<
typename Po
intT>
42 : plane_params_ (plane_params)
46 template<
typename Po
intT>
51 template<
typename Po
intT>
void
54 plane_params_ = plane_params;
57 template<
typename Po
intT>
const Eigen::Vector4f&
69 template<
typename Po
intT>
float
72 return (plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z + plane_params_[3]);
75 template<
typename Po
intT>
bool
78 return ((plane_params_[0] * point.x + plane_params_[1] * point.y + plane_params_[2] * point.z ) >= -plane_params_[3]);
84 template<
typename Po
intT>
bool
87 float dist1 = getDistance (point1);
88 float dist2 = getDistance (point2);
90 if (dist1 * dist2 > 0)
93 float lambda = dist2 / (dist2 - dist1);
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;
103 point2 = intersection;
105 point1 = intersection;
113 template<
typename Po
intT>
void
116 clipped_polygon.clear ();
117 clipped_polygon.reserve (polygon.size ());
120 if (polygon.size () < 3)
122 if (polygon.size () == 1)
125 if (clipPoint3D (polygon [0]))
126 clipped_polygon.push_back (polygon [0]);
128 else if (polygon.size () == 2)
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 ();
138 float previous_distance = getDistance (polygon [0]);
140 if (previous_distance > 0)
141 clipped_polygon.push_back (polygon [0]);
143 typename std::vector<PointT, Eigen::aligned_allocator<PointT> >::const_iterator prev_it = polygon.begin ();
145 for (
typename std::vector<
PointT, Eigen::aligned_allocator<PointT> >::const_iterator pIt = prev_it + 1; pIt != polygon.end (); prev_it = pIt++)
148 float distance = getDistance (*pIt);
149 if (
distance * previous_distance < 0)
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;
158 clipped_polygon.push_back (intersection);
161 clipped_polygon.push_back (*pIt);
170 template<
typename Po
intT>
void
173 std::vector<PointT, Eigen::aligned_allocator<PointT> > clipped;
174 clipPlanarPolygon3D (polygon, clipped);
179 template<
typename Po
intT>
void
182 if (indices.empty ())
184 clipped.reserve (cloud_in.
size ());
216 for (
unsigned pIdx = 0; pIdx < cloud_in.
size (); ++pIdx)
217 if (clipPoint3D (cloud_in[pIdx]))
218 clipped.push_back (pIdx);
222 for (
const auto& index : indices)
223 if (clipPoint3D (cloud_in[index]))
224 clipped.push_back (index);
Base class for 3D clipper objects.
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.
float distance(const PointT &p1, const PointT &p2)
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.