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

void savePointsPly(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, const string& name)
{
    stringstream s;
    s.str("");
    s<< path + name.c_str();
    string filename=s.str();
    string filenameNumb=filename+".ply";
    ofstream plyfile;
    plyfile.open(filenameNumb.c_str());
    plyfile << "ply\n";
    plyfile << "format ascii 1.0\n";
    plyfile << "element vertex " << cloud->width <<"\n";
    plyfile << "property float x\n";
    plyfile << "property float y\n";
    plyfile << "property float z\n";
    plyfile << "property uchar diffuse_red\n";
    plyfile << "property uchar diffuse_green\n";
    plyfile << "property uchar diffuse_blue\n";
    plyfile << "end_header\n";

    for (unsigned int i=0; i<cloud->width; i++){
        plyfile << cloud->at(i).x << " " << cloud->at(i).y << " " << cloud->at(i).z << " " << (int)cloud->at(i).r << " " << (int)cloud->at(i).g << " " << (int)cloud->at(i).b << "\n";
    }
    plyfile.close();
    fprintf(stdout, "Writing finished\n");
}

pcl::CorrespondencesPtr
findingCorrespondence()
{ 
//  Find Model-Scene Correspondences with KdTree

  pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());

  pcl::KdTreeFLANN<DescriptorType> match_search;
  match_search.setInputCloud (model_descriptors);

  //  For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
  for (size_t i = 0; i < scene_descriptors->size (); ++i)
  {
    std::vector<int> neigh_indices (1);
    std::vector<float> neigh_sqr_dists (1);
    if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
    {
      continue;
    }
    int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
    if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) //  add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
    {
      pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
      model_scene_corrs->push_back (corr);
    }
  }
  std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;

  return model_scene_corrs;
}

void showSingleNormal(Body * b, pcl::PointCloud<pcl::PointNormal> & cloud, int normalIndex)
{
    b->breakVirtualContacts();

    PointContact c(b, b, position(cloud.at(normalIndex).x,cloud.at(normalIndex).y,cloud.at(normalIndex).z),
                   vec3(cloud.at(normalIndex).normal_x,cloud.at(normalIndex).normal_y,cloud.at(normalIndex).normal_z));
    VirtualContact * vc = new VirtualContact(1, 1, &c);
    b->addVirtualContact(vc);

    b->showFrictionCones(1);
}

void Writer::saveCloudMatrix(const std::string &filename_,
							 const pcl::PointCloud<pcl::PointNormal>::Ptr &cloud_)
{
	cv::Mat items = cv::Mat::zeros(cloud_->size(), 3, CV_32FC1);
	for (size_t i = 0; i < cloud_->size(); i++)
	{
		items.at<float>(i, 0) = cloud_->at(i).x;
		items.at<float>(i, 1) = cloud_->at(i).y;
		items.at<float>(i, 2) = cloud_->at(i).z;
	}

	Writer::writeMatrix(filename_, items);
}

//**********************************************************************************************************************
void saveSelectedPointCloud ()
{
  //cout << "Indices Size: " << pcl_indices.size () << "\t Cloud Size: " << gCloud->size ()   << endl;
  pcl::PointCloud<PointT>::Ptr nCloud (new pcl::PointCloud<PointT>);
  for (int i = 0; i < pcl_indices.size (); i++) {
    int index = pcl_indices.at (i);
    nCloud->push_back (gCloud->at (index));
    cout << "PCL Value # " << gCloud->at (index) << "\t " << nCloud->points[i]  << endl;    
  }
  cout << endl << "Indices Size: " << pcl_indices.size () << "\tCopied Size: " << nCloud->size () << endl;

  pcl::io::savePCDFileASCII ("/home/krishneel/Desktop/readPCL/selected_cloud.pcd", *nCloud);
  
}

cv::Mat getImage(const pcl::PointCloud<PointT>::Ptr cloud, float ap_ratio, float fx, float fy, float center_x, float center_y)
{
    int img_h = 480 * ap_ratio;
    int img_w = 640 * ap_ratio;
    
    cv::Mat img = cv::Mat::zeros(img_h, img_w, CV_8UC3);
    for( size_t i = 0 ; i < cloud->size() ; i++ )
    {
        PointT *pt_ptr = &cloud->at(i);
        uint32_t rgb = pt_ptr->rgba;
        
        int img_x = round(((*pt_ptr).x / (*pt_ptr).z * fx + center_x)*ap_ratio);
        int img_y = round(((*pt_ptr).y / (*pt_ptr).z * fy + center_y)*ap_ratio);
        if( img_x < 0 ) img_x = 0; 
        if( img_y < 0 ) img_y = 0;
        if( img_x >= img_w ) img_x = img_w-1;
        if( img_y >= img_h ) img_y = img_h-1;
        
        img.at<uchar>(img_y, img_x*3+2) = (rgb >> 16) & 0x0000ff;
        img.at<uchar>(img_y, img_x*3+1) = (rgb >> 8) & 0x0000ff;
        img.at<uchar>(img_y, img_x*3+0) = (rgb) & 0x0000ff;
    }
    
    return img;
}

