int main(int argc, char** argv) { Options opts; if (options(argc, argv, opts)) return 1; // Load stuff PCL_INFO("Loading src_pd: %s\n", opts.src_pcd_file.c_str()); PointCloud::Ptr src_cloud(new PointCloud()); if (pcl::io::loadPCDFile(opts.src_pcd_file, *src_cloud) < 0) { std::cout << "Error loading input point cloud " << opts.src_pcd_file << std::endl; return -1; } // Do downsampling PointCloud::Ptr out_cloud(new PointCloud()); pcl::VoxelGrid<PointT> voxel_grid; voxel_grid.setInputCloud(src_cloud); // FIXME Should be an argument voxel_grid.setLeafSize(opts.leaf_size, opts.leaf_size, opts.leaf_size); voxel_grid.filter(*out_cloud); // Save downsampled cloud pcl::io::savePCDFileBinary(opts.out_pcd_file, *out_cloud); PCL_INFO("Downsampled from %d to %d points\n", src_cloud->size(), out_cloud->size()); return 0; }
void Segmentation::computeHeight() { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr gr(new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZRGBA>); gr = _ground.getPlaneCloud(); pcl::transformPointCloud(*gr, *out_cloud, transformXY); Eigen::Vector4f centroid; pcl::compute3DCentroid (*gr, centroid); for(size_t i = 0; i < _planes.size();i++) { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane_out(new pcl::PointCloud<pcl::PointXYZRGBA>); plane = _planes[i].getPlaneCloud(); pcl::transformPointCloud(*plane, *plane_out, transformXY); Eigen::Vector4f centroidPlane; pcl::compute3DCentroid (*plane, centroidPlane); Eigen::Vector4f height = centroidPlane - centroid; _planes[i].setHeight(height); Eigen::Vector4f height_out = _planes[i].getHeight(); _planes[i].setCentroid(centroidPlane); //std::cout << "centroid:" << centroidPlane[0] << " " << centroidPlane[1] << " " << centroidPlane[2] << " " <<centroidPlane[3] << " \n"; //std::cout << "height:" << height[0] << " " << height[1] << " " << height[2] << " " <<height[3] << " \n"; // pcl::PointXYZ c; } }
PCLCloud::Ptr loadSensorMessage(const QString &filename, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation) { PCLCloud::Ptr out_cloud(new PCLCloud); //Load the given file if (pcl::io::loadPCDFile(filename.toStdString(), *out_cloud, origin, orientation) < 0) { //loading failed out_cloud.reset(); } return out_cloud; }
int main(int argc, char** argv) { Options opts; if (options(argc, argv, opts)) return 1; // Load stuff PCL_INFO("Loading src_pd: %s\n", opts.src_pcd_file.c_str()); pcl::PointCloud<pcl::PointXYZI>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZI>()); if (pcl::io::loadPCDFile(opts.src_pcd_file, *src_cloud) < 0) { std::cout << "Error loading input point cloud " << opts.src_pcd_file << std::endl; return -1; } // Do normal estimation PointCloudWithNormals::Ptr out_cloud(new PointCloudWithNormals); pcl::NormalEstimation<pcl::PointXYZI, pcl::PointXYZINormal> norm_est; pcl::search::KdTree<pcl::PointXYZI>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZI>()); norm_est.setSearchMethod(tree); norm_est.setKSearch(opts.k_search); norm_est.setInputCloud(src_cloud); // Only outputs the normals norm_est.compute (*out_cloud); // This copies over the XYZ coordinates pcl::copyPointCloud (*src_cloud, *out_cloud); // Save cloud with normals pcl::io::savePCDFileBinary(opts.out_pcd_file, *out_cloud); return 0; }
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."); }
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."); }
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 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; }
int main(int argc, char** argv) { Options opts; if (options(argc, argv, opts)) return 1; // Load stuff Eigen::MatrixXf ldr_data; readLDRFile(opts.src_ldr_file, ldr_data); std::cout << "rows: " << ldr_data.rows() << " cols: " << ldr_data.cols() << std::endl; std::cout << ldr_data.block<10, 6>(0, 0) << std::endl; pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>()); ldr_to_cloud(ldr_data, *src_cloud); std::cout << "Cloud size: " << src_cloud->size() << std::endl; /* PCL_INFO("Loading src_pcd: %s\n", opts.src_pcd_file.c_str()); pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>()); if (pcl::io::loadPCDFile(opts.src_pcd_file, *src_cloud) < 0) { std::cout << "Error loading input point cloud " << opts.src_pcd_file << std::endl; return -1; } */ // Do upsampling pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls_upsampling; // Set up KD-tree // TODO Weight intensity pcl::search::KdTree<pcl::PointXYZ>::Ptr tree; // Set parameters // FIXME PARAMS mls_upsampling.setInputCloud(src_cloud); mls_upsampling.setComputeNormals (true); mls_upsampling.setPolynomialFit (true); mls_upsampling.setSearchMethod (tree); mls_upsampling.setSearchRadius (0.25); //mls_upsampling.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::VOXEL_GRID_DILATION); //mls_upsampling.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::SAMPLE_LOCAL_PLANE); mls_upsampling.setUpsamplingMethod (pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal>::NONE); mls_upsampling.setUpsamplingRadius (0.25); mls_upsampling.setUpsamplingStepSize (0.125); //mls_upsampling.setDilationVoxelSize(0.25); //mls_upsampling.setDilationIterations(1); mls_upsampling.setPointDensity(1); NormalCloud::Ptr out_cloud(new NormalCloud()); std::cout << "Running upsampling" << std::endl; mls_upsampling.process(*out_cloud); std::cout << "Finished upsampling" << std::endl; pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>()); pcl::copyPointCloud(*out_cloud, *out_cloud_xyz); // TODO For each point in upsampled cloud, assign it the // time, laser num, intensity of the closet point in // the original cloud // TODO Will have to read in original ldr file to do this //pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2; Eigen::MatrixXf out_matrix(out_cloud_xyz->size(), 6); for (int k = 0; k < out_cloud_xyz->size(); k++) { pcl::PointXYZ pt = out_cloud_xyz->at(k); // FIXME Fill in with intensity, laser out_matrix.row(k) << (float)pt.x, (float)pt.y, (float)pt.z, 0.0f, 0.0f, 0.0f; } // Save upsampled cloud if (opts.out_pcd_file != "") pcl::io::savePCDFileBinary(opts.out_pcd_file, *out_cloud); // Save ldr file if (opts.out_ldr_file != "") writeLDRFile(opts.out_ldr_file, out_matrix); PCL_INFO("Upsampled from %d to %d points\n", src_cloud->size(), out_cloud->size()); return 0; }
int main(int argc, char **argv) { if (argc < 3) { std::cerr << "Needs two pcd input files." << std::endl; return (-1); } pcl::PointCloud<pcl::RGB>::Ptr left_cloud (new pcl::PointCloud<pcl::RGB>); pcl::PointCloud<pcl::RGB>::Ptr right_cloud (new pcl::PointCloud<pcl::RGB>); //Read pcd files pcl::PCDReader pcd; if (pcd.read (argv[1], *left_cloud) == -1) return (-1); if (pcd.read (argv[2], *right_cloud) == -1) return (-1); if ( !left_cloud->isOrganized() || !right_cloud->isOrganized() || left_cloud->width != right_cloud -> width || left_cloud->height != left_cloud->height) { std::cout << "Wrong stereo pair; please check input pcds .." << std::endl; return 0; } pcl::AdaptiveCostSOStereoMatching stereo; stereo.setMaxDisparity(60); stereo.setXOffset(0); stereo.setRadius(5); stereo.setSmoothWeak(20); stereo.setSmoothStrong(100); stereo.setGammaC(25); stereo.setGammaS(10); stereo.setRatioFilter(20); stereo.setPeakFilter(0); stereo.setLeftRightCheck(true); stereo.setLeftRightCheckThreshold(1); stereo.setPreProcessing(true); stereo.compute(*left_cloud, *right_cloud); stereo.medianFilter(4); pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud( new pcl::PointCloud<pcl::PointXYZRGB> ); stereo.getPointCloud(318.112200, 224.334900, 368.534700, 0.8387445, out_cloud, left_cloud); pcl::PointCloud<pcl::RGB>::Ptr vmap( new pcl::PointCloud<pcl::RGB> ); stereo.getVisualMap(vmap); pcl::visualization::ImageViewer iv ("My viewer"); iv.addRGBImage<pcl::RGB> (vmap); //iv.addRGBImage<pcl::RGB> (left_cloud); //iv.spin (); // press 'q' to exit boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0, 0, 0); //viewer->addPointCloud<pcl::PointXYZRGB> (out_cloud, "stereo"); pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> intensity(out_cloud); viewer->addPointCloud<pcl::PointXYZRGB> (out_cloud, intensity, "stereo"); //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "stereo"); viewer->initCameraParameters (); //viewer->spin(); while (!viewer->wasStopped ()) { viewer->spinOnce (100); iv.spinOnce (100); // press 'q' to exit boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } }