void WorldDownloadManager::extractKnownWorker(kinfu_msgs::KinfuTsdfRequestConstPtr req)
{
ROS_INFO("kinfu: Extract Known Worker started.");
// prepare with same header
kinfu_msgs::KinfuTsdfResponsePtr resp(new kinfu_msgs::KinfuTsdfResponse());
resp->tsdf_header = req->tsdf_header;
resp->reference_frame_id = m_reference_frame_name;
ROS_INFO("kinfu: Locking kinfu...");
if (!lockKinfu())
return; // shutting down or synchronization error
ROS_INFO("kinfu: Locked.");
m_kinfu->syncKnownPoints();
if (req->request_reset)
m_kinfu->reset();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = m_cube_listener->GetOccupiedVoxelCenters();
unlockKinfu();
Eigen::Affine3f transformation = m_reverse_initial_transformation;
if (req->request_subsample)
{
ROS_INFO("kinfu: Subsampling to voxel size %f",float(req->subsample_voxel_size));
pcl::BitmaskOctree<3> octree;
octree.SetOccupiedVoxelsCenters(*cloud,req->subsample_voxel_size);
cloud->clear();
octree.GetOccupiedVoxelsCenters(*cloud,req->subsample_voxel_size);
}
// if needed, transform
if (req->request_transformation)
{
ROS_INFO("kinfu: A custom transformation will be applied.");
Eigen::Affine3f tr = toEigenAffine(req->transformation_linear,req->transformation_translation);
transformation = tr * transformation;
}
ROS_INFO("kinfu: Applying transformation...");
pcl::transformPointCloud(*cloud,*cloud,transformation);
// if needed, crop
if (req->request_bounding_box)
{
ROS_INFO("kinfu: Cropping...");
PointCloud::Ptr out_cloud(new PointCloud());
TrianglesPtr empty(new Triangles());
cropMesh(req->bounding_box_min,req->bounding_box_max,cloud,empty,out_cloud,empty);
cloud = out_cloud;
}
ROS_INFO("kinfu: Publishing...");
pclPointCloudToMessage(cloud,resp->point_cloud);
m_pub.publish(resp);
ROS_INFO("kinfu: Extract Known Worker complete.");
}
void WorldDownloadManager::extractMeshWorker(kinfu_msgs::KinfuTsdfRequestConstPtr req)
{
ROS_INFO("kinfu: Extract Mesh Worker started.");
// prepare with same header
kinfu_msgs::KinfuTsdfResponsePtr resp(new kinfu_msgs::KinfuTsdfResponse());
resp->tsdf_header = req->tsdf_header;
resp->reference_frame_id = m_reference_frame_name;
ROS_INFO("kinfu: Locking kinfu...");
if (!lockKinfu())
return; // shutting down or synchronization error
ROS_INFO("kinfu: Locked.");
pcl::PointCloud<pcl::PointXYZI>::Ptr kinfu_cloud = req->request_reset ?
m_kinfu->extractWorldAndReset() : m_kinfu->extractWorld();
unlockKinfu();
Eigen::Affine3f transformation = m_reverse_initial_transformation;
std::vector<Mesh::Ptr> meshes;
ROS_INFO("kinfu: Marching cubes...");
if (!marchingCubes(kinfu_cloud,meshes))
return; // ERROR
kinfu_cloud->clear(); // save some memory
std::vector<PointCloud::Ptr> clouds;
std::vector<TrianglesPtr> clouds_triangles;
ROS_INFO("kinfu: Divide triangles and points...");
for (uint i = 0; i < meshes.size(); i++)
{
clouds.push_back(PointCloud::Ptr(new PointCloud()));
clouds_triangles.push_back(TrianglesPtr(new Triangles()));
separateMesh(meshes[i],clouds[i],clouds_triangles[i]);
}
meshes.clear(); // save some memory
TrianglesPtr triangles(new Triangles());
PointCloud::Ptr cloud(new PointCloud());
ROS_INFO("kinfu: Merging points...");
mergePointCloudsAndMesh(clouds,cloud,&clouds_triangles,&(*triangles));
// if needed, transform
if (req->request_transformation)
{
ROS_INFO("kinfu: A custom transformation will be applied.");
Eigen::Affine3f tr = toEigenAffine(req->transformation_linear,req->transformation_translation);
transformation = tr * transformation;
}
ROS_INFO("kinfu: Applying transformation...");
pcl::transformPointCloud(*cloud,*cloud,transformation);
// if needed, crop
if (req->request_bounding_box)
{
ROS_INFO("kinfu: Cropping...");
TrianglesPtr out_triangles(new Triangles());
PointCloud::Ptr out_cloud(new PointCloud());
cropMesh(req->bounding_box_min,req->bounding_box_max,cloud,triangles,out_cloud,out_triangles);
cloud = out_cloud;
triangles = out_triangles;
}
ROS_INFO("kinfu: Publishing...");
pclPointCloudToMessage(cloud,resp->point_cloud);
resp->triangles.swap(*triangles);
m_pub.publish(resp);
ROS_INFO("kinfu: Extract Mesh Worker complete.");
}
Eigen::Affine3f WorldDownloadManager::toEigenAffine(const kinfu_msgs::KinfuPose & pose)
{
return toEigenAffine(pose.linear,pose.translation);
}
