// /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
template<typename PointT> void
pcl::PlaneClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, std::vector<int>& clipped, const std::vector<int>& indices) const
{
if (indices.empty ())
{
clipped.reserve (cloud_in.size ());
/*
#if 0
Eigen::MatrixXf points = cloud_in.getMatrixXfMap (4, sizeof (PointT) / sizeof (float), offsetof(PointT,x) / sizeof (float));
Eigen::VectorXf distances = plane_params_.transpose () * points;
for (register unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
{
if (distances (rIdx, 0) >= -plane_params_[3])
clipped.push_back (rIdx);
}
#else
Eigen::Matrix4Xf points (4, cloud_in.size ());
for (register unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
{
points (0, rIdx) = cloud_in[rIdx].x;
points (1, rIdx) = cloud_in[rIdx].y;
points (2, rIdx) = cloud_in[rIdx].z;
points (3, rIdx) = 1;
}
Eigen::VectorXf distances = plane_params_.transpose () * points;
for (register unsigned rIdx = 0; rIdx < cloud_in.size (); ++ rIdx)
{
if (distances (rIdx, 0) >= 0)
clipped.push_back (rIdx);
}
#endif
//cout << "points : " << points.rows () << " x " << points.cols () << " * " << plane_params_.transpose ().rows () << " x " << plane_params_.transpose ().cols () << endl;
//cout << "distances: " << distances.rows () << " x " << distances.cols () << endl;
/*/
for (register unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
if (clipPoint3D (cloud_in[pIdx]))
clipped.push_back (pIdx);
//*/
}
else
{
for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
if (clipPoint3D (cloud_in[*iIt]))
clipped.push_back (*iIt);
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
template<typename PointT> void
pcl::BoxClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, std::vector<int>& clipped, const std::vector<int>& indices) const
{
clipped.clear ();
if (indices.empty ())
{
clipped.reserve (cloud_in.size ());
for (size_t pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
if (clipPoint3D (cloud_in[pIdx]))
clipped.push_back (pIdx);
}
else
{
for (const int &index : indices)
if (clipPoint3D (cloud_in[index]))
clipped.push_back (index);
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// /ToDo: write fast version using eigen map and single matrix vector multiplication, that uses advantages of eigens SSE operations.
template<typename PointT> void
pcl::BoxClipper3D<PointT>::clipPointCloud3D (const pcl::PointCloud<PointT>& cloud_in, std::vector<int>& clipped, const std::vector<int>& indices) const
{
clipped.clear ();
if (indices.empty ())
{
clipped.reserve (cloud_in.size ());
for (register unsigned pIdx = 0; pIdx < cloud_in.size (); ++pIdx)
if (clipPoint3D (cloud_in[pIdx]))
clipped.push_back (pIdx);
}
else
{
for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
if (clipPoint3D (cloud_in[*iIt]))
clipped.push_back (*iIt);
}
}
/**
* @attention untested code
*/
template<typename PointT> void
pcl::PlaneClipper3D<PointT>::clipPlanarPolygon3D (const std::vector<PointT>& polygon, std::vector<PointT>& clipped_polygon) const
{
clipped_polygon.clear ();
clipped_polygon.reserve (polygon.size ());
// test for degenerated polygons
if (polygon.size () < 3)
{
if (polygon.size () == 1)
{
// point outside clipping area ?
if (clipPoint3D (polygon [0]))
clipped_polygon.push_back (polygon [0]);
}
else if (polygon.size () == 2)
{
clipped_polygon.push_back (polygon [0]);
clipped_polygon.push_back (polygon [1]);
if (!clipLineSegment3D (clipped_polygon [0], clipped_polygon [1]))
clipped_polygon.clear ();
}
return;
}
float previous_distance = getDistance (polygon [0]);
if (previous_distance > 0)
clipped_polygon.push_back (polygon [0]);
typename std::vector<PointT>::const_iterator prev_it = polygon.begin ();
for (typename std::vector<PointT>::const_iterator pIt = prev_it + 1; pIt != polygon.end (); prev_it = pIt++)
{
// if we intersect plane
float distance = getDistance (*pIt);
if (distance * previous_distance < 0)
{
float lambda = distance / (distance - previous_distance);
PointT intersection;
intersection.x = (prev_it->x - pIt->x) * lambda + pIt->x;
intersection.y = (prev_it->y - pIt->y) * lambda + pIt->y;
intersection.z = (prev_it->z - pIt->z) * lambda + pIt->z;
clipped_polygon.push_back (intersection);
}
if (distance > 0)
clipped_polygon.push_back (*pIt);
previous_distance = distance;
}
}