// PlaneSegmentationPclRgbAlgorithm Public API
pcl::PointCloud<pcl::PointXYZRGB>::Ptr PlaneSegmentationPclRgbAlgorithm::segmentBiggestPlane (pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_orig, 
											      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_orig, 
											      pcl::ModelCoefficients::Ptr coefficients)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_dst (new pcl::PointCloud<pcl::PointXYZRGB>);
	
	// we will need a copy of the pointcloud
	*cloud_rgb_dst = *cloud_rgb_orig; 
	
	// Plane segmentation indices
	pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
	
	// Downsample to improve performance
	if (pointcloud_downsample)
		getBiggestPlaneInliersDownsampling(inliers, coefficients, cloud_orig);
	else
		getBiggestPlaneInliers(inliers, coefficients, cloud_orig);
	
	
	// set all points but inliers to black
	for (size_t i = 0; i < cloud_rgb_dst->size(); ++i)
	{
		cloud_rgb_dst->at(i).rgb = 0;
	}
	
	for (size_t i = 0; i < inliers->indices.size(); ++i)
	{
		cloud_rgb_dst->at(inliers->indices[i]).rgb = cloud_rgb_orig->at(inliers->indices[i]).rgb;
	}
	
	// coefficients should be always pointing to the camera. If they are not, they will be inverted
	fixPlaneCoefficientsOrientation(coefficients);
	
	return cloud_rgb_dst;
}

void adjustNormalsToViewPoints(
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
		pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud)
{
	if(viewpoints->size() && cloud.size())
	{
		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
		tree->setInputCloud (viewpoints);

		for(unsigned int i=0; i<cloud.size(); ++i)
		{
			std::vector<int> indices;
			std::vector<float> dist;
			tree->nearestKSearch(pcl::PointXYZ(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z), 1, indices, dist);
			UASSERT(indices.size() == 1);

			Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud.points[i].getVector3fMap();
			Eigen::Vector3f n(cloud.points[i].normal_x, cloud.points[i].normal_y, cloud.points[i].normal_z);

			float result = v.dot(n);
			if(result < 0)
			{
				//reverse normal
				cloud.points[i].normal_x *= -1.0f;
				cloud.points[i].normal_y *= -1.0f;
				cloud.points[i].normal_z *= -1.0f;
			}
		}
	}
}

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
MultiviewRecognizerWithChangeDetection<PointT>::reconstructionFiltering(typename pcl::PointCloud<PointT>::Ptr observation,
        pcl::PointCloud<pcl::Normal>::Ptr observation_normals, const Eigen::Matrix4f &absolute_pose, size_t view_id) {
    CloudPtr observation_transformed(new Cloud);
    pcl::transformPointCloud(*observation, *observation_transformed, absolute_pose);
    CloudPtr cloud_tmp(new Cloud);
    std::vector<int> indices;
    v4r::ChangeDetector<PointT>::difference(observation_transformed, removed_points_cumulated_history_[view_id],
                                            cloud_tmp, indices, param_.tolerance_for_cloud_diff_);

    /* Visualization of changes removal for reconstruction:
    Cloud rec_changes;
    rec_changes += *cloud_transformed;
    v4r::VisualResultsStorage::copyCloudColored(*removed_points_cumulated_history_[view_id], rec_changes, 255, 0, 0);
    v4r::VisualResultsStorage::copyCloudColored(*cloud_tmp, rec_changes, 200, 0, 200);
    stringstream ss;
    ss << view_id;
    visResStore.savePcd("reconstruction_changes_" + ss.str(), rec_changes);*/

    std::vector<bool> preserved_mask(observation->size(), false);
    for (std::vector<int>::iterator i = indices.begin(); i < indices.end(); i++) {
        preserved_mask[*i] = true;
    }
    for (size_t j = 0; j < preserved_mask.size(); j++) {
        if (!preserved_mask[j]) {
            setNan(observation->at(j));
            setNan(observation_normals->at(j));
        }
    }
    PCL_INFO("Points by change detection removed: %d\n", observation->size() - indices.size());
}

pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudXYZtoRGBA(
        pcl::PointCloud<pcl::PointXYZ>::ConstPtr inCloud)
{
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr newCloud
        (new pcl::PointCloud<pcl::PointXYZRGBA>);

    int numPoints = inCloud->points.size();
    newCloud->resize(numPoints);

    for (int i; i<numPoints; i++) {
        pcl::PointXYZRGBA &np = newCloud->at(i);
        const pcl::PointXYZ &op = inCloud->at(i);

        np.x = op.x;
        np.y = op.y;
        np.z = op.z;

        np.r = 1.0;
        np.g = 1.0;
        np.b = 1.0;

        np.a = 0.0;
    }

    return newCloud;
}

