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

pcl::PointCloud<pcl::PointXYZRGB> CPlaneExtraction::extractHorizontalSurfaceFromNormals(
		pcl::PointCloud<pcl::PointXYZRGB> &point_cloud, bool surface) {

	Eigen::Vector3f axis = Eigen::Vector3f(1.0, 0, 0); //x

	//ROS_DEBUG("before in %d", (int)point_cloud.points.size ());
	pcl::PointCloud<pcl::PointXYZRGB> cloud_projected;
	pcl::PointIndices inliers;
	pcl::ModelCoefficients coefficients;
	pcl::SACSegmentationFromNormals<pcl::PointXYZRGB, pcl::Normal> seg;
	pcl::PointCloud<pcl::Normal> cloud_normals;
	pcl::PointCloud<pcl::PointNormal> cloud_pointNormals;

	cloud_normals = this->toolBox.estimatingNormals(point_cloud, 10);
	//cloud_pointNormals = this->toolBox.movingLeastSquares(point_cloud,0.005f);

	seg.setOptimizeCoefficients(true);
	// seg.setModelType (pcl::SACMODEL_NORMAL_PLANE + pcl::SACMODEL_PARALLEL_PLANE);
	seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	//seg.setAxis(axis);
	seg.setNormalDistanceWeight(0.1); //0.1
	seg.setMaxIterations(10000); //10000
	seg.setDistanceThreshold(0.1); //0.1 //must be low to get a really restricted horizontal plane
	//seg.setProbability(0.99);

	seg.setInputCloud(point_cloud.makeShared());
	seg.setInputNormals(cloud_normals.makeShared());

	seg.segment(inliers, coefficients);

	if (inliers.indices.size() == 0) {
		ROS_ERROR("[extractHorizontalSurfaceFromNormals] Could not estimate a planar model for the given dataset.");
		return cloud_projected;
	}

	pcl::ProjectInliers<pcl::PointXYZRGB> proj;
	proj.setModelType(pcl::SACMODEL_NORMAL_PLANE);
	proj.setInputCloud(point_cloud.makeShared());
	proj.setModelCoefficients(boost::make_shared<pcl::ModelCoefficients>(
			coefficients));
	proj.filter(cloud_projected);

	//ROS_DEBUG("after in %d", (int)cloud_projected.points.size ());
	//pcl::copyPointCloud(point_cloud,inliers,cloud_projected);
	//point_cloud = cloud_projected;

	return cloud_projected;
}

void mario::removePlane( const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud,  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr & dst, double threshould )
{
	dst = cloud->makeShared();

	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);
	// Mandatory
	seg.setModelType (pcl::SACMODEL_PLANE);
	seg.setMethodType (pcl::SAC_RANSAC);
	seg.setDistanceThreshold (threshould);

	seg.setInputCloud (cloud);
	seg.segment (*inliers, *coefficients);

	//for (size_t i = 0; i < inliers->indices.size (); ++i) {
	//	cloud->points[inliers->indices[i]].r = 255;
	//	cloud->points[inliers->indices[i]].g = 0;
	//	cloud->points[inliers->indices[i]].b = 0;
	//}

	//フィルタリング
	pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
	extract.setInputCloud( cloud );
	extract.setIndices( inliers );

	// true にすると平面を除去、false にすると平面以外を除去
	extract.setNegative( true );
	extract.filter( *dst );
}

pcl::PointCloud<pcl::PointXYZ>::Ptr MovingObjectsIdentificator::removeLargePlanes(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    if(verbose) std::cout << "Removing large planes ... ";
    pcl::PointCloud<pcl::PointXYZ>::Ptr resultCloud (cloud->makeShared());
    pcl::SACSegmentation<pcl::PointXYZ> segmentation;
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);

    segmentation.setOptimizeCoefficients(true);
    segmentation.setModelType(pcl::SACMODEL_PLANE);
    segmentation.setMethodType(pcl::SAC_RANSAC);
    segmentation.setMaxIterations(ransacMaxIterations);
    segmentation.setDistanceThreshold(ransacDistanceThreshold);

    int pointsCount = resultCloud->points.size();
    while(resultCloud->points.size() > 0.3 * pointsCount) {
        segmentation.setInputCloud(resultCloud);
        segmentation.segment(*inliers, *coefficients);

        if(inliers->indices.size() <= largePlaneMinimumSize) {
            break;
        }

        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(resultCloud);
        extract.setIndices(inliers);
        extract.setNegative(true);
        extract.filterDirectly(resultCloud);
    }

    if(verbose) std::cout << "done!" << std::endl;

    return resultCloud;
}

