void WorldDownloadManager::cropCloudWithSphere(const Eigen::Vector3f & center,const float radius, typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud) { const uint cloud_size = cloud->size(); std::vector<bool> valid_points(cloud_size,true); // check the points for (uint i = 0; i < cloud_size; i++) { const PointT & pt = (*cloud)[i]; const Eigen::Vector3f ept(pt.x,pt.y,pt.z); if ((ept - center).squaredNorm() > radius * radius) valid_points[i] = false; } // discard invalid points out_cloud->clear(); out_cloud->reserve(cloud_size); uint count = 0; for (uint i = 0; i < cloud_size; i++) if (valid_points[i]) { out_cloud->push_back((*cloud)[i]); count++; } out_cloud->resize(count); }
void WorldDownloadManager::cropCloud(const Eigen::Vector3f & min,const Eigen::Vector3f & max, typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud) { const uint cloud_size = cloud->size(); std::vector<bool> valid_points(cloud_size,true); // check the points for (uint i = 0; i < cloud_size; i++) { const PointT & pt = (*cloud)[i]; if (pt.x > max.x() || pt.y > max.y() || pt.z > max.z() || pt.x < min.x() || pt.y < min.y() || pt.z < min.z()) valid_points[i] = false; } // discard invalid points out_cloud->clear(); out_cloud->reserve(cloud_size); uint count = 0; for (uint i = 0; i < cloud_size; i++) if (valid_points[i]) { out_cloud->push_back((*cloud)[i]); count++; } out_cloud->resize(count); }
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min, const kinfu_msgs::KinfuCloudPoint & max,typename pcl::PointCloud<PointT>::ConstPtr cloud, TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud, TrianglesPtr out_triangles) { const uint triangles_size = triangles->size(); const uint cloud_size = cloud->size(); std::vector<bool> valid_points(cloud_size,true); std::vector<uint> valid_points_remap(cloud_size,0); uint offset; // check the points for (uint i = 0; i < cloud_size; i++) { const PointT & pt = (*cloud)[i]; if (pt.x > max.x || pt.y > max.y || pt.z > max.z || pt.x < min.x || pt.y < min.y || pt.z < min.z) valid_points[i] = false; } // discard invalid points out_cloud->clear(); out_cloud->reserve(cloud_size); offset = 0; for (uint i = 0; i < cloud_size; i++) if (valid_points[i]) { out_cloud->push_back((*cloud)[i]); // save new position for triangles remap valid_points_remap[i] = offset; offset++; } out_cloud->resize(offset); // discard invalid triangles out_triangles->clear(); out_triangles->reserve(triangles_size); offset = 0; for (uint i = 0; i < triangles_size; i++) { const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i]; bool is_valid = true; // validate all the vertices for (uint h = 0; h < 3; h++) if (!valid_points[tri.vertex_id[h]]) { is_valid = false; break; } if (is_valid) { kinfu_msgs::KinfuMeshTriangle out_tri; // remap the triangle for (uint h = 0; h < 3; h++) out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]]; out_triangles->push_back(out_tri); offset++; } } out_triangles->resize(offset); }
template <typename PointT> void pcl::approximatePolygon2D (const typename pcl::PointCloud<PointT>::VectorType &polygon, typename pcl::PointCloud<PointT>::VectorType &approx_polygon, float threshold, bool refine, bool closed) { approx_polygon.clear (); if (polygon.size () < 3) return; std::vector<std::pair<unsigned, unsigned> > intervals; std::pair<unsigned,unsigned> interval (0, 0); if (closed) { float max_distance = .0f; for (unsigned idx = 1; idx < polygon.size (); ++idx) { float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) + (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y); if (distance > max_distance) { max_distance = distance; interval.second = idx; } } for (unsigned idx = 1; idx < polygon.size (); ++idx) { float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) + (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y); if (distance > max_distance) { max_distance = distance; interval.first = idx; } } if (max_distance < threshold * threshold) return; intervals.push_back (interval); std::swap (interval.first, interval.second); intervals.push_back (interval); } else { interval.first = 0; interval.second = static_cast<unsigned int> (polygon.size ()) - 1; intervals.push_back (interval); } std::vector<unsigned> result; // recursively refine while (!intervals.empty ()) { std::pair<unsigned, unsigned>& currentInterval = intervals.back (); float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y; float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x; float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x; float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y); line_x *= linelen; line_y *= linelen; line_d *= linelen; float max_distance = 0.0; unsigned first_index = currentInterval.first + 1; unsigned max_index = 0; // => 0-crossing if (currentInterval.first > currentInterval.second) { for (unsigned idx = first_index; idx < polygon.size(); idx++) { float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d); if (distance > max_distance) { max_distance = distance; max_index = idx; } } first_index = 0; } for (unsigned int idx = first_index; idx < currentInterval.second; idx++) { float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d); if (distance > max_distance) { max_distance = distance; max_index = idx; } } if (max_distance > threshold) { std::pair<unsigned, unsigned> interval (max_index, currentInterval.second); currentInterval.second = max_index; intervals.push_back (interval); } else { result.push_back (currentInterval.second); intervals.pop_back (); } } approx_polygon.reserve (result.size ()); if (refine) { std::vector<Eigen::Vector3f> lines (result.size ()); std::reverse (result.begin (), result.end ()); for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx) { unsigned nIdx = rIdx + 1; if (nIdx == result.size ()) nIdx = 0; Eigen::Vector2f centroid = Eigen::Vector2f::Zero (); Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero (); unsigned pIdx = result[rIdx]; unsigned num_points = 0; if (pIdx > result[nIdx]) { num_points = static_cast<unsigned> (polygon.size ()) - pIdx; for (; pIdx < polygon.size (); ++pIdx) { covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x; covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y; covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y; centroid [0] += polygon [pIdx].x; centroid [1] += polygon [pIdx].y; } pIdx = 0; } num_points += result[nIdx] - pIdx; for (; pIdx < result[nIdx]; ++pIdx) { covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x; covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y; covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y; centroid [0] += polygon [pIdx].x; centroid [1] += polygon [pIdx].y; } covariance.coeffRef (2) = covariance.coeff (1); float norm = 1.0f / float (num_points); centroid *= norm; covariance *= norm; covariance.coeffRef (0) -= centroid [0] * centroid [0]; covariance.coeffRef (1) -= centroid [0] * centroid [1]; covariance.coeffRef (3) -= centroid [1] * centroid [1]; float eval; Eigen::Vector2f normal; eigen22 (covariance, eval, normal); // select the one which is more "parallel" to the original line Eigen::Vector2f direction; direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x; direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y; direction.normalize (); if (fabs (direction.dot (normal)) > float(M_SQRT1_2)) { std::swap (normal [0], normal [1]); normal [0] = -normal [0]; } // needs to be on the left side of the edge if (direction [0] * normal [1] < direction [1] * normal [0]) normal *= -1.0; lines [rIdx].head<2> () = normal; lines [rIdx] [2] = -normal.dot (centroid); } float threshold2 = threshold * threshold; for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx) { unsigned nIdx = rIdx + 1; if (nIdx == result.size ()) nIdx = 0; Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]); vertex /= vertex [2]; vertex [2] = 0.0; PointT point; // test whether we need another edge since the intersection point is too far away from the original vertex Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex; pq [2] = 0.0; float distance = pq.squaredNorm (); if (distance > threshold2) { // test whether the old point is inside the new polygon or outside if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) && (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) ) { float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2]; float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2]; point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0]; point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1]; approx_polygon.push_back (point); vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0]; vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1]; } } point.getVector3fMap () = vertex; approx_polygon.push_back (point); } } else { // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise) for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it) approx_polygon.push_back (polygon [*it]); } }