本文整理汇总了C++中typenamepcl::PointCloud::push_back方法的典型用法代码示例。如果您正苦于以下问题:C++ PointCloud::push_back方法的具体用法?C++ PointCloud::push_back怎么用?C++ PointCloud::push_back使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类typenamepcl::PointCloud
的用法示例。
在下文中一共展示了PointCloud::push_back方法的4个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: generateVoxelCloudImpl
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);
}
}
示例2: cropMesh
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
const kinfu_msgs::KinfuCloudPoint & max,typename pcl::PointCloud<PointT>::ConstPtr cloud,
TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud, TrianglesPtr out_triangles)
{
const uint triangles_size = triangles->size();
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
std::vector<uint> valid_points_remap(cloud_size,0);
uint offset;
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const PointT & pt = (*cloud)[i];
if (pt.x > max.x || pt.y > max.y || pt.z > max.z ||
pt.x < min.x || pt.y < min.y || pt.z < min.z)
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
offset = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
// save new position for triangles remap
valid_points_remap[i] = offset;
offset++;
}
out_cloud->resize(offset);
// discard invalid triangles
out_triangles->clear();
out_triangles->reserve(triangles_size);
offset = 0;
for (uint i = 0; i < triangles_size; i++)
{
const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i];
bool is_valid = true;
// validate all the vertices
for (uint h = 0; h < 3; h++)
if (!valid_points[tri.vertex_id[h]])
{
is_valid = false;
break;
}
if (is_valid)
{
kinfu_msgs::KinfuMeshTriangle out_tri;
// remap the triangle
for (uint h = 0; h < 3; h++)
out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]];
out_triangles->push_back(out_tri);
offset++;
}
}
out_triangles->resize(offset);
}
示例3: cloud
bool
segmentCloud (pointing_object_evaluation::ObjectSegmentationRequest::Request &req, pointing_object_evaluation::ObjectSegmentationRequest::Response &res)
{
CloudPtr cloud (new Cloud ());
{
//boost::lock_guard<boost::mutex> lock (cloud_mutex_);
*cloud = *cloud_;
}
unsigned char red [6] = {255, 0, 0, 255, 255, 0};
unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
pcl::PointCloud<pcl::PointXYZRGB> aggregate_cloud;
// Estimate Normals
double ne_start = pcl::getTime ();
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
ne_.setInputCloud (cloud);
ne_.compute (*normal_cloud);
double ne_end = pcl::getTime ();
//std::cout << "NE took: " << double(ne_end - ne_start) << std::endl;
// Segment Planes
double mps_start = pcl::getTime ();
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
mps_.setInputNormals (normal_cloud);
mps_.setInputCloud (cloud);
//mps.segment (regions);
mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
//Segment Objects
typename pcl::PointCloud<PointT>::CloudVectorType clusters;
if (regions.size () > 0)
{
//printf ("got some planes!\n");
std::vector<bool> plane_labels;
plane_labels.resize (label_indices.size (), false);
for (size_t i = 0; i < label_indices.size (); i++)
{
if (label_indices[i].indices.size () > 10000)
{
plane_labels[i] = true;
}
}
//printf ("made label vec\n");
typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>());
euclidean_cluster_comparator_->setInputCloud (cloud);
euclidean_cluster_comparator_->setLabels (labels);
euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
euclidean_cluster_comparator_->setDistanceThreshold (0.01f, false);
pcl::PointCloud<pcl::Label> euclidean_labels;
std::vector<pcl::PointIndices> euclidean_label_indices;
pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
euclidean_segmentation.setInputCloud (cloud);
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
//printf ("done segmenting the clusters\n");
std::vector<Eigen::Vector4f> centroids;
for (size_t i = 0; i < euclidean_label_indices.size (); i++)
{
if (euclidean_label_indices[i].indices.size () > 1000)
{
pcl::PointCloud<PointT> cluster;
pcl::copyPointCloud (*cloud,euclidean_label_indices[i].indices,cluster);
clusters.push_back (cluster);
/*Eigen::Vector4f centroid;
pcl::compute3DCentroid (cluster, centroid);
centroids.push_back (centroid);*/
//pcl::PointXYZ centroid_pt (centroid[0], centroid[1], centroid[2]);
//double dist = sqrt (centroid[0]*centroid[0] + centroid[1]*centroid[1] + centroid[2]*centroid[2]);
//object_points_.push_back (centroid_pt);
// Send to RViz
pcl::PointCloud<pcl::PointXYZRGB> color_cluster;
pcl::copyPointCloud (cluster, color_cluster);
for (size_t j = 0; j < color_cluster.size (); j++)
{
color_cluster.points[j].r = (color_cluster.points[j].r + red[i%6]) / 2;
color_cluster.points[j].g = (color_cluster.points[j].g + grn[i%6]) / 2;
color_cluster.points[j].b = (color_cluster.points[j].b + blu[i%6]) / 2;
}
aggregate_cloud += color_cluster;
}
//.........这里部分代码省略.........
示例4: interval
template <typename PointT> void
pcl::approximatePolygon2D (const typename pcl::PointCloud<PointT>::VectorType &polygon,
typename pcl::PointCloud<PointT>::VectorType &approx_polygon,
float threshold, bool refine, bool closed)
{
approx_polygon.clear ();
if (polygon.size () < 3)
return;
std::vector<std::pair<unsigned, unsigned> > intervals;
std::pair<unsigned,unsigned> interval (0, 0);
if (closed)
{
float max_distance = .0f;
for (unsigned idx = 1; idx < polygon.size (); ++idx)
{
float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) +
(polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);
if (distance > max_distance)
{
max_distance = distance;
interval.second = idx;
}
}
for (unsigned idx = 1; idx < polygon.size (); ++idx)
{
float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) +
(polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);
if (distance > max_distance)
{
max_distance = distance;
interval.first = idx;
}
}
if (max_distance < threshold * threshold)
return;
intervals.push_back (interval);
std::swap (interval.first, interval.second);
intervals.push_back (interval);
}
else
{
interval.first = 0;
interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
intervals.push_back (interval);
}
std::vector<unsigned> result;
// recursively refine
while (!intervals.empty ())
{
std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);
line_x *= linelen;
line_y *= linelen;
line_d *= linelen;
float max_distance = 0.0;
unsigned first_index = currentInterval.first + 1;
unsigned max_index = 0;
// => 0-crossing
if (currentInterval.first > currentInterval.second)
{
for (unsigned idx = first_index; idx < polygon.size(); idx++)
{
float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
if (distance > max_distance)
{
max_distance = distance;
max_index = idx;
}
}
first_index = 0;
}
for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
{
float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
if (distance > max_distance)
{
max_distance = distance;
max_index = idx;
}
}
if (max_distance > threshold)
{
std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
//.........这里部分代码省略.........