void Writer::writeOuputData(const pcl::PointCloud<pcl::PointNormal>::Ptr &cloud_,
							const std::vector<BandPtr> &bands_,
							const DescriptorParamsPtr &params_,
							const int targetPoint_)
{
	DCHParams *params = dynamic_cast<DCHParams *>(params_.get());
	if (params == NULL)
	{
		LOGW << "Output data generation only for DCH, skipping";
		return;
	}


	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr colorCloud = CloudFactory::createColorCloud(cloud_, Utils::palette12(0));
	pcl::io::savePCDFileASCII(OUTPUT_DIR "cloud.pcd", *colorCloud);


	(*colorCloud)[targetPoint_].rgba = Utils::getColor(255, 0, 0);
	pcl::io::savePCDFileASCII(OUTPUT_DIR "pointPosition.pcd", *colorCloud);


	pcl::PointCloud<pcl::PointNormal>::Ptr patch = Extractor::getNeighbors(cloud_, cloud_->at(targetPoint_), params->searchRadius);
	pcl::io::savePCDFileASCII(OUTPUT_DIR "patch.pcd", *patch);


	std::vector<pcl::PointCloud<pcl::PointNormal>::Ptr> planes = generatePlanes(bands_, params);
	for (size_t i = 0; i < bands_.size(); i++)
	{
		if (!bands_[i]->points->empty())
		{
			char name[100];
			sprintf(name, OUTPUT_DIR "band%d.pcd", (int) i);
			pcl::io::savePCDFileASCII(name, *CloudFactory::createColorCloud(bands_[i]->points, Utils::palette12(i + 1)));

			sprintf(name, OUTPUT_DIR "planeBand%d.pcd", (int) i);
			pcl::io::savePCDFileASCII(name, *planes[i]);
		}
	}


	// Write histogram data
	std::vector<Histogram> angleHistograms = DCH::generateAngleHistograms(bands_, params->useProjection);
	Writer::writeHistogram("angle_distribution", "Angle Distribution Across the Bands", angleHistograms, DEG2RAD(20), -M_PI / 2, M_PI / 2);


	// Write the descriptor to a file
	std::ofstream output;
	output.open(OUTPUT_DIR "descriptor.dat", std::fstream::out);
	output << std::fixed << std::setprecision(4);
	for (size_t i = 0; i < bands_.size(); i++)
	{
		for (size_t j = 0; j < bands_.at(i)->descriptor.size(); j++)
			output << bands_.at(i)->descriptor[j] << "\t";
		output << "\n";
	}
	output.close();
}

void
PointCloud2Vector2d (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::on_nurbs::vector_vec2d &data)
{
  for (unsigned i = 0; i < cloud->size (); i++)
  {
    pcl::PointXYZ &p = cloud->at (i);
    if (!pcl_isnan (p.x) && !pcl_isnan (p.y))
      data.push_back (Eigen::Vector2d (p.x, p.y));
  }
}

void
PointCloud2Vector3d (pcl::PointCloud<Point>::Ptr cloud, pcl::on_nurbs::vector_vec3d &data)
{
  for (unsigned i = 0; i < cloud->size (); i++)
  {
    Point &p = cloud->at (i);
    if (!pcl_isnan (p.x) && !pcl_isnan (p.y) && !pcl_isnan (p.z))
      data.push_back (Eigen::Vector3d (p.x, p.y, p.z));
  }
}

position calcPosition (std::vector<cv::Point> contours)
{
    position World_Pose;
    int cnt_z = 0;
    int cnt_y = 0;
    int cnt_x = 0;
    pcl::PointXYZ p1;

    if (globalcloud.size() != 0)
    {
        for ( int b = 0; b < contours.size();  b++ )
        {
            p1.x = globalcloud.at(contours[b].x, contours[b].y).x;
            p1.y = globalcloud.at(contours[b].x, contours[b].y).y;
            p1.z = globalcloud.at(contours[b].x, contours[b].y).z;

            if ( !isnan(p1.x))
            {
                World_Pose.x += p1.x;
                cnt_x++;
            }
            if ( !isnan(p1.y))
            {
                World_Pose.y += p1.y;
                cnt_y++;
            }
            if ( !isnan(p1.z) && (p1.z > 0))
            {
                World_Pose.z += p1.z;
                cnt_z++;
            }
        }
        if ((cnt_x != 0) && (cnt_y != 0) && (cnt_z != 0))
        {
            World_Pose.x = World_Pose.x / cnt_x;
            World_Pose.y = World_Pose.y / cnt_y;
            World_Pose.z = World_Pose.z / cnt_z;
        }
        // printf(" [%f, %f] ", World_Pose.x, World_Pose.z );
    }
    return World_Pose;
}