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

void
GrabCutHelper::setInputCloud (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
{
  pcl::GrabCut<pcl::PointXYZRGB>::setInputCloud (cloud);
  // Reset clouds
  n_links_image_.reset (new pcl::PointCloud<float> (cloud->width, cloud->height, 0));
  t_links_image_.reset (new pcl::segmentation::grabcut::Image (cloud->width, cloud->height));
  gmm_image_.reset (new pcl::segmentation::grabcut::Image (cloud->width, cloud->height));
  alpha_image_.reset (new pcl::PointCloud<float> (cloud->width, cloud->height, 0));
  image_height_1_ = cloud->height-1;
  image_width_1_ = cloud->width-1;
}

    Extractor() {
        tree.reset(new pcl::KdTreeFLANN<Point > ());
        cloud.reset(new pcl::PointCloud<Point>);
        cloud_filtered.reset(new pcl::PointCloud<Point>);
        cloud_normals.reset(new pcl::PointCloud<pcl::Normal>);
        coefficients_plane_.reset(new pcl::ModelCoefficients);
        coefficients_cylinder_.reset(new pcl::ModelCoefficients);
        inliers_plane_.reset(new pcl::PointIndices);
        inliers_cylinder_.reset(new pcl::PointIndices);

        // Filter Pass
        pass.setFilterFieldName("z");
        pass.setFilterLimits(-100, 100);

        // VoxelGrid for Downsampling
        LEAFSIZE = 0.01f;
        vg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE);

        // Any object < CUT_THRESHOLD will be abandoned.
        //CUT_THRESHOLD = (int) (LEAFSIZE * LEAFSIZE * 7000000); // 700
        CUT_THRESHOLD =  700; //for nonfiltering

        // Clustering
        cluster_.setClusterTolerance(0.06 * UNIT);
        cluster_.setMinClusterSize(50.0);
        cluster_.setSearchMethod(clusters_tree_);

        // Normals
        ne.setSearchMethod(tree);
        ne.setKSearch(50); // 50 by default

        // plane SAC
        seg_plane.setOptimizeCoefficients(true);
        seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE);
        seg_plane.setNormalDistanceWeight(0.1); // 0.1
        seg_plane.setMethodType(pcl::SAC_RANSAC);
        seg_plane.setMaxIterations(1000); // 10000
        seg_plane.setDistanceThreshold(0.05); // 0.03

        // cylinder SAC
        seg_cylinder.setOptimizeCoefficients(true);
        seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER);
        seg_cylinder.setMethodType(pcl::SAC_RANSAC);
        seg_cylinder.setNormalDistanceWeight(0.1);
        seg_cylinder.setMaxIterations(10000);
        seg_cylinder.setDistanceThreshold(0.02); // 0.05
        seg_cylinder.setRadiusLimits(0.02, 0.07); // [0, 0.1]
    }

template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::getCentroidCloud (pcl::PointCloud<PointSuperVoxel>::Ptr &cloud )
{
  bool use_existing_points = true;
  if ((cloud == 0) || (cloud->size () != this->OctreeBaseT::getLeafCount ()))
  {
    cloud.reset ();
    cloud = boost::make_shared<pcl::PointCloud<PointSuperVoxel> > ();
    cloud->reserve (this->OctreeBaseT::getLeafCount ());
    use_existing_points = false;
  }
  typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr;
  leaf_itr = this->leaf_begin ();
  
  LeafContainerT* leafContainer;
  PointSuperVoxel point;
  for ( int index = 0; leaf_itr != this->leaf_end (); ++leaf_itr,++index)
  {
    leafContainer = dynamic_cast<LeafContainerT*> (*leaf_itr);
    if (!use_existing_points)
    {
      leafContainer->getCentroid (point);
      cloud->push_back (point);
    }
    else
    {
      leafContainer->getCentroid (cloud->points[index]);
    }
    
  }  
}

void ShowCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud, const std::string& name) { 
	cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::copyPointCloud(*_cloud,*cloud);
	cloud_to_show_name = name;
	show_cloud = true;
	show_cloud_A = true;
}

void ShowClouds(const vector<cv::Point3d>& pointcloud,
				const vector<cv::Vec3b>& pointcloud_RGB,
				const vector<cv::Point3d>& pointcloud1,
				const vector<cv::Vec3b>& pointcloud1_RGB) 
{
	cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	cloud1.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	orig_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    
	PopulatePCLPointCloud(cloud,pointcloud,pointcloud_RGB);
	PopulatePCLPointCloud(cloud1,pointcloud1,pointcloud1_RGB);
	copyPointCloud(*cloud,*orig_cloud);
	cloud_to_show_name = "";
	show_cloud = true;
	show_cloud_A = true;
}

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]));
    }
}

