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; }