void registration(pcl::PointCloud<pcl::PointXYZRGB>& cloud, pcl::PointCloud<pcl::PointXYZRGB>& model, pcl::PointCloud<pcl::PointXYZRGB>& cloud_out, pcl::PointCloud<pcl::PointXYZRGB>& tmp_rgb, Eigen::Matrix4f& m)
{
    pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
    icp.setInputSource(cloud.makeShared ());
    icp.setInputTarget(model.makeShared ());
    pcl::PointCloud<pcl::PointXYZRGB> Final;
    icp.align(Final);
    m = icp.getFinalTransformation();

    pcl::transformPointCloud (cloud, cloud, m);
    pcl::transformPointCloud (tmp_rgb, tmp_rgb, m);
    pcl::copyPointCloud(model, cloud_out);
    cloud_out += cloud;

    return;
}

Eigen::Vector3i extractC3HLACSignature117(pcl::VoxelGrid<PointT> grid, pcl::PointCloud<PointT> cloud, std::vector< std::vector<float> > &feature, int color_threshold_r, int color_threshold_g, int color_threshold_b, const float voxel_size, const int subdivision_size, const int offset_x , const int offset_y, const int offset_z ){
  feature.resize( 0 );
  pcl::PointCloud<pcl::C3HLACSignature117> c3_hlac_signature;
  pcl::C3HLAC117Estimation<PointT, pcl::C3HLACSignature117> c3_hlac_;

  c3_hlac_.setRadiusSearch (0.000000001); // not used actually.
  c3_hlac_.setSearchMethod ( boost::make_shared<pcl::KdTreeFLANN<PointT> > () ); // not used actually.
  c3_hlac_.setColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b );
  if( c3_hlac_.setVoxelFilter (grid, subdivision_size, offset_x, offset_y, offset_z, voxel_size) ){
    c3_hlac_.setInputCloud ( cloud.makeShared() );
    t1 = my_clock();
    c3_hlac_.compute( c3_hlac_signature );
    t2 = my_clock();
#ifndef QUIET
    ROS_INFO (" %d c3_hlac estimated. (%f sec)", (int)c3_hlac_signature.points.size (), t2-t1);
#endif
    const int hist_num = c3_hlac_signature.points.size();
    feature.resize( hist_num );
    for( int h=0; h<hist_num; h++ ){
      feature[ h ].resize( DIM_C3HLAC_117_1_3_ALL );
      for( int i=0; i<DIM_C3HLAC_117_1_3_ALL; i++)
	feature[ h ][ i ] = c3_hlac_signature.points[ h ].histogram[ i ];
    }
  }
  return c3_hlac_.getSubdivNum();
}

