C++ PointCloud::swap方法代码示例

void CloudPlaneFiltrationPerp(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
	pcl::PointCloud<pcl::PointXYZ>::Ptr &planed_cloud,
	float distancethreshold,
	float zepsangle,
	bool negative){

	pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ExtractIndices<pcl::PointXYZ> extract;

	//Finding plane segments and filtering positive and negative delta
	pcl::SACSegmentation<pcl::PointXYZ> plane_seg;
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

	plane_seg.setOptimizeCoefficients(true);
	//plane_seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);// (pcl::SACMODEL_PLANE);
	plane_seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
	
	plane_seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
	plane_seg.setMethodType(pcl::SAC_RANSAC);
	plane_seg.setMaxIterations(1000);
	plane_seg.setDistanceThreshold(distancethreshold);
	plane_seg.setAxis(Eigen::Vector3f(0, 0, 1));
	plane_seg.setEpsAngle(zepsangle);
	plane_seg.setInputCloud(cloud);
	plane_seg.segment(*inliers, *coefficients);

	extract.setInputCloud(cloud);
	extract.setIndices(inliers);
	extract.setNegative(negative);
	extract.filter(*plane_cloud_tmp);
	planed_cloud.swap(plane_cloud_tmp);
}

void pointCloudCallback(const sensor_msgs::PointCloud2& scan_msg) {
	ROS_INFO("Received a new PointCloud2 message");

	DynamicJoinPclConfig dynjoinpcl_config = dynjoinpcl.getConfig();

	std::vector<Quad> quads;

	pcl::PointCloud<pcl::PointXYZ> scan_pcl;
	pcl::PointCloud<pcl::PointXYZRGBNormal> map_new_pcl;
	pcl::PointXYZ laser_center;

	map_new_pcl.header = map_pcl.header;

	convpcl.transform(scan_msg, scan_pcl);
	convpcl.getFrameOrigin(dynjoinpcl_config.laser_frame, laser_center);

	dynjoinpcl.joinPCL(scan_pcl, map_pcl, map_new_pcl, laser_center, quads);
	map_pcl.swap(map_new_pcl);

	sensor_msgs::PointCloud2 map_msg_out;
	pcl::toROSMsg(map_pcl, map_msg_out);

	pcl_pub.publish(map_msg_out);

	visualization_msgs::MarkerArray quads_marker_array;
	makeQuadsMarkerArray(quads, quads_marker_array);
	quads_marker_array_pub.publish(quads_marker_array);
}

void VoxelGridFiltration(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &voxeled_cloud, float voxeldensity){
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::VoxelGrid<pcl::PointXYZ> vg;
	vg.setInputCloud(cloud);
	vg.setLeafSize(voxeldensity, voxeldensity, voxeldensity); //FIXME. Need to create function with flexeble values for leaf cloud size (Cloud, Count point per dimentions -> BoundingBox -> dimentions -> setLeafSize -> LightCloud)
	vg.filter(*filtered_cloud);
 	voxeled_cloud.swap(filtered_cloud);
}

void CloudPlaneFiltration(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
	pcl::PointCloud<pcl::PointXYZ>::Ptr &planed_cloud,
	float clastertollerance, int minclastersize, int maxclastersize, float cloud_zstep){

	pcl::PointCloud<pcl::PointXYZ>::Ptr result_cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
	
	float minz = FLT_MAX;
	float maxz = -FLT_MAX;
	float cloud_height = 0;
	float cloud_step = cloud_zstep;

	//Get min z coordinate
	//Get max z coordinate
	for (int i = 0; i < cloud->size(); i++){
		if (minz > cloud->points[i].z)
			minz = cloud->points[i].z;
		if (maxz < cloud->points[i].z)
			maxz = cloud->points[i].z;
	}
	
	//Get height of cloud with oZ axiz
	cloud_height = std::abs(maxz - minz);

	for (float istep = minz; istep < maxz; istep += cloud_step){
		vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> step_clasters;
		pcl::PointCloud<pcl::PointXYZ>::Ptr step_cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointIndices::Ptr step_inliers(new pcl::PointIndices);
		pcl::ExtractIndices<pcl::PointXYZ> extract;
		
		//std::cout << istep << std::endl; //DEBUG
		for (int i = 0; i < cloud->size(); i++){
			if (cloud->points[i].z >= istep && cloud->points[i].z < istep + cloud_step){
				step_inliers->indices.push_back(i);
				//plane_cloud_tmp->push_back(cloud->points[i].z);
			}
		}

		extract.setInputCloud(cloud);
		extract.setIndices(step_inliers);
		extract.setNegative(false);
		extract.filter(*step_cloud_tmp);

		//Find clasters in step cloud
		if (step_cloud_tmp->points.size() >= 0){
			FindClasters(step_cloud_tmp, step_clasters, clastertollerance, minclastersize, maxclastersize);

			if (step_clasters.size() > 0){
				for (int k = 0; k < step_clasters.size(); k++){
					for (int p = 0; p < step_clasters[k]->points.size(); p++){
						result_cloud_tmp->points.push_back(step_clasters[k]->points[p]);
					}
				}
			}
		}
	}
	
	planed_cloud.swap(result_cloud_tmp);
}