void FindFloorPlaneRANSAC()
{	
	double t = getTickCount();
	cout << "RANSAC...";
	/*
	 pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> (cloud));
	 */
	pcl::SampleConsensusModelNormalPlane<pcl::PointXYZRGB,pcl::Normal>::Ptr model_p(
					new pcl::SampleConsensusModelNormalPlane<pcl::PointXYZRGB,pcl::Normal>(cloud));
	//	model_p->setInputCloud(cloud);
	model_p->setInputNormals(mls_normals);
	model_p->setNormalDistanceWeight(0.75);
	
	inliers.reset(new vector<int>);
	
	ransac.reset(new pcl::RandomSampleConsensus<pcl::PointXYZRGB>(model_p));
	ransac->setDistanceThreshold (.1);
	ransac->computeModel();	
	ransac->getInliers(*inliers);
	
	t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "Done. (" << t <<"s)"<< endl;
	
	ransac->getModelCoefficients(coeffs[0]);
	model_p->optimizeModelCoefficients(*inliers,coeffs[0],coeffs[1]);
	
	floorcloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud(*cloud,*inliers,*floorcloud);

}

inline bool calcPointsPCL(Mat &depth_img, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, float scale) {

	// TODO: dont handle only scale, but also the offset (c_x, c_y) of the given images center to the original image center (for training and roi images!)
	
	cloud.reset(new pcl::PointCloud<pcl::PointXYZ>(depth_img.cols, depth_img.rows));
	const float bad_point = 0;//std::numeric_limits<float>::quiet_NaN ();
	const float constant_x = M_PER_MM / F_X;
	const float constant_y = M_PER_MM / F_Y;
	bool is_valid = false;
	int centerX = depth_img.cols/2.0;
	int centerY = depth_img.rows/2.0;
	float x, y, z;
	int row, col = 0;

	for (row = 0, y = -centerY; row < depth_img.rows; ++row, ++y) {
		float* r_ptr = depth_img.ptr<float>(row);

		for (col = 0, x = -centerX; col < depth_img.cols; ++col, ++x) {
			pcl::PointXYZ newPoint;
			z = r_ptr[col];

			if(z) {
				newPoint.x = (x/scale)*z*constant_x;
				newPoint.y = (y/scale)*z*constant_y;
				newPoint.z = z*M_PER_MM;
				is_valid = true;
			} else {
				newPoint.x = newPoint.y = newPoint.z = bad_point;
			}
			cloud->at(col,row) = newPoint;
		}
	}

	return is_valid;
}

void Camera::getPointCloud(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
{
    cloud.reset(new  pcl::PointCloud<pcl::PointXYZRGBA>);
    for (unsigned int h = 0; h < _depth.rows; h++)
    {
        for (unsigned int w = 0; w < _depth.cols; w++)
        {
            //            point.z = 1024 * rand () / (RAND_MAX + 1.0f); // depthValue
            //            //point.z /= 1000;
            //            point.x = 1024 * rand () / (RAND_MAX + 1.0f) ;//(w - 319.5); //321.075 1024 * rand () / (RAND_MAX + 1.0f) //(w - 319.5) * point.z * rgbFocalInvertedX;
            //            point.y = 1024
            unsigned short depthValue;
            depthValue = _depth.at<unsigned short>(h,w);
            //cv::Vec3f pt3D = _bgr.at<Vec3f>(h, w);
            pcl::PointXYZRGBA point;
            point.z = depthValue; // depthValue
            point.z /= 1000;
            point.x = (w - C_X) * point.z * rgbFocalInvertedX ;//(w - 319.5); //321.075 1024 * rand () / (RAND_MAX + 1.0f) //(w - 319.5) * point.z * rgbFocalInvertedX;
            point.y = (h - C_Y) * point.z * rgbFocalInvertedY; //248.919//(h - 239.5) * point.z * rgbFocalInvertedY
            point.r = 255;
            point.g = 255;
            point.b = 255;
            cloud->push_back(point);
            //std::cout << " point x " << point.x << "  " << point.y << " " << point.z << std::endl;
        }

    }

}

void KinectGrabber_Rawlog2::getCurrentColouredPointCloudPtr(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& colouredPointCloudPtr)
{
    colouredPointCloudPtr=currentColouredPointCloudPtr;

    //Change MRPT coordinate system to the PCL system
    Eigen::Matrix4f rotationMatrix;
    rotationMatrix(0,0)=0;
    rotationMatrix(0,1)=-1;
    rotationMatrix(0,2)=0;
    rotationMatrix(0,3)=0;
    rotationMatrix(1,0)=0;
    rotationMatrix(1,1)=0;
    rotationMatrix(1,2)=-1;
    rotationMatrix(1,3)=0;
    rotationMatrix(2,0)=1;
    rotationMatrix(2,1)=0;
    rotationMatrix(2,2)=0;
    rotationMatrix(2,3)=0;
    rotationMatrix(3,0)=0;
    rotationMatrix(3,1)=0;
    rotationMatrix(3,2)=0;
    rotationMatrix(3,3)=1;

    colouredPointCloudPtr.reset(new pcl::PointCloud<pcl::PointXYZRGBA>());
    pcl::transformPointCloud(*currentColouredPointCloudPtr,*colouredPointCloudPtr,rotationMatrix);
}

void FindNormalsMLS()
//find normals using MLS
{
	double t = getTickCount();
	cout << "MLS...";
	
	mls_normals.reset(new pcl::PointCloud<pcl::Normal> ());
	
	// Create a KD-Tree
	pcl::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
	
	// Output has the same type as the input one, it will be only smoothed
	pcl::PointCloud<pcl::PointXYZRGB> mls_points;
	
	// Init object (second point type is for the normals, even if unused)
	pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::Normal> mls;
	
	// Optionally, a pointer to a cloud can be provided, to be set by MLS
	mls.setOutputNormals (mls_normals);
	
	// Set parameters
	mls.setInputCloud (cloud);
	mls.setPolynomialFit (true);
	mls.setSearchMethod (tree);
	mls.setSearchRadius (0.16);
	
	// Reconstruct
	mls.reconstruct (mls_points);
	pcl::copyPointCloud<pcl::PointXYZRGB,pcl::PointXYZRGB>(mls_points,*cloud);
	
	t = ((double)getTickCount() - t)/getTickFrequency();
	cout << "Done. (" << t <<"s)"<< endl;
}

void
pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
{
  int idx = event.getPointIndex ();
  if (idx == -1)
    return;

  if (!cloud)
  {
    cloud = *reinterpret_cast<pcl::PCLPointCloud2::Ptr*> (cookie);
    xyzcloud.reset (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2 (*cloud, *xyzcloud);
    search.setInputCloud (xyzcloud);
  }
  // Return the correct index in the cloud instead of the index on the screen
  std::vector<int> indices (1);
  std::vector<float> distances (1);

  // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
  pcl::PointXYZ picked_pt;
  event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
  //TODO: Look into this.
  search.nearestKSearch (picked_pt, 1, indices, distances);

  PCL_INFO ("Point index picked: %d (real: %d) - [%f, %f, %f]\n", idx, indices[0], picked_pt.x, picked_pt.y, picked_pt.z);

  idx = indices[0];
  // If two points were selected, draw an arrow between them
  pcl::PointXYZ p1, p2;
  if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p)
  {
    std::stringstream ss;
    ss << p1 << p2;
    p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ());
    return;
  }

  // Else, if a single point has been selected
  std::stringstream ss;
  ss << idx;
  // Get the cloud's fields
  for (size_t i = 0; i < cloud->fields.size (); ++i)
  {
    if (!isMultiDimensionalFeatureField (cloud->fields[i]))
      continue;
    PCL_INFO ("Multidimensional field found: %s\n", cloud->fields[i].name.c_str ());
#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
    ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, idx, ss.str ());
    ph_global.renderOnce ();
#endif
  }
  if (p)
  {
    pcl::PointXYZ pos;
    event.getPoint (pos.x, pos.y, pos.z);
    p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
  }
  
}

 void
 SegmenterLight::computeNormals (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_in,
                            pcl::PointCloud<pcl::Normal>::Ptr &normals_out)
 {
   normals_out.reset (new pcl::PointCloud<pcl::Normal>);
   surface::ZAdaptiveNormals::Parameter za_param;
   za_param.adaptive = true;
   surface::ZAdaptiveNormals nor (za_param);
   nor.setInputCloud (cloud_in);
   nor.compute ();
   nor.getNormals (normals_out);
 }