int main (int argc, char** argv)
{
  cloud.width = 5;
  cloud.height = 4 ;
  cloud.is_dense = true;
  cloud.resize(20);
  cloud[0].x = 100;   cloud[0].y = 8;    cloud[0].z = 5;
  cloud[1].x = 228;   cloud[1].y = 21;   cloud[1].z = 2;
  cloud[2].x = 341;   cloud[2].y = 31;   cloud[2].z = 10;
  cloud[3].x = 472;   cloud[3].y = 40;   cloud[3].z = 15;
  cloud[4].x = 578;   cloud[4].y = 48;   cloud[4].z = 3;
  cloud[5].x = 699;   cloud[5].y = 60;   cloud[5].z = 12;
  cloud[6].x = 807;   cloud[6].y = 71;   cloud[6].z = 14;
  cloud[7].x = 929;   cloud[7].y = 79;   cloud[7].z = 16;
  cloud[8].x = 1040;  cloud[8].y = 92;   cloud[8].z = 18;
  cloud[9].x = 1160;  cloud[9].y = 101;  cloud[9].z = 38;
  cloud[10].x = 1262; cloud[10].y = 109; cloud[10].z = 28;
  cloud[11].x = 1376; cloud[11].y = 121; cloud[11].z = 32;
  cloud[12].x = 1499; cloud[12].y = 128; cloud[12].z = 35;
  cloud[13].x = 1620; cloud[13].y = 143; cloud[13].z = 28;
  cloud[14].x = 1722; cloud[14].y = 150; cloud[14].z = 30;
  cloud[15].x = 1833; cloud[15].y = 159; cloud[15].z = 15;
  cloud[16].x = 1948; cloud[16].y = 172; cloud[16].z = 12;
  cloud[17].x = 2077; cloud[17].y = 181; cloud[17].z = 33;
  cloud[18].x = 2282; cloud[18].y = 190; cloud[18].z = 23;
  cloud[19].x = 2999; cloud[19].y = 202; cloud[19].z = 29;  

  pca.setInputCloud (cloud.makeShared ());

  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}

pcl::PCA<PointT>::PCA (const pcl::PointCloud<PointT>& X, bool basis_only)
{
    Base ();
    basis_only_ = basis_only;
    setInputCloud (X.makeShared ());
    compute_done_ = initCompute ();
}

	template <class _type> int cutOffFilter(pcl::PointCloud<_type>& pointsIn, pcl::PointCloud<_type>& pointsOut)
	{
		pcl::PointCloud<_type> aux1, aux2;
		pcl::PassThrough<_type> pass;

		//	Set x-limits
		pass.setInputCloud(pointsIn.makeShared());
		pass.setFilterFieldName("x");
		pass.setFilterLimits(cutOffLimits.x.min,cutOffLimits.x.max);
		pass.filter(aux1);

		//	Set y-limits
		pass.setInputCloud(aux1.makeShared());
		pass.setFilterFieldName("y");
		pass.setFilterLimits(cutOffLimits.y.min,cutOffLimits.y.max);
		pass.filter(aux2);

		//	Set z-limits
		pass.setInputCloud(aux2.makeShared());
		pass.setFilterFieldName("z");
		pass.setFilterLimits(cutOffLimits.z.min,cutOffLimits.z.max);
		pass.filter(pointsOut);

		return 0;
	}

Coordinate<typeM> mario::redDetection( const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr & cloud, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr & dst )
{
	dst = cloud->makeShared();
	long double x=0,y=0,z=0;
	int rcount=0;
	static double RED_VAL = FileIO::getConst("RED_VAL");
	static double RED_RATE = FileIO::getConst("RED_RATE");
	for(int count=0;count<dst->points.size();count++){
		if( dst->points[count].r > RED_VAL && 
			dst->points[count].r > dst->points[count].g*RED_RATE &&
			dst->points[count].r > dst->points[count].b*RED_RATE ){
				dst->points[count].r = 0;
				dst->points[count].g = 255;
				dst->points[count].b = 0;

				x += dst->points[count].x;
				y += dst->points[count].y;
				z += dst->points[count].z;
				rcount++;
		}
	}
	x/=rcount;
	y/=rcount;
	z/=rcount;
	cout<<"RedDetection:"<<x<<" "<<y<<" "<<z<<" :"<<rcount<<endl;
	return Coordinate<typeM>(x,y,z);
}

	template <class _type> int voxelFilter(pcl::PointCloud<_type>& pointsIn, pcl::PointCloud<_type>& pointsOut)
	{
		pcl::VoxelGrid<_type> grid;

		grid.setLeafSize(this->voxelLeafSizes.x, this->voxelLeafSizes.y, this->voxelLeafSizes.z);
		grid.setInputCloud(pointsIn.makeShared());
		grid.filter(pointsOut);
	}

    void OnNewMapFrame(pcl::PointCloud< pcl::PointXYZ > mapFrame)
    {
        _viewer.removeAllPointClouds(0);
        _viewer.addPointCloud(mapFrame.makeShared(), "map");
        _viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "map");
        _viewer.spinOnce();

    }

