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

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::IndicesPtr radiusFiltering(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		float radiusSearch,
		int minNeighborsInRadius)
{
	typedef typename pcl::search::KdTree<PointT> KdTree;
	typedef typename KdTree::Ptr KdTreePtr;
	KdTreePtr tree (new KdTree(false));

	if(indices->size())
	{
		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
		int oi = 0; // output iterator
		tree->setInputCloud(cloud, indices);
		for(unsigned int i=0; i<indices->size(); ++i)
		{
			std::vector<int> kIndices;
			std::vector<float> kDistances;
			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
			if(k > minNeighborsInRadius)
			{
				output->at(oi++) = indices->at(i);
			}
		}
		output->resize(oi);
		return output;
	}
	else
	{
		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
		int oi = 0; // output iterator
		tree->setInputCloud(cloud);
		for(unsigned int i=0; i<cloud->size(); ++i)
		{
			std::vector<int> kIndices;
			std::vector<float> kDistances;
			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
			if(k > minNeighborsInRadius)
			{
				output->at(oi++) = i;
			}
		}
		output->resize(oi);
		return output;
	}
}

void projectCloudOnXYPlane(
		typename pcl::PointCloud<PointT>::Ptr & cloud)
{
	for(unsigned int i=0; i<cloud->size(); ++i)
	{
		cloud->at(i).z = 0;
	}
}

  void OctreeVoxelGrid::generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr& input_msg)
  {

    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
    typename pcl::PointCloud<PointT>::Ptr cloud_voxeled (new pcl::PointCloud<PointT> ());
    pcl::fromROSMsg(*input_msg, *cloud);

    // generate octree
    typename pcl::octree::OctreePointCloud<PointT> octree(resolution_);
    // add point cloud to octree
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();
    // get points where grid is occupied
    typename pcl::octree::OctreePointCloud<PointT>::AlignedPointTVector point_vec;
    octree.getOccupiedVoxelCenters(point_vec);
    // put points into point cloud
    cloud_voxeled->width = point_vec.size();
    cloud_voxeled->height = 1;
    for (int i = 0; i < point_vec.size(); i++) {
      cloud_voxeled->push_back(point_vec[i]);
    }

    // publish point cloud
    sensor_msgs::PointCloud2 output_msg;
    toROSMsg(*cloud_voxeled, output_msg);
    output_msg.header = input_msg->header;
    pub_cloud_.publish(output_msg);

    // publish marker
    if (publish_marker_flag_) {
      visualization_msgs::Marker marker_msg;
      marker_msg.type = visualization_msgs::Marker::CUBE_LIST;
      marker_msg.scale.x = resolution_;
      marker_msg.scale.y = resolution_;
      marker_msg.scale.z = resolution_;
      marker_msg.header = input_msg->header;
      marker_msg.pose.orientation.w = 1.0;
      if (marker_color_ == "flat") {
        marker_msg.color = jsk_topic_tools::colorCategory20(0);
        marker_msg.color.a = marker_color_alpha_;
      }
      
      // compute min and max
      Eigen::Vector4f minpt, maxpt;
      pcl::getMinMax3D<PointT>(*cloud_voxeled, minpt, maxpt);
      PointT p;
      for (size_t i = 0; i < cloud_voxeled->size(); i++) {
        p = cloud_voxeled->at(i);
        geometry_msgs::Point point_ros;
        point_ros.x = p.x;
        point_ros.y = p.y;
        point_ros.z = p.z;
        marker_msg.points.push_back(point_ros);
        if (marker_color_ == "flat") {
          marker_msg.colors.push_back(jsk_topic_tools::colorCategory20(0));
        }
        else if (marker_color_ == "z") {
          marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.z - minpt[2]) / (maxpt[2] - minpt[2])));
        }
        else if (marker_color_ == "x") {
          marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.x - minpt[0]) / (maxpt[0] - minpt[0])));
        }
        else if (marker_color_ == "y") {
          marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.y - minpt[1]) / (maxpt[1] - minpt[1])));
        }
        marker_msg.colors[marker_msg.colors.size() - 1].a = marker_color_alpha_;
      }
      pub_marker_.publish(marker_msg);
    }
  }