void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
const kinfu_msgs::KinfuCloudPoint & max,PointCloud::ConstPtr cloud,
TrianglesConstPtr triangles,PointCloud::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);
std::cout << "Starting with " << cloud_size << " points and " << triangles_size << " triangles.\n";
uint offset;
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const pcl::PointXYZ & 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);
std::cout << "Ended with " << out_cloud->size() << " points and " << out_triangles->size() << " triangles.\n";
}
void WorldDownloadManager::removeDuplicatePoints(typename pcl::PointCloud<PointT>::ConstPtr cloud,
TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud,TrianglesPtr out_triangles)
{
const uint64 input_size = cloud->size();
std::vector<uint64> ordered(input_size);
for (uint64 i = 0; i < input_size; i++)
ordered[i] = i;
std::sort(ordered.begin(),ordered.end(),WorldDownloadManager_mergeMeshTrianglesComparator<PointT>(*cloud));
typedef WorldDownloadManager_mergeMeshTrianglesAverageClass<PointT> AverageClass;
std::vector<uint64> re_association_vector(input_size);
std::vector<AverageClass, Eigen::aligned_allocator<AverageClass> > average_vector;
uint64 output_i = 0;
re_association_vector[ordered[0]] = 0;
for (uint64 src_i = 1; src_i < input_size; src_i++)
{
const PointT & prev = (*cloud)[ordered[src_i - 1]];
const PointT & current = (*cloud)[ordered[src_i]];
const int not_equal = std::memcmp(prev.data,current.data,sizeof(float) * 3);
if (not_equal)
output_i++;
re_association_vector[ordered[src_i]] = output_i;
}
const uint64 output_size = output_i + 1;
out_cloud->resize(output_size);
average_vector.resize(output_size);
for (uint64 i = 0; i < input_size; i++)
average_vector[re_association_vector[i]].insert((*cloud)[i]);
for (uint64 i = 0; i < output_size; i++)
(*out_cloud)[i] = average_vector[i].get();
if (out_triangles && triangles)
{
const uint64 triangles_size = triangles->size();
uint64 out_triangles_size = 0;
out_triangles->resize(triangles_size);
for (uint64 i = 0; i < triangles_size; i++)
{
kinfu_msgs::KinfuMeshTriangle new_tri;
for (uint h = 0; h < 3; h++)
new_tri.vertex_id[h] = re_association_vector[(*triangles)[i].vertex_id[h]];
bool degenerate = false;
for (uint h = 0; h < 3; h++)
if (new_tri.vertex_id[h] == new_tri.vertex_id[(h + 1) % 3])
degenerate = true;
if (!degenerate)
{
(*out_triangles)[out_triangles_size] = new_tri;
out_triangles_size++;
}
}
out_triangles->resize(out_triangles_size);
}
}