bool LaserSensorProcessor::cleanPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud)
{
  pcl::PointCloud<pcl::PointXYZRGB> tempPointCloud;
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*pointCloud, tempPointCloud, indices);
  tempPointCloud.is_dense = true;
  pointCloud->swap(tempPointCloud);
  ROS_DEBUG("ElevationMap: cleanPointCloud() reduced point cloud to %i points.", static_cast<int>(pointCloud->size()));
  return true;
}

void CloudNoizeFiltration(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &filtered_cloud){
	// Create the filtering object
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud);

	sor.setMeanK(10);
	sor.setStddevMulThresh(1.0);
	sor.setNegative(false);
	sor.filter(*filtered_cloud_tmp);
	filtered_cloud.swap(filtered_cloud_tmp);
}

bool StereoSensorProcessor::cleanPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud)
{
  pcl::PointCloud<pcl::PointXYZRGB> tempPointCloud;

  originalWidth_ = pointCloud->width;
  pcl::removeNaNFromPointCloud(*pointCloud, tempPointCloud, indices_);
  tempPointCloud.is_dense = true;
  pointCloud->swap(tempPointCloud);

  ROS_DEBUG("ElevationMap: cleanPointCloud() reduced point cloud to %i points.", static_cast<int>(pointCloud->size()));
  return true;
}

    pcl::PointCloud<PointT>::Ptr removeWallsCloud(pcl::PointCloud<PointT>::Ptr cloud_seg)
    {
        pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT>);
        pcl::ModelCoefficients::Ptr coeff (new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

        pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
        pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
        pcl::NormalEstimation<PointT, pcl::Normal> ne;

        pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
        pcl::ExtractIndices<PointT> extract;

        // Estimate point normals
        ne.setSearchMethod (tree);
        ne.setKSearch (50);

        seg.setOptimizeCoefficients (true);
        seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
        seg.setMethodType (pcl::SAC_RANSAC);
        seg.setDistanceThreshold (seg_distance);
        seg.setNormalDistanceWeight (normal_distance_weight);
        seg.setMaxIterations (1000);

        int i = 0, nr_points = (int) cloud_seg->points.size ();
        // While 20% of the original cloud is still there
        while (cloud_seg->points.size () > seg_percentage * nr_points && i < 10 && cloud_seg->points.size() > 0)
        {
            //seg.setInputCloud (cloud);
            ne.setInputCloud (cloud_seg);
            ne.compute (*cloud_normals);
            //seg.setInputCloud (cloud);
            seg.setInputCloud (cloud_seg);
            seg.setInputNormals (cloud_normals);
            seg.segment (*inliers, *coeff);
            if (inliers->indices.size () == 0)
            {
                break;
            }
            if(inliers->indices.size() < nr_points/20 || inliers->indices.size() < 10){
                i++;
                continue;
            }
            // Extract the planar inliers from the input cloud
            extract.setInputCloud (cloud_seg);
            extract.setIndices (inliers);
            extract.setNegative (true);
            extract.filter (*cloud_plane);
            cloud_seg.swap (cloud_plane);
            i++;
        }
        return cloud_seg;
    }

bool KinectSensorProcessor::cleanPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud)
{
	pcl::PassThrough<pcl::PointXYZRGB> passThroughFilter;
	pcl::PointCloud<pcl::PointXYZRGB> tempPointCloud;

	passThroughFilter.setInputCloud(pointCloud);
	passThroughFilter.setFilterFieldName("z");
	passThroughFilter.setFilterLimits(sensorParameters_.at("cutoff_min_depth"), sensorParameters_.at("cutoff_max_depth"));
	// This makes the point cloud also dense (no NaN points).
	passThroughFilter.filter(tempPointCloud);
	tempPointCloud.is_dense = true;
	pointCloud->swap(tempPointCloud);

	ROS_DEBUG("cleanPointCloud() reduced point cloud to %i points.", static_cast<int>(pointCloud->size()));
	return true;
}