void RunVisualization(const vector<cv::Point3d>& pointcloud,
					  const std::vector<cv::Vec3b>& pointcloud_RGB,
					  const Mat& img_1_orig, 
					  const Mat& img_2_orig,
					  const vector<KeyPoint>& correspImg1Pt) {
	cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	orig_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
	//  pcl::io::loadPCDFile ("output.pcd", *cloud);
    
	PopulatePCLPointCloud(pointcloud,pointcloud_RGB,img_1_orig,img_2_orig,correspImg1Pt);
//	SORFilter();
	copyPointCloud(*cloud,*orig_cloud);
//	FindNormalsMLS();
//	FindFloorPlaneRANSAC();
	
	//	pcl::PointCloud<pcl::PointXYZRGB>::Ptr final (new pcl::PointCloud<pcl::PointXYZRGB>);
	//	pcl::copyPointCloud<pcl::PointXYZRGB>(*cloud, inliers, *final);
	
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    
    //blocks until the cloud is actually rendered
    viewer.showCloud(orig_cloud,"orig");
	//	viewer.showCloud(final);
    
    //use the following functions to get access to the underlying more advanced/powerful
    //PCLVisualizer
    
    //This will only get called once
    viewer.runOnVisualizationThreadOnce (viewerOneOff);
    
    //This will get called once per visualization iteration
	viewer.runOnVisualizationThread (viewerThread);
    while (!viewer.wasStopped ())
    {
		//you can also do cool processing here
		//FIXME: Note that this is running in a separate thread from viewerPsycho
		//and you should guard against race conditions yourself...
		//		user_data++;
    }
}	

void GeneralTransform::Mat2PCD_Trans(cv::Mat& cloud_mat, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    int size = cloud_mat.rows;
    std::vector<pcl::PointXYZ> points_vec(size);
    cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
    for(int i = 0; i < size; i++)
    {
        pcl::PointXYZ point;
        point.x = cloud_mat.at<double>(i, 0);
        point.y = cloud_mat.at<double>(i, 1);
        point.z = cloud_mat.at<double>(i, 2);
        cloud->push_back(point);
    }   
}