void callback(const sensor_msgs::PointCloud2::ConstPtr& pc) { PointCloudT::Ptr cloud = boost::make_shared<PointCloudT>(); pcl::fromROSMsg(*pc, *cloud); size_t height = 8; // 8 layers size_t width = round((start_angle_ - end_angle_) / angular_resolution_) + 1; PointT invalid; invalid.x = invalid.y = invalid.z = std::numeric_limits<float>::quiet_NaN(); PointCloudT::Ptr cloud_out = boost::make_shared<PointCloudT>(width, height, invalid); cloud_out->is_dense = false; for (size_t i = 0; i < cloud->size(); i++) { const PointT &p = cloud->points[i]; int col = round((atan2(p.y, p.x) - end_angle_) / angular_resolution_); int row = p.layer; if (col < 0 || col >= width || row < 0 || row >= height) { ROS_ERROR("Invalid coordinates: (%d, %d) is outside (0, 0)..(%zu, %zu)!", col, row, width, height); continue; } (*cloud_out)[row * width + col] = p; } sensor_msgs::PointCloud2::Ptr msg = boost::make_shared<sensor_msgs::PointCloud2>(); pcl::toROSMsg(*cloud_out, *msg); msg->header = pc->header; pub.publish(msg); }
/* collects a cloud by aggregating k successive frames */ void waitForCloudK(int k){ ros::Rate r(30); cloud_aggregated->clear(); int counter = 0; while (ros::ok()){ ros::spinOnce(); r.sleep(); if (new_cloud_available_flag){ *cloud_aggregated+=*cloud; new_cloud_available_flag = false; counter ++; if (counter >= k){ cloud_aggregated->header = cloud->header; break; } } } }
void move_to_frame(const PointCloudT::Ptr &input, const string &target_frame, PointCloudT::Ptr &output) { ROS_INFO("Transforming Input Point Cloud to %s frame...", target_frame.c_str()); ROS_INFO(" Input Cloud Size: %zu", input->size()); if (input->header.frame_id == target_frame) { output = input; return; } while (ros::ok()) { tf::StampedTransform stamped_transform; try { // Look up transform tf_listener->lookupTransform(target_frame, input->header.frame_id, ros::Time(0), stamped_transform); // Apply transform pcl_ros::transformPointCloud(*input, *output, stamped_transform); // Store Header Details output->header.frame_id = target_frame; pcl_conversions::toPCL(ros::Time::now(), output->header.stamp); break; } //keep trying until we get the transform catch (tf::TransformException &ex) { ROS_ERROR_THROTTLE(1, "%s", ex.what()); ROS_WARN_THROTTLE(1, " Waiting for transform from cloud frame (%s) to %s frame. Trying again", input->header.frame_id.c_str(), target_frame.c_str()); continue; } } }
void trk3dFeatures::trkSeq(const PointCloudT::Ptr &cloud2, const PointCloudT::Ptr &keyPts2, const float params[], PointCloudT::Ptr &keyPts, pcl::PointCloud<SHOT1344>::Ptr &Shot1344Cloud_1, std::vector<u16> &matchIdx1, std::vector<u16> &matchIdx2, std::vector<f32> &matchDist) { // construct descriptors pcl::PointCloud<SHOT1344>::Ptr Shot1344Cloud_2(new pcl::PointCloud<SHOT1344>); extractFeatures featureDetector; Shot1344Cloud_2 = featureDetector.Shot1344Descriptor(cloud2, keyPts2, params); // // match keypoints // featureDetector.crossMatching(Shot1344Cloud_1, Shot1344Cloud_2, // &matchIdx1, &matchIdx2, &matchDist); // find the matching from desc_1 -> desc_2 std::cout<<"size of Shot1344Cloud_1 in trkSeq: "<<Shot1344Cloud_1->points.size()<<std::endl; std::cout<<"size of Shot1344Cloud_2 in trkSeq: "<<Shot1344Cloud_2->points.size()<<std::endl; featureDetector.matchKeyPts(Shot1344Cloud_1, Shot1344Cloud_2, &matchIdx2, &matchDist); std::cout<<"size of matches in trkSeq: "<<matchIdx2.size()<<std::endl; std::cout<<"size of matchDist in trkSeq: "<<matchDist.size()<<std::endl; Shot1344Cloud_1.reset(new pcl::PointCloud<SHOT1344>); keyPts->points.clear(); for(size_t i=0; i<matchIdx2.size();i++) { keyPts->push_back(keyPts2->points.at(matchIdx2[i])); Shot1344Cloud_1->push_back(Shot1344Cloud_2->points.at(matchIdx2[i])); } }
double calculate_density(const PointCloudT::Ptr &cloud, const bwi_perception::BoundingBox &box) { // TODO: Calculate true volume // If the cloud is one point thick in some dimension, we'll assign that dimension a magnitude of 1cm double x_dim = max(abs(box.max[0] - box.min[0]), 0.01f); double y_dim = max(abs(box.max[1] - box.min[1]), 0.01f); double z_dim = max(abs(box.max[2] - box.min[2]), 0.01f); double volume = x_dim * y_dim * z_dim; return (double) cloud->size() / volume; }
void removeLowConfidencePoints(cv::Mat& confidence_image, int threshold, PointCloudT::Ptr& cloud) { for (int i=0;i<cloud->height;i++) { for (int j=0;j<cloud->width;j++) { if (confidence_image.at<unsigned char>(i,j) < threshold) { cloud->at(j,i).x = std::numeric_limits<float>::quiet_NaN(); cloud->at(j,i).y = std::numeric_limits<float>::quiet_NaN(); cloud->at(j,i).z = std::numeric_limits<float>::quiet_NaN(); confidence_image.at<unsigned char>(i,j) = 0; // just for visualization } else confidence_image.at<unsigned char>(i,j) = 255; // just for visualization } } cloud->is_dense = false; }
void computeNeonVoxels(PointCloudT::Ptr in, PointCloudT::Ptr green, PointCloudT::Ptr orange) { green->clear(); orange->clear(); //Point Cloud to store out neon cap //PointCloudT::Ptr temp_neon_cloud (new PointCloudT); for (int i = 0; i < in->points.size(); i++) { unsigned int r, g, b; r = in->points[i].r; g = in->points[i].g; b = in->points[i].b; // Look for mostly neon value points if (g > 175 && (r + b) < 150) { green->push_back(in->points[i]); } else if(r > 200 && (g + b) < 150){ orange->push_back(in->points[i]); } } }
void PlayWindow::ShowPC(PointCloudT::Ptr PC) { updateModelMutex.lock(); if (ui->checkBox_ShowPC->isChecked()) { if (!viewer->updatePointCloud(PC)) viewer->addPointCloud(PC); Eigen::Matrix4f currentPose = Eigen::Matrix4f::Identity(); currentPose.block(0,0,3,3) = PC->sensor_orientation_.matrix(); currentPose.block(0,3,3,1) = PC->sensor_origin_.head(3); viewer->updatePointCloudPose("cloud",Eigen::Affine3f(currentPose) ); } ui->qvtkWidget->update (); updateModelMutex.unlock(); // Timing times.push_back(QDateTime::currentDateTime().toMSecsSinceEpoch() - PC->header.stamp); double mean; if (times.size() > 60) { mean = std::accumulate(times.begin(), times.end(), 0.0) / times.size(); times.erase(times.begin()); } if (ui->checkBox_SavePC->isChecked()) pcl::io::savePCDFileASCII(PC->header.frame_id, *PC); std::vector<int> ind; pcl::removeNaNFromPointCloud(*PC, *PC, ind); ui->label_nPoint->setText(QString("Nb Point : %1").arg(PC->size())); ui->label_time->setText(QString("Time (ms) : %1").arg(mean)); ui->label_fps->setText(QString("FPS (Hz) : %1").arg(1000/mean)); return; }
int main(int argc, char** argv) { if (argc != 3) { printUsage(argv); return -1; } std::string filename_in = argv[1]; std::string filename_out = argv[2]; // read in printf("Reading cloud\n"); PointCloudT::Ptr cloud; cloud.reset(new rgbdtools::PointCloudT()); pcl::PCDReader reader; reader.read(filename_in, *cloud); alignGlobalCloud(cloud); return 0; }
void PC_to_Mat(PointCloudT::Ptr &cloud, cv::Mat &result){ if (cloud->isOrganized()) { std::cout << "PointCloud is organized..." << std::endl; result = cv::Mat(cloud->height, cloud->width, CV_8UC3); if (!cloud->empty()) { for (int h=0; h<result.rows; h++) { for (int w=0; w<result.cols; w++) { PointT point = cloud->at(w, h); Eigen::Vector3i rgb = point.getRGBVector3i(); result.at<cv::Vec3b>(h,w)[0] = rgb[2]; result.at<cv::Vec3b>(h,w)[1] = rgb[1]; result.at<cv::Vec3b>(h,w)[2] = rgb[0]; } } } } }
void filter_PC_from_BB(PointCloudT::Ptr &cloud, cv::Mat &result, int x, int y, int width, int height){ const float bad_point = std::numeric_limits<float>::quiet_NaN(); if (cloud->isOrganized()) { std::cout << "PointCloud is organized..." << std::endl; result = cv::Mat(cloud->height, cloud->width, CV_8UC3); if (!cloud->empty()) { for (int h=0; h<result.rows; h++) { for (int w=0; w<result.cols; w++) { // Check if in bounding window if ( (h>y && h<(y+height)) && ((w > x) && w < (x+width)) ){ // do nothing } else { // remove point //PointT point = cloud->at(w, h); //cloud->at(w, h); cloud->at(w, h).x = bad_point; cloud->at(w, h).y = bad_point; cloud->at(w, h).z = bad_point; cloud->at(w, h).r = bad_point; cloud->at(w, h).g = bad_point; cloud->at(w, h).b = bad_point; } } } } } }
// Align a rigid object to a scene with clutter and occlusions int main (int argc, char **argv) { // Point clouds PointCloudT::Ptr object (new PointCloudT); PointCloudT::Ptr object_aligned (new PointCloudT); PointCloudT::Ptr scene (new PointCloudT); FeatureCloudT::Ptr object_features (new FeatureCloudT); FeatureCloudT::Ptr scene_features (new FeatureCloudT); // Get input object and scene if (argc != 3) { pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]); return (1); } // Load object and scene pcl::console::print_highlight ("Loading point clouds...\n"); if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 || pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0) { pcl::console::print_error ("Error loading object/scene file!\n"); return (1); } // Downsample pcl::console::print_highlight ("Downsampling...\n"); pcl::VoxelGrid<PointNT> grid; const float leaf = 0.005f; grid.setLeafSize (leaf, leaf, leaf); grid.setInputCloud (object); grid.filter (*object); grid.setInputCloud (scene); grid.filter (*scene); // Estimate normals for scene pcl::console::print_highlight ("Estimating scene normals...\n"); pcl::NormalEstimationOMP<PointNT,PointNT> nest; nest.setRadiusSearch (0.01); nest.setInputCloud (scene); nest.compute (*scene); // Estimate features pcl::console::print_highlight ("Estimating features...\n"); FeatureEstimationT fest; fest.setRadiusSearch (0.025); fest.setInputCloud (object); fest.setInputNormals (object); fest.compute (*object_features); fest.setInputCloud (scene); fest.setInputNormals (scene); fest.compute (*scene_features); // Perform alignment pcl::console::print_highlight ("Starting alignment...\n"); pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align; align.setInputSource (object); align.setSourceFeatures (object_features); align.setInputTarget (scene); align.setTargetFeatures (scene_features); align.setMaximumIterations (100000); // Number of RANSAC iterations align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose align.setCorrespondenceRandomness (5); // Number of nearest features to use align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis { pcl::ScopeTime t("Alignment"); align.align (*object_aligned); } if (align.hasConverged ()) { // Print results printf ("\n"); Eigen::Matrix4f transformation = align.getFinalTransformation (); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2)); pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2)); pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2)); pcl::console::print_info ("\n"); pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3)); pcl::console::print_info ("\n"); pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ()); // Show alignment pcl::visualization::PCLVisualizer visu("Alignment"); visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene"); visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned"); visu.spin (); } else { pcl::console::print_error ("Alignment failed!\n"); return (1); } return (0); }
int main (int argc, char** argv) { PointCloudT::Ptr cloud (new PointCloudT); PointCloudT::Ptr new_cloud (new PointCloudT); bool new_cloud_available_flag = false; //pcl::Grabber* grab = new pcl::OpenNIGrabber (); PointCloudT::Ptr ddd; boost::function<void (const PointCloudT::ConstPtr&)> f = boost::bind(&grabberCallback, _1, cloud, &new_cloud_available_flag); grab->registerCallback (f); viewer->registerKeyboardCallback(keyboardEventOccurred); grab->start (); bool first_time = true; FILE* objects; objects = fopen ("objects.txt","a"); while(!new_cloud_available_flag) boost::this_thread::sleep(boost::posix_time::milliseconds(1)); new_cloud_available_flag=false; //////////////////// // invert correction //////////////////// Eigen::Matrix4f transMat = Eigen::Matrix4f::Identity(); transMat (1,1) = -1; //////////////////// // pass filter //////////////////// PointCloudT::Ptr passed_cloud; pcl::PassThrough<PointT> pass; passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT); //////////////////// // voxel grid //////////////////// PointCloudT::Ptr voxelized_cloud; voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT); pcl::VoxelGrid<PointT> vg; vg.setLeafSize (0.001, 0.001, 0.001); //////////////////// // sac segmentation //////////////////// PointCloudT::Ptr cloud_f; PointCloudT::Ptr cloud_plane; PointCloudT::Ptr cloud_filtered; cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT); cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT); cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT); pcl::SACSegmentation<PointT> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); //////////////////// // euclidean clustering //////////////////// std::vector<pcl::PointIndices> cluster_indices; std::vector<PointCloudT::Ptr> extracted_clusters; pcl::search::KdTree<PointT>::Ptr eucl_tree (new pcl::search::KdTree<PointT>); pcl::EuclideanClusterExtraction<PointT> ec; ec.setClusterTolerance (0.04); ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (eucl_tree); PointCloudT::Ptr cloud_cluster; //////////////////// // vfh estimate //////////////////// pcl::NormalEstimation<PointT, pcl::Normal> ne; pcl::search::KdTree<PointT>::Ptr vfh_tree1 (new pcl::search::KdTree<PointT> ()); pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh; pcl::search::KdTree<PointT>::Ptr vfh_tree2 (new pcl::search::KdTree<PointT> ()); std::vector<pcl::PointCloud<pcl::VFHSignature308>::Ptr> computed_vfhs; ne.setSearchMethod (vfh_tree1); ne.setRadiusSearch (0.05); vfh.setSearchMethod (vfh_tree2); vfh.setRadiusSearch (0.05); pcl::PointCloud<pcl::Normal>::Ptr normals; pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs; //////////////////// // nearest neighbour //////////////////// int k = 6; std::string kdtree_idx_file_name = "kdtree.idx"; std::string training_data_h5_file_name = "training_data.h5"; std::string training_data_list_file_name = "training_data.list"; // std::vector<vfh_model> models; // flann::Matrix<float> data; // loadFileList (models, training_data_list_file_name); // flann::load_from_file (data, // training_data_h5_file_name, // "training_data"); // // flann::Index<flann::ChiSquareDistance<float> > index (data, // flann::SavedIndexParams // ("kdtree.idx")); //////////////////// // process nearest neighbour //////////////////// std::vector<hypothesis> final_hypothesis; final_hypothesis.clear(); double last = pcl::getTime(); while (! viewer->wasStopped()) { if (new_cloud_available_flag) { new_cloud_available_flag = false; double now = pcl::getTime(); //////////////////// // pass filter //////////////////// //passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT); //////////////////// // voxel grid //////////////////// //voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT); //////////////////// // sac segmentation //////////////////// //cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT); //cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT); //cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT); //////////////////// // euclidean clustering //////////////////// extracted_clusters.clear(); cluster_indices.clear(); //////////////////// // vfh estimate //////////////////// computed_vfhs.clear(); //////////////////// // nearest neighbour //////////////////// cloud_mutex.lock(); //displayCloud(viewer,cloud); boost::thread displayCloud_(displayCloud,viewer,cloud); if(now-last > 13 || first_time) { first_time = false; last=now; //////////////////// // invert correction //////////////////// pcl::transformPointCloud(*cloud,*new_cloud,transMat); //////////////////// // pass filter //////////////////// pass.setInputCloud (new_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (-0.5, 0.5); //pass.setFilterLimitsNegative (true); pass.filter (*passed_cloud); //////////////////// // voxel grid //////////////////// vg.setInputCloud (passed_cloud); vg.filter (*voxelized_cloud); //////////////////// // sac segmentation //////////////////// *cloud_filtered = *voxelized_cloud; int i=0, nr_points = (int) voxelized_cloud->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Couldnt estimate a planar model for the dataset.\n"; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<PointT> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); // Get the points associated with the planar surface extract.filter (*cloud_plane); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); *cloud_filtered = *cloud_f; } //////////////////// // euclidean clustering //////////////////// // Creating the KdTree object for the search method of the extraction eucl_tree->setInputCloud (cloud_filtered); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices); for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { //PointCloudT::Ptr cloud_cluster (new PointCloudT); cloud_cluster = boost::shared_ptr<PointCloudT>(new PointCloudT); for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) cloud_cluster->points.push_back (cloud_filtered->points [*pit]); cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; extracted_clusters.push_back(cloud_cluster); } cloud_cluster.reset(); //////////////////// // vfh estimate //////////////////// for (int z=0; z<extracted_clusters.size(); ++z) { vfhs = boost::shared_ptr<pcl::PointCloud<pcl::VFHSignature308> > (new pcl::PointCloud<pcl::VFHSignature308>); normals = boost::shared_ptr<pcl::PointCloud<pcl::Normal> > (new pcl::PointCloud<pcl::Normal>); ne.setInputCloud (extracted_clusters.at(z)); ne.compute (*normals); vfh.setInputCloud (extracted_clusters.at(z)); vfh.setInputNormals (normals); vfh.compute (*vfhs); computed_vfhs.push_back(vfhs); } vfhs.reset(); normals.reset(); //////////////////// // nearest neighbour //////////////////// std::vector<vfh_model> models; flann::Matrix<int> k_indices; flann::Matrix<float> k_distances; flann::Matrix<float> data; loadFileList (models, training_data_list_file_name); flann::load_from_file (data, training_data_h5_file_name, "training_data"); flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("kdtree.idx")); for(int z=0; z<computed_vfhs.size(); ++z) { vfh_model histogram; histogram.second.resize(308); for (size_t i = 0; i < 308; ++i) { histogram.second[i] = computed_vfhs.at(z)->points[0].histogram[i]; } index.buildIndex (); nearestKSearch (index, histogram, k, k_indices, k_distances); hypothesis x; x.distance = k_distances[0][0]; size_t index = models.at(k_indices[0][0]).first.find("_v",5); x.object_name = models.at(k_indices[0][0]).first.substr(5,index-5); ddd = boost::shared_ptr<PointCloudT>(new PointCloudT); pcl::transformPointCloud(*extracted_clusters.at(z),*ddd,transMat); x.cluster = ddd; ddd.reset(); std::string filename =""; filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1); filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd"; x.cluster_name = filename.c_str(); final_hypothesis.push_back(x); x.cluster.reset(); //delete x; // std::string filename =""; // filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1); // filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd"; // const char* filen = filename.c_str(); // fprintf(objects,"%s",filen); // fprintf(objects,"::"); // fprintf(objects,models.at (k_indices[0][0]).first.c_str()); // fprintf(objects,"::"); // fprintf(objects,"%f",k_distances[0][0]); // fprintf(objects,"\n"); } delete[] k_indices.ptr (); delete[] k_distances.ptr (); delete[] data.ptr (); std::cout << final_hypothesis.size() << std::endl; viewer->removeAllShapes(); for(int z=0; z<final_hypothesis.size();++z) { if(final_hypothesis.at(z).distance < 100) { fprintf(objects,"%s",final_hypothesis.at(z).cluster_name.c_str()); fprintf(objects,"::"); fprintf(objects,"%s",final_hypothesis.at(z).object_name.c_str()); fprintf(objects,"::"); fprintf(objects,"%f",final_hypothesis.at(z).distance); fprintf(objects,"\n"); std::stringstream ddd; ddd << final_hypothesis.at(z).object_name; ddd << "\n" << "("; ddd << final_hypothesis.at(z).distance; ddd << ")"; viewer->addText3D(ddd.str().c_str(), final_hypothesis.at(z).cluster->points[0], 0.02,1,1,1, boost::lexical_cast<std::string>(z)); drawBoundingBox(final_hypothesis.at(z).cluster,viewer,z); } } //boost::thread allBoxes_(allBoxes,viewer,final_hypothesis); //allBoxes_.join(); viewer->spinOnce(); final_hypothesis.clear(); j++; } // for(int z=0; z<extracted_clusters.size(); ++z) // { // //viewer->addPointCloud<PointT>(extracted_clusters.at(z), // // boost::lexical_cast<std::string>(z)); // // std::string filename =""; // filename += "inputcloud_" + boost::lexical_cast<std::string>(j); // filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd"; // pcl::io::savePCDFile(filename,*extracted_clusters.at(z),false); // } // for(int z=0; z<computed_vfhs.size(); ++z) // { // //viewer->addPointCloud<PointT>(extracted_clusters.at(z), // // boost::lexical_cast<std::string>(z)); // // std::string filename =""; // filename += "inputcloud_" + boost::lexical_cast<std::string>(j); // filename += "_" + boost::lexical_cast<std::string>(z) + "_vfhs.pcd"; // pcl::io::savePCDFileASCII<pcl::VFHSignature308> (filename, *computed_vfhs.at(z)); // } //viewer->removeAllShapes(); // viewer->removeAllPointClouds(); // viewer->setCameraPosition(0, 0, 0, 0, 0, 1, 0, -1, 0); // pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud); // viewer->addPointCloud<PointT>(cloud,rgb,"input_cloud"); // viewer->spinOnce(); //std::cout << final_hypothesis.at(0).cluster->points[0]; //boost::this_thread::sleep(boost::posix_time::milliseconds(10)); displayCloud_.join(); cloud_mutex.unlock(); } } grab->stop(); return 0; }
int main (int argc, char ** argv) { if (argc < 2) { pcl::console::print_info ("Syntax is: %s {-p <pcd-file> OR -r <rgb-file> -d <depth-file>} \n --NT (disables use of single camera transform) \n -o <output-file> \n -O <refined-output-file> \n-l <output-label-file> \n -L <refined-output-label-file> \n-v <voxel resolution> \n-s <seed resolution> \n-c <color weight> \n-z <spatial weight> \n-n <normal_weight>] \n", argv[0]); return (1); } /////////////////////////////// ////////////////////////////// ////////////////////////////// ////////////////////////////// ////// THIS IS ALL JUST INPUT HANDLING - Scroll down until ////// pcl::SupervoxelClustering<pcl::PointXYZRGB> super ////////////////////////////// ////////////////////////////// std::string rgb_path; bool rgb_file_specified = pcl::console::find_switch (argc, argv, "-r"); if (rgb_file_specified) pcl::console::parse (argc, argv, "-r", rgb_path); std::string depth_path; bool depth_file_specified = pcl::console::find_switch (argc, argv, "-d"); if (depth_file_specified) pcl::console::parse (argc, argv, "-d", depth_path); PointCloudT::Ptr cloud = boost::make_shared < PointCloudT >(); NormalCloudT::Ptr input_normals = boost::make_shared < NormalCloudT > (); bool pcd_file_specified = pcl::console::find_switch (argc, argv, "-p"); std::string pcd_path; if (!depth_file_specified || !rgb_file_specified) { std::cout << "Using point cloud\n"; if (!pcd_file_specified) { std::cout << "No cloud specified!\n"; return (1); }else { pcl::console::parse (argc,argv,"-p",pcd_path); } } bool disable_transform = pcl::console::find_switch (argc, argv, "--NT"); bool ignore_provided_normals = pcl::console::find_switch (argc, argv, "--nonormals"); bool has_normals = false; std::string out_path = "test_output.png";; pcl::console::parse (argc, argv, "-o", out_path); std::string out_label_path = "test_output_labels.png"; pcl::console::parse (argc, argv, "-l", out_label_path); std::string refined_out_path = "refined_test_output.png"; pcl::console::parse (argc, argv, "-O", refined_out_path); std::string refined_out_label_path = "refined_test_output_labels.png";; pcl::console::parse (argc, argv, "-L", refined_out_label_path); float voxel_resolution = 0.008f; pcl::console::parse (argc, argv, "-v", voxel_resolution); float seed_resolution = 0.08f; pcl::console::parse (argc, argv, "-s", seed_resolution); float color_importance = 0.2f; pcl::console::parse (argc, argv, "-c", color_importance); float spatial_importance = 0.4f; pcl::console::parse (argc, argv, "-z", spatial_importance); float normal_importance = 1.0f; pcl::console::parse (argc, argv, "-n", normal_importance); if (!pcd_file_specified) { //Read the images vtkSmartPointer<vtkImageReader2Factory> reader_factory = vtkSmartPointer<vtkImageReader2Factory>::New (); vtkImageReader2* rgb_reader = reader_factory->CreateImageReader2 (rgb_path.c_str ()); //qDebug () << "RGB File="<< QString::fromStdString(rgb_path); if ( ! rgb_reader->CanReadFile (rgb_path.c_str ())) { std::cout << "Cannot read rgb image file!"; return (1); } rgb_reader->SetFileName (rgb_path.c_str ()); rgb_reader->Update (); //qDebug () << "Depth File="<<QString::fromStdString(depth_path); vtkImageReader2* depth_reader = reader_factory->CreateImageReader2 (depth_path.c_str ()); if ( ! depth_reader->CanReadFile (depth_path.c_str ())) { std::cout << "Cannot read depth image file!"; return (1); } depth_reader->SetFileName (depth_path.c_str ()); depth_reader->Update (); vtkSmartPointer<vtkImageFlip> flipXFilter = vtkSmartPointer<vtkImageFlip>::New(); flipXFilter->SetFilteredAxis(0); // flip x axis flipXFilter->SetInputConnection(rgb_reader->GetOutputPort()); flipXFilter->Update(); vtkSmartPointer<vtkImageFlip> flipXFilter2 = vtkSmartPointer<vtkImageFlip>::New(); flipXFilter2->SetFilteredAxis(0); // flip x axis flipXFilter2->SetInputConnection(depth_reader->GetOutputPort()); flipXFilter2->Update(); vtkSmartPointer<vtkImageData> rgb_image = flipXFilter->GetOutput (); int *rgb_dims = rgb_image->GetDimensions (); vtkSmartPointer<vtkImageData> depth_image = flipXFilter2->GetOutput (); int *depth_dims = depth_image->GetDimensions (); if (rgb_dims[0] != depth_dims[0] || rgb_dims[1] != depth_dims[1]) { std::cout << "Depth and RGB dimensions to not match!"; std::cout << "RGB Image is of size "<<rgb_dims[0] << " by "<<rgb_dims[1]; std::cout << "Depth Image is of size "<<depth_dims[0] << " by "<<depth_dims[1]; return (1); } cloud->points.reserve (depth_dims[0] * depth_dims[1]); cloud->width = depth_dims[0]; cloud->height = depth_dims[1]; cloud->is_dense = false; // Fill in image data int centerX = static_cast<int>(cloud->width / 2.0); int centerY = static_cast<int>(cloud->height / 2.0); unsigned short* depth_pixel; unsigned char* color_pixel; float scale = 1.0f/1000.0f; float focal_length = 525.0f; float fl_const = 1.0f / focal_length; depth_pixel = static_cast<unsigned short*>(depth_image->GetScalarPointer (depth_dims[0]-1,depth_dims[1]-1,0)); color_pixel = static_cast<unsigned char*> (rgb_image->GetScalarPointer (depth_dims[0]-1,depth_dims[1]-1,0)); for (size_t y=0; y<cloud->height; ++y) { for (size_t x=0; x<cloud->width; ++x, --depth_pixel, color_pixel-=3) { PointT new_point; // uint8_t* p_i = &(cloud_blob->data[y * cloud_blob->row_step + x * cloud_blob->point_step]); float depth = static_cast<float>(*depth_pixel) * scale; if (depth == 0.0f) { new_point.x = new_point.y = new_point.z = std::numeric_limits<float>::quiet_NaN (); } else { new_point.x = (static_cast<float> (x) - centerX) * depth * fl_const; new_point.y = (static_cast<float> (centerY) - y) * depth * fl_const; // vtk seems to start at the bottom left image corner new_point.z = depth; } uint32_t rgb = static_cast<uint32_t>(color_pixel[0]) << 16 | static_cast<uint32_t>(color_pixel[1]) << 8 | static_cast<uint32_t>(color_pixel[2]); new_point.rgb = *reinterpret_cast<float*> (&rgb); cloud->points.push_back (new_point); } } } else { /// check if the provided pcd file contains normals pcl::PCLPointCloud2 input_pointcloud2; if (pcl::io::loadPCDFile (pcd_path, input_pointcloud2)) { PCL_ERROR ("ERROR: Could not read input point cloud %s.\n", pcd_path.c_str ()); return (3); } pcl::fromPCLPointCloud2 (input_pointcloud2, *cloud); if (!ignore_provided_normals) { if (hasField (input_pointcloud2,"normal_x")) { std::cout << "Using normals contained in file. Set --nonormals option to disable this.\n"; pcl::fromPCLPointCloud2 (input_pointcloud2, *input_normals); has_normals = true; } } } std::cout << "Done making cloud!\n"; /////////////////////////////// ////////////////////////////// ////////////////////////////// ////////////////////////////// ////// This is how to use supervoxels ////////////////////////////// ////////////////////////////// ////////////////////////////// ////////////////////////////// // If we're using the single camera transform no negative z allowed since we use log(z) if (!disable_transform) { for (PointCloudT::iterator cloud_itr = cloud->begin (); cloud_itr != cloud->end (); ++cloud_itr) if (cloud_itr->z < 0) { PCL_ERROR ("Points found with negative Z values, this is not compatible with the single camera transform!\n"); PCL_ERROR ("Set the --NT option to disable the single camera transform!\n"); return 1; } std::cout <<"You have the single camera transform enabled - this should be used with point clouds captured from a single camera.\n"; std::cout <<"You can disable the transform with the --NT flag\n"; } pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution,!disable_transform); super.setInputCloud (cloud); if (has_normals) super.setNormalCloud (input_normals); super.setColorImportance (color_importance); super.setSpatialImportance (spatial_importance); super.setNormalImportance (normal_importance); std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters; std::cout << "Extracting supervoxels!\n"; super.extract (supervoxel_clusters); std::cout << "Found " << supervoxel_clusters.size () << " Supervoxels!\n"; PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud (); PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud (); PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters); PointLCloudT::Ptr full_labeled_cloud = super.getLabeledCloud (); std::cout << "Getting supervoxel adjacency\n"; std::multimap<uint32_t, uint32_t> label_adjacency; super.getSupervoxelAdjacency (label_adjacency); std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > refined_supervoxel_clusters; std::cout << "Refining supervoxels \n"; super.refineSupervoxels (3, refined_supervoxel_clusters); PointLCloudT::Ptr refined_labeled_voxel_cloud = super.getLabeledVoxelCloud (); PointNCloudT::Ptr refined_sv_normal_cloud = super.makeSupervoxelNormalCloud (refined_supervoxel_clusters); PointLCloudT::Ptr refined_full_labeled_cloud = super.getLabeledCloud (); // THESE ONLY MAKE SENSE FOR ORGANIZED CLOUDS if (cloud->isOrganized ()) { pcl::io::savePNGFile (out_label_path, *full_labeled_cloud, "label"); pcl::io::savePNGFile (refined_out_label_path, *refined_full_labeled_cloud, "label"); //Save RGB from labels pcl::io::PointCloudImageExtractorFromLabelField<PointLT> pcie (pcie.io::PointCloudImageExtractorFromLabelField<PointLT>::COLORS_RGB_GLASBEY); //We need to set this to account for NAN points in the organized cloud pcie.setPaintNaNsWithBlack (true); pcl::PCLImage image; pcie.extract (*full_labeled_cloud, image); pcl::io::savePNGFile (out_path, image); pcie.extract (*refined_full_labeled_cloud, image); pcl::io::savePNGFile (refined_out_path, image); } std::cout << "Constructing Boost Graph Library Adjacency List...\n"; typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float> VoxelAdjacencyList; typedef VoxelAdjacencyList::vertex_descriptor VoxelID; typedef VoxelAdjacencyList::edge_descriptor EdgeID; VoxelAdjacencyList supervoxel_adjacency_list; super.getSupervoxelAdjacencyList (supervoxel_adjacency_list); std::cout << "Loading visualization...\n"; boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0, 0, 0); viewer->registerKeyboardCallback(keyboard_callback, 0); bool refined_normal_shown = show_refined; bool refined_sv_normal_shown = show_refined; bool sv_added = false; bool normals_added = false; bool graph_added = false; std::vector<std::string> poly_names; std::cout << "Loading viewer...\n"; while (!viewer->wasStopped ()) { if (show_supervoxels) { if (!viewer->updatePointCloud ((show_refined)?refined_labeled_voxel_cloud:labeled_voxel_cloud, "colored voxels")) { viewer->addPointCloud ((show_refined)?refined_labeled_voxel_cloud:labeled_voxel_cloud, "colored voxels"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3.0, "colored voxels"); } } else { viewer->removePointCloud ("colored voxels"); } if (show_voxel_centroids) { if (!viewer->updatePointCloud (voxel_centroid_cloud, "voxel centroids")) { viewer->addPointCloud (voxel_centroid_cloud, "voxel centroids"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids"); } } else { viewer->removePointCloud ("voxel centroids"); } if (show_supervoxel_normals) { if (refined_sv_normal_shown != show_refined || !sv_added) { viewer->removePointCloud ("supervoxel_normals"); viewer->addPointCloudNormals<PointNormal> ((show_refined)?refined_sv_normal_cloud:sv_normal_cloud,1,0.05f, "supervoxel_normals"); sv_added = true; } refined_sv_normal_shown = show_refined; } else if (!show_supervoxel_normals) { viewer->removePointCloud ("supervoxel_normals"); } if (show_normals) { std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr>::iterator sv_itr,sv_itr_end; sv_itr = ((show_refined)?refined_supervoxel_clusters.begin ():supervoxel_clusters.begin ()); sv_itr_end = ((show_refined)?refined_supervoxel_clusters.end ():supervoxel_clusters.end ()); for (; sv_itr != sv_itr_end; ++sv_itr) { std::stringstream ss; ss << sv_itr->first <<"_normal"; if (refined_normal_shown != show_refined || !normals_added) { viewer->removePointCloud (ss.str ()); viewer->addPointCloudNormals<PointT,Normal> ((sv_itr->second)->voxels_,(sv_itr->second)->normals_,10,0.02f,ss.str ()); // std::cout << (sv_itr->second)->normals_->points[0]<<"\n"; } } normals_added = true; refined_normal_shown = show_refined; } else if (!show_normals) { std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr>::iterator sv_itr,sv_itr_end; sv_itr = ((show_refined)?refined_supervoxel_clusters.begin ():supervoxel_clusters.begin ()); sv_itr_end = ((show_refined)?refined_supervoxel_clusters.end ():supervoxel_clusters.end ()); for (; sv_itr != sv_itr_end; ++sv_itr) { std::stringstream ss; ss << sv_itr->first << "_normal"; viewer->removePointCloud (ss.str ()); } } if (show_graph && !graph_added) { poly_names.clear (); std::multimap<uint32_t,uint32_t>::iterator label_itr = label_adjacency.begin (); for ( ; label_itr != label_adjacency.end (); ) { //First get the label uint32_t supervoxel_label = label_itr->first; //Now get the supervoxel corresponding to the label pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (supervoxel_label); //Now we need to iterate through the adjacent supervoxels and make a point cloud of them PointCloudT adjacent_supervoxel_centers; std::multimap<uint32_t,uint32_t>::iterator adjacent_itr = label_adjacency.equal_range (supervoxel_label).first; for ( ; adjacent_itr!=label_adjacency.equal_range (supervoxel_label).second; ++adjacent_itr) { pcl::Supervoxel<PointT>::Ptr neighbor_supervoxel = supervoxel_clusters.at (adjacent_itr->second); adjacent_supervoxel_centers.push_back (neighbor_supervoxel->centroid_); } //Now we make a name for this polygon std::stringstream ss; ss << "supervoxel_" << supervoxel_label; poly_names.push_back (ss.str ()); addSupervoxelConnectionsToViewer (supervoxel->centroid_, adjacent_supervoxel_centers, ss.str (), viewer); //Move iterator forward to next label label_itr = label_adjacency.upper_bound (supervoxel_label); } graph_added = true; } else if (!show_graph && graph_added) { for (std::vector<std::string>::iterator name_itr = poly_names.begin (); name_itr != poly_names.end (); ++name_itr) { viewer->removeShape (*name_itr); } graph_added = false; } if (show_help) { viewer->removeShape ("help_text"); printText (viewer); } else { removeText (viewer); if (!viewer->updateText("Press h to show help", 5, 10, 12, 1.0, 1.0, 1.0,"help_text") ) viewer->addText("Press h to show help", 5, 10, 12, 1.0, 1.0, 1.0,"help_text"); } viewer->spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } return (0); }
int main (int argc, char** argv) { ros::init(argc, argv, "cv_proj"); //std::string input_file = "/home/igor/pcds/assembly_objs/ardrone_02_indoor.pcd"; //std::string input_file = "/home/igor/pcds/assembly_objs/ardrone_03_outdoor.pcd"; /* std::string input_file = "/home/igor/pcds/cluttered/3_objs_ardrone_indoor.pcd"; std::string output_file = "/home/igor/pcds/cv_proj_out/out_cluttered_indoor_01.pcd"; std::string template_file = "/home/igor/pcds/templates/indoor_template.pcd"; std::string out_rgb = "/home/igor/pcds/cv_proj_out/out_result_05.jpg"; */ std::string input_file, out_pcd, template_file, out_rgb, out_transf_pcd; ros::param::param<std::string>("/cv_proj/input_file", input_file, "/home/igor/pcds/cluttered/3_objs_ardrone_indoor.pcd"); //ros::param::param<std::string>("/cv_proj/out_pcd", out_pcd, "/home/igor/pcds/cv_proj_out/out_cluttered_indoor_01.pcd"); ros::param::param<std::string>("/cv_proj/template_file", template_file, "/home/igor/pcds/templates/indoor_template.pcd"); //ros::param::param<std::string>("/cv_proj/out_rgb", out_rgb, "/home/igor/pcds/cv_proj_out/out_result_05.jpg"); boost::filesystem::path filepath(input_file); boost::filesystem::path filename = filepath.filename(); filename = filename.stem(); // Get rid of the extension boost::filesystem::path dir = filepath.parent_path(); std::string opencv_out_ext = "_filtered.png"; std::string pcl_out_ext = "_filtered.pcd"; std::string output_folder = "/output_cv_proj/"; std::string output_stem; output_stem = dir.string() + output_folder + filename.string(); out_rgb = output_stem + opencv_out_ext; out_pcd = output_stem + pcl_out_ext; out_transf_pcd = output_stem + "_templ" + pcl_out_ext; std::cout << out_rgb << std::endl; std::cout << out_pcd << std::endl; // Read in the cloud data pcl::PCDReader reader; PointCloudT::Ptr cloud (new PointCloudT), cloud_f (new PointCloudT); reader.read (input_file, *cloud); std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //* if (cloud->isOrganized()) { std::cout << "-- PointCloud cloud is organized" << std::endl; std::cout << "-- PointCloud cloud width: " << cloud->width << std::endl; std::cout << "-- PointCloud cloud height: " << cloud->height << std::endl; } /* // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<PointT> vg; PointCloudT::Ptr cloud_filtered (new PointCloudT); vg.setInputCloud (cloud); //vg.setLeafSize (0.01f, 0.01f, 0.01f); vg.setLeafSize (0.001f, 0.001f, 0.001f); // vg.filter (*cloud_filtered); std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //* */ /**/ PointCloudT::Ptr cloud_filtered (new PointCloudT); *cloud_filtered = *cloud; // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<PointT> seg; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); PointCloudT::Ptr cloud_plane (new PointCloudT ()); pcl::PCDWriter writer; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.02); int i=0, nr_points = (int) cloud_filtered->points.size (); while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the planar inliers from the input cloud pcl::ExtractIndices<PointT> extract; extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); //extract.setUserFilterValue(999); extract.setKeepOrganized(true); // Get the points associated with the planar surface extract.filter (*cloud_plane); //extract.filterDirectly(cloud_plane); std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl; // To keep point cloud organized //extract.setKeepOrganized(true); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_f); //extract.filterDirectly(cloud_f); *cloud_filtered = *cloud_f; } cv::Mat result; PC_to_Mat(cloud_filtered,result); imwrite( out_rgb, result ); int rgb_x, rgb_y, rgb_width, rgb_height; rgb_x = 240; rgb_y = 150; rgb_width = 200; rgb_height = 200; filter_PC_from_BB ( cloud_filtered, result, rgb_x, rgb_y, rgb_width, rgb_height); // For templates /* // Box filter pcl::PassThrough<PointT> pass_x; pass_x.setInputCloud (cloud_filtered); pass_x.setFilterFieldName ("x"); pass_x.setFilterLimits (-0.2, 0.4); //pass.setFilterLimitsNegative (true); pass_x.filter (*cloud_filtered); pcl::PassThrough<PointT> pass_y; pass_y.setInputCloud (cloud_filtered); pass_y.setFilterFieldName ("y"); pass_y.setFilterLimits (-0.2, 0.4); //pass.setFilterLimitsNegative (true); pass_y.filter (*cloud_filtered); */ // Save file pcl::io::savePCDFileASCII (out_pcd, *cloud_filtered); std::cerr << "Saved " << (*cloud_filtered).points.size () << " data points to PCD file." << std::endl; // Read in template file PointCloudT::Ptr cloud_template (new PointCloudT); reader.read (template_file, *cloud_template); pcl::VoxelGrid<PointT> vg_in; PointCloudT::Ptr cloud_filtered_vg (new PointCloudT); vg_in.setInputCloud (cloud_filtered); vg_in.setLeafSize (0.001f, 0.001f, 0.001f); //vg.setLeafSize (0.01f, 0.01f, 0.01f); vg_in.filter (*cloud_filtered_vg); pcl::VoxelGrid<PointT> vg_temp; PointCloudT::Ptr cloud_template_vg (new PointCloudT); vg_temp.setInputCloud (cloud_template); vg_temp.setLeafSize (0.001f, 0.001f, 0.001f); //vg.setLeafSize (0.01f, 0.01f, 0.01f); vg_temp.filter (*cloud_template_vg); // ICP PointCloudT::Ptr cloud_in (new PointCloudT), cloud_out (new PointCloudT); *cloud_in = *cloud_template_vg; *cloud_out = *cloud_filtered_vg; pcl::IterativeClosestPoint<PointT, PointT> icp; icp.setInputCloud(cloud_in); icp.setInputTarget(cloud_out); PointCloudT Final; icp.align(Final); std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl; Eigen::Matrix4f transform = icp.getFinalTransformation (); std::cout << transform << std::endl; // Transform template to original frame PointCloudT::Ptr cloud_out2 (new PointCloudT); pcl::transformPointCloud(*cloud_in,*cloud_out2,transform); pcl::io::savePCDFileASCII (out_transf_pcd, *cloud_out2); std::cerr << "Saved " << (*cloud_out2).points.size () << " data points to PCD file." << std::endl; return (0); }
// Align a rigid object to a scene with clutter and occlusions int main (int argc, char **argv) { // Point clouds PointCloudT::Ptr object (new PointCloudT); PointCloudT::Ptr scene (new PointCloudT); PointCloudT::Ptr object_aligned (new PointCloudT); // Get input object and scene if (argc < 3) { pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]); return (1); } pcl::console::parse_argument (argc, argv, "--max_iterations", in_max_iterations); pcl::console::parse_argument (argc, argv, "--num_samples", in_num_samples); pcl::console::parse_argument (argc, argv, "--s_thresh", in_similarity_thresh); pcl::console::parse_argument (argc, argv, "--max_cdist", in_max_corresp_dist); pcl::console::parse_argument (argc, argv, "--inlier_frac", in_inlier_frac); pcl::console::parse_argument (argc, argv, "--leaf", in_leaf); pcl::console::parse_argument (argc, argv, "--normal_radius", normal_radius); pcl::console::parse_argument (argc, argv, "--feature_radius", feature_radius); pcl::console::parse_argument (argc, argv, "--icp", in_icp); pcl::console::parse_argument (argc, argv, "--max_corr_icp", max_corr_icp); pcl::console::parse_argument (argc, argv, "--icp_eps", max_eps_icp); // Load object and scene pcl::console::print_highlight ("Loading point clouds...\n"); pcl_tools::cloud_from_stl(argv[2], *object); if (pcl::io::loadPCDFile<PointNT> (argv[1], *scene) < 0) { pcl::console::print_error ("Error loading object/scene file!\n"); return (1); } std::vector<int> indices; pcl::removeNaNFromPointCloud(*scene, *scene, indices); pcl::removeNaNFromPointCloud(*object, *object, indices); // /*pcl_tools::icp_result align = */alp_align(object, scene, object_aligned, 50000, 3, 0.9f, 5.5f * leaf, 0.7f); // /*pcl_tools::icp_result align = */alp_align(object_aligned, scene, object_aligned, 50000, 3, 0.9f, 7.5f * leaf, 0.4f); std::cout << "Inlier frac " << in_inlier_frac << std::endl; pcl_tools::icp_result align = alp_align(object, scene, object_aligned, in_max_iterations, in_num_samples, in_similarity_thresh, in_max_corresp_dist, in_inlier_frac, in_leaf); pcl::visualization::PCLVisualizer visu("Alignment"); if (align.converged) { pcl::console::print_info ("Inliers: %i/%i, scene: %i\n", align.inliers, object->size (), scene->size ()); // Show alignment visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object"); visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene"); visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned"); // visu.addPointCloudNormals<PointNT>(object); visu.spin (); } else { pcl::console::print_error ("Alignment failed!\n"); return (1); } if (in_icp) { pcl::console::print_highlight ("Applying ICP now\n"); pcl::IterativeClosestPointNonLinear<PointNT, PointNT> icp; // pcl::IterativeClosestPoint<PointNT, PointNT> icp; pcl_tools::affine_cloud(Eigen::Vector3f::UnitZ(), 0.0, Eigen::Vector3f(0.0, 0.0, 0.02), *object_aligned, *object_aligned); icp.setMaximumIterations (100); icp.setMaxCorrespondenceDistance(max_corr_icp); icp.setTransformationEpsilon (max_eps_icp); icp.setInputSource (object_aligned); icp.setInputTarget (scene); icp.align (*object_aligned); if (icp.hasConverged()) { pcl::console::print_highlight ("ICP: Converged with fitness %f\n", icp.getFitnessScore()); } // pcl::visualization::PCLVisualizer visu("Alignment"); // visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object"); // visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene"); visu.updatePointCloud (object_aligned, ColorHandlerT (object_aligned, 100.0, 50.0, 200.0), "object_aligned"); // visu.addPointCloudNormals<PointNT>(object); visu.spin (); } return (0); }