void pointCloudCallback(const sensor_msgs::PointCloud2& scan_msg)
{
	ROS_INFO("Received a new PointCloud2 message");

	DynamicJoinPclConfig dynjoinpcl_config = dynjoinpcl.getConfig();
	NormalEstimationPclConfig normal_config = normal.getConfig();

	// downsample
	pcl::PointCloud<pcl::PointXYZ> scan_pcl;
	pcl::PointCloud<pcl::PointXYZRGBNormal> scan_norm_pcl;
	convpcl.transform(scan_msg, scan_pcl);
	scan_norm_pcl.header = scan_pcl.header;
	pcl::VoxelGrid<pcl::PointXYZ> sor;
	sor.setInputCloud(scan_pcl.makeShared());
	sor.setLeafSize(dynjoinpcl_config.leaf_size, dynjoinpcl_config.leaf_size, dynjoinpcl_config.leaf_size);
	sor.filter(scan_pcl);
	pcl::copyPointCloud(scan_pcl, scan_norm_pcl);

	// normal estimation
	pcl::KdTreeFLANN<pcl::PointXYZRGBNormal> scan_norm_kdtree;
	scan_norm_kdtree.setInputCloud(scan_norm_pcl.makeShared());
	tf::Transform t;
	pcl::PointXYZ laser_center;
	convpcl.getLastTransform(t);
	convpcl.getFrameOrigin(dynjoinpcl_config.laser_frame, laser_center);
	normal.computeNormals(scan_norm_pcl, scan_norm_kdtree, laser_center);

	// dynamic join

	pcl::PointCloud<pcl::PointXYZRGBNormal> map_new_pcl;
	map_new_pcl.header = map_pcl.header;
	dynjoinpcl.joinPCL(scan_norm_pcl, map_pcl, map_new_pcl, laser_center);
	map_pcl.swap(map_new_pcl);
	//colorNormalsPCL(map_pcl);
	visualizeNormals(map_pcl);
	sensor_msgs::PointCloud2 map_msg_out;
	pcl::toROSMsg(map_pcl, map_msg_out);
	pcl_pub.publish(map_msg_out);
}

/*
* Find the ground in the environment.
* Params[in/out]: the cloud, the coeff of the planes, the indices, the ground cloud, the hull cloud
*/
void Segmentation::removeWall(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
{
    pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr indices(new pcl::PointIndices);

    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
    // Optional
    seg.setOptimizeCoefficients (true);

    // Mandatory
    seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
    seg.setMethodType (pcl::SAC_RANSAC);

    seg.setDistanceThreshold (0.023); //0.25

    seg.setAxis(Eigen::Vector3f(0,-std::sqrt(2)/2,std::sqrt(2)/2)); // Axis Y 0, -1, 0
    seg.setEpsAngle(30.0f * (M_PI/180.0f)); // 50 degrees of error

    pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
    seg.setInputCloud (cloud);
    seg.segment (*indices, *coeff);

    if (indices->indices.size () == 0)
    {
        PCL_ERROR ("Could not estimate a planar model (Ground).");
    }
    else
    {
        extract.setInputCloud(cloud);
        extract.setIndices(indices);
        extract.setNegative(true);
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
        extract.filter(*cloud_f);
        cloud.swap(cloud_f);
    }

}

/*
* Find the ground in the environment.
* Params[in/out]: the cloud, the coeff of the planes, the indices, the ground cloud, the hull cloud
*/
bool Segmentation::findGround(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud, MainPlane &mp)
{
    pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr indices(new pcl::PointIndices);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ground(new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr hullGround(new pcl::PointCloud<pcl::PointXYZRGBA>);

    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
    // Optional
    seg.setOptimizeCoefficients (true);

    // Mandatory
    seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
    seg.setMethodType (pcl::SAC_RANSAC);

    seg.setDistanceThreshold (0.030); //0.25 / 35

    seg.setAxis(_axis); // Axis Y 0, -1, 0
    seg.setEpsAngle(30.0f * (M_PI/180.0f)); // 50 degrees of error

    pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
    seg.setInputCloud (cloud);
    seg.segment (*indices, *coeff);
    mp.setCoefficients(coeff);
    //mp.setIndices(indices);

    if (indices->indices.size () == 0)
    {
        PCL_ERROR ("Could not estimate a planar model (Ground).");
        return false;
    }
    else
    {
        extract.setInputCloud(cloud);
        extract.setIndices(indices);
        extract.setNegative(false);
        extract.filter(*ground);
        mp.setPlaneCloud(ground);

        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZRGBA>);
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGBA>);
        // Copy the points of the plane to a new cloud.
        pcl::ExtractIndices<pcl::PointXYZRGBA> extractHull;
        extractHull.setInputCloud(cloud);
        extractHull.setIndices(indices);
        extractHull.filter(*plane);


        pcl::ConcaveHull<pcl::PointXYZRGBA> chull;
        chull.setInputCloud (plane);
        chull.setAlpha (0.1);
        chull.reconstruct (*concaveHull);

        mp.setHull(concaveHull);
        //vectorCloudinliers.push_back(convexHull);
        extract.setNegative(true);
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
        extract.filter(*cloud_f);
        cloud.swap(cloud_f);


        return true;
    }

}

