int NormalEstimation::compute()
{
//pointer to selected cloud
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
//if we have normals delete them!
if (cloud->hasNormals())
cloud->unallocateNorms();
if (cloud->hasNormals())
cloud->unallocateNorms();
//get xyz in sensor_msgs format
cc2smReader converter;
converter.setInputCloud(cloud);
sensor_msgs::PointCloud2 sm_cloud = converter.getXYZ();
//get as pcl point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(sm_cloud, *pcl_cloud);
//create storage for normals
pcl::PointCloud<pcl::PointNormal>::Ptr normals (new pcl::PointCloud<pcl::PointNormal>);
//now compute
int result = compute_normals<pcl::PointXYZ, pcl::PointNormal>(pcl_cloud, m_useKnn ? m_knn_radius: m_radius, m_useKnn, normals);
sensor_msgs::PointCloud2::Ptr sm_normals (new sensor_msgs::PointCloud2);
pcl::toROSMsg(*normals, *sm_normals);
sm2ccConverter converter2(sm_normals);
converter2.addNormals(cloud);
converter2.addScalarField(cloud, "curvature", m_overwrite_curvature);
emit entityHasChanged(cloud);
return 1;
}
int MLSSmoothingUpsampling::compute()
{
//pointer to selected cloud
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
//get xyz in sensor_msgs format
cc2smReader converter;
converter.setInputCloud(cloud);
//take out the xyz info
sensor_msgs::PointCloud2 sm_xyz = converter.getXYZ();
sensor_msgs::PointCloud2 sm_cloud;
//take out the current scalar field (if any)
if (cloud->getCurrentDisplayedScalarField())
{
const char* current_sf_name = cloud->getCurrentDisplayedScalarField()->getName();
sensor_msgs::PointCloud2 sm_field = converter.getFloatScalarField(current_sf_name);
//change its name
std::string new_name = "scalar";
sm_field.fields[0].name = new_name.c_str();
//put everithing together
pcl::concatenateFields(sm_xyz, sm_field, sm_cloud);
}
else
{
sm_cloud = sm_xyz;
}
//get as pcl point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(sm_cloud, *pcl_cloud);
//create storage for outcloud
pcl::PointCloud<pcl::PointNormal>::Ptr normals (new pcl::PointCloud<pcl::PointNormal>);
#ifdef LP_PCL_PATCH_ENABLED
pcl::PointIndicesPtr mapping_indices;
smooth_mls<pcl::PointXYZ, pcl::PointNormal> (pcl_cloud, *m_parameters, normals, mapping_indices);
#else
smooth_mls<pcl::PointXYZ, pcl::PointNormal> (pcl_cloud, *m_parameters, normals);
#endif
sensor_msgs::PointCloud2::Ptr sm_normals (new sensor_msgs::PointCloud2);
pcl::toROSMsg(*normals, *sm_normals);
ccPointCloud* new_cloud = sm2ccConverter(sm_normals).getCCloud();
if (!new_cloud)
{
//conversion failed (not enough memory?)
return -1;
}
new_cloud->setName(cloud->getName()+QString("_smoothed")); //original name + suffix
new_cloud->setDisplay(cloud->getDisplay());
#ifdef LP_PCL_PATCH_ENABLED
//copy the original scalar fields here
copyScalarFields(cloud, new_cloud, mapping_indices, true);
#endif
//disable original cloud
cloud->setEnabled(false);
if (cloud->getParent())
cloud->getParent()->addChild(new_cloud);
emit newEntity(new_cloud);
return 1;
}
