// /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; } }