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;
}
Esempio n. 9
0
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];
        }
      }
    }
  }
}
Esempio n. 11
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;
}
Esempio n. 14
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);
}
Esempio n. 15
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);
}
Esempio n. 16
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);
}