int BaseFilter::hasSelectedScalarField(std::string field_name)
{
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
int result = cloud->getScalarFieldIndexByName(field_name.c_str());
return (result >= 0 ? 1 : 0);
}
int BaseFilter::hasSelectedRGB()
{
if (isFirstSelectedCcPointCloud() != 1)
return -1;
//get the cloud
ccPointCloud * cloud;
cloud = getSelectedEntityAsCCPointCloud();
return cloud->hasColors();
}
int BaseFilter::hasSelectedScalarField()
{
if (isFirstSelectedCcPointCloud() != 1)
return -1;
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
return (cloud->hasScalarFields() ? 1 : 0);
}
std::vector<std::string> BaseFilter::getSelectedAvailableScalarFields()
{
std::vector<std::string> field_names;
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return field_names;
unsigned n_fields = cloud->getNumberOfScalarFields();
field_names.reserve(n_fields);
for (unsigned i = 0; i < n_fields; i++)
field_names.push_back(cloud->getScalarFieldName(i));
return field_names;
}
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 NormalEstimation::openDialog()
{
if (!m_dialog)
{
m_dialog = new NormalEstimationDialog;
//initially these are invisible
m_dialog->surfaceComboBox->setVisible(false);
m_dialog->searchSurfaceCheckBox->setVisible(false);
}
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (cloud)
{
ccBBox bBox = cloud->getBB(true,false);
if (bBox.isValid())
m_dialog->radiusDoubleSpinBox->setValue(bBox.getDiagNorm() * 0.005);
}
return m_dialog->exec() ? 1 : 0;
}
int ExtractSIFT::compute()
{
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
PCLCloud::Ptr sm_cloud (new PCLCloud);
std::vector<std::string> req_fields;
req_fields.resize(2);
req_fields[0] = "xyz"; // always needed
switch (m_mode)
{
case RGB:
req_fields[1] = "rgb";
break;
case SCALAR_FIELD:
req_fields[1] = m_field_to_use;
break;
}
cc2smReader converter;
converter.setInputCloud(cloud);
converter.getAsSM(req_fields, *sm_cloud);
//Now change the name of the field to use to a standard name, only if in OTHER_FIELD mode
if (m_mode == SCALAR_FIELD)
{
int field_index = pcl::getFieldIndex(*sm_cloud, m_field_to_use_no_space);
sm_cloud->fields.at(field_index).name = "intensity"; //we always use intensity as name... even if it is curvature or another field.
}
//initialize all possible clouds
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_i (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//Now do the actual computation
if (m_mode == SCALAR_FIELD)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_i);
estimateSIFT<pcl::PointXYZI, pcl::PointXYZ>(cloud_i, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
else if (m_mode == RGB)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_rgb);
estimateSIFT<pcl::PointXYZRGB, pcl::PointXYZ>(cloud_rgb, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
PCLCloud::Ptr out_cloud_sm (new PCLCloud);
TO_PCL_CLOUD(*out_cloud, *out_cloud_sm);
if ( (out_cloud_sm->height * out_cloud_sm->width) == 0)
{
//cloud is empty
return -53;
}
ccPointCloud* out_cloud_cc = sm2ccConverter(out_cloud_sm).getCCloud();
if (!out_cloud_cc)
{
//conversion failed (not enough memory?)
return -1;
}
std::stringstream name;
if (m_mode == RGB)
name << "SIFT Keypoints_" << m_nr_octaves << "_" << "rgb" << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
else
name << "SIFT Keypoints_" << m_nr_octaves << "_" << m_field_to_use_no_space << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
out_cloud_cc->setName(name.str().c_str());
out_cloud_cc->setDisplay(cloud->getDisplay());
if (cloud->getParent())
cloud->getParent()->addChild(out_cloud_cc);
emit newEntity(out_cloud_cc);
return 1;
}
int SavePCD::compute()
{
ccPointCloud * cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
//search for a sensor as child
size_t n_childs = cloud->getChildrenNumber();
ccSensor * sensor(0);
for (size_t i = 0; i < n_childs; ++i)
{
ccHObject * child = cloud->getChild(i);
//try to cast to a ccSensor
if (!child->isKindOf(CC_TYPES::SENSOR))
continue;
sensor = ccHObjectCaster::ToSensor(child);
}
PCLCloud::Ptr out_cloud (new PCLCloud);
cc2smReader* converter = new cc2smReader();
converter->setInputCloud(cloud);
int result = converter->getAsSM(*out_cloud);
delete converter;
converter=0;
Eigen::Vector4f pos;
Eigen::Quaternionf ori;
if(!sensor)
{
//we append to the cloud null sensor informations
pos = Eigen::Vector4f::Zero ();
ori = Eigen::Quaternionf::Identity ();
}
else
{
//we get out valid sensor informations
ccGLMatrix mat = sensor->getRigidTransformation();
CCVector3 trans = mat.getTranslationAsVec3D();
pos(0) = trans[0];
pos(1) = trans[1];
pos(2) = trans[2];
//also the rotation
Eigen::Matrix3f eigrot;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
eigrot(i,j) = mat.getColumn(j)[i];
// now translate to a quaternion notation
ori = Eigen::Quaternionf(eigrot);
}
if (result != 1)
{
return -31;
}
if (pcl::io::savePCDFile( m_filename.toStdString(), *out_cloud, pos, ori, true) < 0)
{
return -32;
}
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;
}
int ExtractSIFT::compute()
{
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
std::list<std::string> req_fields;
try
{
req_fields.push_back("xyz"); // always needed
switch (m_mode)
{
case RGB:
req_fields.push_back("rgb");
break;
case SCALAR_FIELD:
req_fields.push_back(qPrintable(m_field_to_use)); //DGM: warning, toStdString doesn't preserve "local" characters
break;
default:
assert(false);
break;
}
}
catch (const std::bad_alloc&)
{
//not enough memory
return -1;
}
PCLCloud::Ptr sm_cloud = cc2smReader(cloud).getAsSM(req_fields);
if (!sm_cloud)
return -1;
//Now change the name of the field to use to a standard name, only if in OTHER_FIELD mode
if (m_mode == SCALAR_FIELD)
{
int field_index = pcl::getFieldIndex(*sm_cloud, m_field_to_use_no_space);
sm_cloud->fields.at(field_index).name = "intensity"; //we always use intensity as name... even if it is curvature or another field.
}
//initialize all possible clouds
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_i (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//Now do the actual computation
if (m_mode == SCALAR_FIELD)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_i);
estimateSIFT<pcl::PointXYZI, pcl::PointXYZ>(cloud_i, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
else if (m_mode == RGB)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_rgb);
estimateSIFT<pcl::PointXYZRGB, pcl::PointXYZ>(cloud_rgb, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
PCLCloud::Ptr out_cloud_sm (new PCLCloud);
TO_PCL_CLOUD(*out_cloud, *out_cloud_sm);
if ( out_cloud_sm->height * out_cloud_sm->width == 0)
{
//cloud is empty
return -53;
}
ccPointCloud* out_cloud_cc = sm2ccConverter(out_cloud_sm).getCloud();
if (!out_cloud_cc)
{
//conversion failed (not enough memory?)
return -1;
}
std::stringstream name;
if (m_mode == RGB)
name << "SIFT Keypoints_" << m_nr_octaves << "_" << "rgb" << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
else
name << "SIFT Keypoints_" << m_nr_octaves << "_" << m_field_to_use_no_space << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
out_cloud_cc->setName(name.str().c_str());
out_cloud_cc->setDisplay(cloud->getDisplay());
//copy global shift & scale
out_cloud_cc->setGlobalScale(cloud->getGlobalScale());
out_cloud_cc->setGlobalShift(cloud->getGlobalShift());
if (cloud->getParent())
cloud->getParent()->addChild(out_cloud_cc);
emit newEntity(out_cloud_cc);
return 1;
}