/*
* Find the other planes in the environment.
* Params[in/out]: the cloud, the normal of the ground, the coeff of the planes, the planes, the hull
* return the number of inliers
*/
int  Segmentation::findOtherPlanesRansac(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud,Eigen::Vector3f axis, std::vector <pcl::ModelCoefficients> &coeffPlanes, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &vectorCloudInliers, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &vectorHull )
{
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
    // Optional
    seg.setOptimizeCoefficients (true);

    vectorCloudInliers.clear();
    coeffPlanes.clear();
    vectorHull.clear();

    // Mandatory
    seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);

    const int nb_points = cloud->points.size();
    pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
    seg.setMaxIterations(5);
    seg.setDistanceThreshold (0.020); //0.15
    seg.setAxis(axis);
    //std::cout<< "axis are  a : " << axis[0] << " b : " << axis[1] << " c ; " << axis[2] << std::endl;

    seg.setEpsAngle(20.0f * (M_PI/180.0f));
    while (cloud->points.size() > _coeffRansac * nb_points)
    {
        // std::cout << "the number is " << cloud->points.size() << std::endl;
        seg.setInputCloud (cloud);
        seg.segment (*inliers, *coefficients);

        if (inliers->indices.size () == 0)
        {
            //PCL_ERROR ("Could not estimate a planar model for the given dataset.");
            break;
        }
        else
        {
            coeffPlanes.push_back(*coefficients);
            //vectorInliers.push_back(*inliers);
            extract.setInputCloud(cloud);
            extract.setIndices(inliers);
            extract.setNegative(false);

            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_p (new pcl::PointCloud<pcl::PointXYZRGBA>);
            extract.filter(*cloud_p);

            //Eigen::Vector4f centroid;
            //pcl::compute3DCentroid(*cloud_p,*inliers,centroid);
            // passthroughFilter(centroid[0] - 2, centroid[0] + 2, centroid[1] - 2, centroid[1] + 2, centroid[2] - 2, centroid[2] + 2, cloud_p, cloud_p);


            //statisticalRemovalOutliers(cloud_p);
            //statisticalRemovalOutliers(cloud_p); -0.00485527 b : -0.895779 c ; -0.444474
            //if((std::abs(coefficients->values[0]) < (0.1 ))  && ( std::abs(coefficients->values[1]) > (0.89)) && (std::abs(coefficients->values[2]) > (0.40)))
            //{
            //  std::cout<< "coeff are  a : " << coefficients->values[0] << " b : " << coefficients->values[1] << " c ; " << coefficients->values[2] << std::endl;

            vectorCloudInliers.push_back(cloud_p);
            //}

            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZRGBA>);
            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGBA>);
            // Copy the points of the plane to a new cloud.
            pcl::ExtractIndices<pcl::PointXYZRGBA> extractHull;
            extractHull.setInputCloud(cloud);
            extractHull.setIndices(inliers);
            extractHull.filter(*plane);

            // Object for retrieving the convex hull.
            //                pcl::ConvexHull<pcl::PointXYZRGBA> hull;
            //                hull.setInputCloud(plane);
            //                hull.reconstruct(*convexHull);

            pcl::ConcaveHull<pcl::PointXYZRGBA> chull;
            //            chull.setInputCloud (plane);
            //            chull.setAlpha (0.1);
            //            chull.reconstruct (*concaveHull);
            //            vectorHull.push_back(concaveHull);

            //vectorCloudinliers.push_back(convexHull);
            extract.setNegative(true);
            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
            extract.filter(*cloud_f);
            cloud.swap(cloud_f);
            //  std::cout << "the number is " << cloud->points.size() << std::endl;
        }

    }

    return vectorCloudInliers.size();
}