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);
227 #endif //PCL_FILTERS_IMPL_PLANE_CLIPPER3D_HPP