void downsampling(pcl::PointCloud<pcl::PointXYZRGB>& cloud, float th)
{
    pcl::VoxelGrid<pcl::PointXYZRGB> sor;
    sor.setInputCloud (cloud.makeShared());
    sor.setLeafSize (th, th, th);
    sor.filter (cloud);

    return;
}

inline void
filter_depth_discontinuity(
		const pcl::PointCloud<PointT> &in,
		pcl::PointCloud<PointT> &out,
		int neighbors = 2,
		float threshold = 0.3,
		float distance_min = 1,
		float distance_max = 300,
		float epsilon = 0.5
)
{
	//std::cout << "neigbors " << neighbors << " epsilon " << epsilon << " distance_max " << distance_max <<std::endl;

	boost::shared_ptr<pcl::search::KdTree<PointT> > tree_n( new pcl::search::KdTree<PointT>() );
	tree_n->setInputCloud(in.makeShared());
	tree_n->setEpsilon(epsilon);

	for(int i = 0; i< in.points.size(); ++i){
		std::vector<int> k_indices;
		std::vector<float> square_distance;

		if ( in.points.at(i).z > distance_max || in.points.at(i).z < distance_min) continue;

		//Position in image is known
		tree_n->nearestKSearch(in.points.at(i), neighbors, k_indices, square_distance);

		//std::cout << "hier " << i << " z " << in.points.at(i).z  <<std::endl;

#ifdef USE_SQUERE_DISTANCE
		const float point_distance = distance_to_origin<PointT>(in.points.at(i));
#else
		const float point_distance = in.points.at(i).z;
#endif
		float value = 0; //point_distance;
		unsigned int idx = 0;
		for(int n = 0; n < k_indices.size(); ++n){

#ifdef USE_SQUERE_DISTANCE
			float distance_n = distance_to_origin<PointT>(in.points.at(k_indices.at(n)));
#else
			float distance_n = in.points.at(k_indices.at((n))).z;
#endif
			if(value < distance_n - point_distance){
				idx = k_indices.at(n);
				value = distance_n - point_distance;
			}
		}

		if(value > threshold){
			out.push_back(in.points.at(i));
			out.at(out.size()-1).intensity = sqrt(value);
		}
	}
}

void mpcl_compute_normals(pcl::PointCloud<pcl::PointXYZ> &cloud,
                          int ksearch,
                          pcl::PointCloud<pcl::Normal> &out)
{
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

    ne.setSearchMethod (tree);
    ne.setInputCloud (cloud.makeShared());
    ne.setKSearch (ksearch);
    ne.compute (out);
}

template <typename PointInT, typename PointOutT> void
pcl_1_8::Edge<PointInT, PointOutT>::sobelMagnitudeDirection (
    const pcl::PointCloud<PointInT> &input_x, 
    const pcl::PointCloud<PointInT> &input_y,
    pcl::PointCloud<PointOutT> &output)
{
  convolution_.setInputCloud (input_x.makeShared());
  pcl::PointCloud<pcl::PointXYZI>::Ptr kernel_x (new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<pcl::PointXYZI>);
  kernel_.setKernelType (kernel<pcl::PointXYZI>::SOBEL_X);
  kernel_.fetchKernel (*kernel_x);
  convolution_.setKernel (*kernel_x);
  convolution_.filter (*magnitude_x);

  convolution_.setInputCloud (input_y.makeShared());
  pcl::PointCloud<pcl::PointXYZI>::Ptr kernel_y (new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<pcl::PointXYZI>);
  kernel_.setKernelType (kernel<pcl::PointXYZI>::SOBEL_Y);
  kernel_.fetchKernel (*kernel_y);
  convolution_.setKernel (*kernel_y);
  convolution_.filter (*magnitude_y);

  const int height = input_x.height;
  const int width = input_x.width;

  output.resize (height * width);
  output.height = height;
  output.width = width;

  for (size_t i = 0; i < output.size (); ++i)
  {
    output[i].magnitude_x = (*magnitude_x)[i].intensity;
    output[i].magnitude_y = (*magnitude_y)[i].intensity;
    output[i].magnitude = 
      sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + 
             (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
    output[i].direction = 
      atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
  }
}