本文整理汇总了C++中cloud函数的典型用法代码示例。如果您正苦于以下问题:C++ cloud函数的具体用法?C++ cloud怎么用?C++ cloud使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了cloud函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: main
int
main(int argc, char** argv)
{
// initialize PointClouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
// populate our PointCloud with points
cloud->width = 500;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if (i % 5 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
else if(i % 2 == 0)
cloud->points[i].z = sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
else
cloud->points[i].z = - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
}
else
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if( i % 2 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
else
cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
}
}
std::vector<int> inliers;
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
if(pcl::console::find_argument (argc, argv, "-f") >= 0)
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
// creates the visualization object and adds either our orignial cloud or all of the inliers
// depending on the command line arguments specified.
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
viewer = simpleVis(final);
else
开发者ID:2php,项目名称:pcl,代码行数:69,代码来源:random_sample_consensus.cpp
示例2: jit_pcl_segeuclidean_matrix_calc
// Methods bound to input/inlets
t_jit_err jit_pcl_segeuclidean_matrix_calc(t_jit_pcl_segeuclidean *x, void *inputs, void *outputs)
{
t_jit_err err = JIT_ERR_NONE;
long in_savelock;
long out_savelock;
t_jit_matrix_info in_minfo;
t_jit_matrix_info out_minfo;
char *in_bp;
char *out_bp;
long i, j;
long dimcount;
long planecount;
long dim[JIT_MATRIX_MAX_DIMCOUNT];
void *in_matrix;
void *out_matrix;
long rowstride;
float *fip, *fop;
in_matrix = jit_object_method(inputs,_jit_sym_getindex,0);
out_matrix = jit_object_method(outputs,_jit_sym_getindex,0);
if (x && in_matrix && out_matrix)
{
in_savelock = (long) jit_object_method(in_matrix, _jit_sym_lock, 1);
out_savelock = (long) jit_object_method(out_matrix, _jit_sym_lock, 1);
jit_object_method(in_matrix, _jit_sym_getinfo, &in_minfo);
jit_object_method(in_matrix, _jit_sym_getdata, &in_bp);
if (!in_bp) {
err=JIT_ERR_INVALID_INPUT;
goto out;
}
//get dimensions/planecount
dimcount = in_minfo.dimcount;
planecount = in_minfo.planecount;
if( planecount < 3 )
{
object_error((t_object *)x, "jit.pcl requires a 3 plane matrix (xyz)");
goto out;
}
if( in_minfo.type != _jit_sym_float32)
{
object_error((t_object *)x, "received: %s jit.pcl uses only float32 matrixes", in_minfo.type->s_name );
goto out;
}
//if dimsize is 1, treat as infinite domain across that dimension.
//otherwise truncate if less than the output dimsize
for (i=0; i<dimcount; i++) {
dim[i] = in_minfo.dim[i];
if ( in_minfo.dim[i]<dim[i] && in_minfo.dim[i]>1) {
dim[i] = in_minfo.dim[i];
}
}
//******
// PCL stuff
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = (uint32_t)dim[0];
cloud->height = (uint32_t)dim[1];
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
rowstride = in_minfo.dimstride[1];// >> 2L;
size_t count = 0;
for (j = 0; j < dim[0]; j++)
{
fip = (float *)(in_bp + j * in_minfo.dimstride[0]);
for( i = 0; i < dim[1]; i++)
{
cloud->points[count].x = ((float *)fip)[0];
cloud->points[count].y = ((float *)fip)[1];
cloud->points[count].z = ((float *)fip)[2];
// cloud->points[count].r = (uint8_t)(((float *)fip)[3] * 255.0);
// cloud->points[count].g = (uint8_t)(((float *)fip)[4] * 255.0);
// cloud->points[count].b = (uint8_t)(((float *)fip)[5] * 255.0);
count++;
fip += rowstride;
}
}
{
/*
//filter
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_voxel_ (new pcl::PointCloud<pcl::PointXYZRGB>);
grid.setLeafSize (x->leafsize, x->leafsize, x->leafsize);
//.........这里部分代码省略.........
开发者ID:ramagottfried,项目名称:optic,代码行数:101,代码来源:jit.pcl.segment.euclidean.cpp
示例3: main
int
main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PCDWriter writer;
pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>);
std::vector<int> filenames;
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
reader.read (argv[filenames[0]], *cloud);
std::string pcd_filename;
std::string png_filename = argv[filenames[0]];
// Take the origional png image out
png::image<png::rgb_pixel> origin_image(cloud->width, cloud->height);
int origin_index = 0;
for (size_t y = 0; y < origin_image.get_height (); ++y) {
for (size_t x = 0; x < origin_image.get_width (); ++x) {
const PointType & p = cloud->points[origin_index++];
origin_image[y][x] = png::rgb_pixel(p.r, p.g, p.b);
}
}
png_filename.replace(png_filename.length () - 4, 4, ".png");
origin_image.write(png_filename);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
float min_depth = 0.1;
pcl::console::parse_argument (argc, argv, "-min_depth", min_depth);
float max_depth = 3.0;
pcl::console::parse_argument (argc, argv, "-max_depth", max_depth);
float max_x = 1.0;
pcl::console::parse_argument (argc, argv, "-max_x", max_x);
float min_x = -1.0;
pcl::console::parse_argument (argc, argv, "-min_x", min_x);
float max_y = 1.0;
pcl::console::parse_argument (argc, argv, "-max_y", max_y);
float min_y = -1.0;
pcl::console::parse_argument (argc, argv, "-min_y", min_y);
int plane_seg_times = 1;
pcl::console::parse_argument (argc, argv, "-plane_seg_times", plane_seg_times);
for (int i = 0; i < plane_seg_times; i++) {
remove_plane(cloud, min_depth, max_depth, min_x, max_x, min_y, max_y);
}
pcd_filename = argv[filenames[0]];
pcd_filename.replace(pcd_filename.length () - 4, 8, "plane.pcd");
pcl::io::savePCDFile(pcd_filename, *cloud);
// std::cout << "PointCloud after removing the plane has: " << cloud->points.size () << " data points." << std::endl; //*
uint32_t xmin = 1000, xmax = 0, ymin = 1000, ymax = 0;
pcl::PointCloud<PointXYZRGBUV>::Ptr cloud_uv (new pcl::PointCloud<PointXYZRGBUV>);
for (size_t index = 0; index < cloud->points.size(); index++) {
const pcl::PointXYZRGB & p = cloud->points[index];
if (p.x != p.x || p.y != p.y || p.z != p.z) { // if current point is invalid
continue;
}
PointXYZRGBUV cp = PointXYZRGBUV(p, index % cloud-> width, index / cloud->width);
cloud_uv->points.push_back (cp);
}
cloud_uv->width = cloud_uv->points.size ();
cloud_uv->height = 1;
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<PointXYZRGBUV>::Ptr tree (new pcl::search::KdTree<PointXYZRGBUV>);
tree->setInputCloud (cloud_uv);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointXYZRGBUV> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (500);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_uv);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
xmin = 1000;
xmax = 0;
ymin = 1000;
ymax = 0;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) {
PointXYZRGBUV& p = cloud_uv->points[*pit];
pcl::PointXYZRGB cp_rgb;
cp_rgb.x = p.x; cp_rgb.y = p.y; cp_rgb.z = p.z;
//.........这里部分代码省略.........
开发者ID:lwt1104,项目名称:pcl_tool_code,代码行数:101,代码来源:plane_cluster_seg.cpp
示例4: callback
void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
std::string base_frame("base_link");
geometry_msgs::PointStamped pout;
geometry_msgs::PointStamped pin;
pin.header.frame_id = msg->header.frame_id;
pin.point.x = 0; pin.point.y = 0; pin.point.z = 0;
geometry_msgs::Vector3Stamped vout;
geometry_msgs::Vector3Stamped vin;
vin.header.frame_id = base_frame;
vin.vector.x = 0; vin.vector.y = 0; vin.vector.z = 1;
float height;
Eigen::Vector3f normal;
try {
listener->transformPoint(base_frame, ros::Time(0), pin, msg->header.frame_id, pout);
height = pout.point.z;
listener->transformVector(msg->header.frame_id, ros::Time(0), vin, base_frame, vout);
normal = Eigen::Vector3f(vout.vector.x, vout.vector.y, vout.vector.z);
}
catch (tf::TransformException ex) {
ROS_INFO("%s",ex.what());
return;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromROSMsg(*msg, *cloud);
Eigen::Vector3f p = -height*normal; // a point in the floor plane
float d = -p.dot(normal); // = height, d in the plane equation
obstacle_cloud->points.clear();
obstacle_cloud->points.reserve(cloud->size());
floor_cloud->points.clear();
Eigen::Vector3f temp;
float floor_dist;
pcl::PointXYZ point;
for (size_t i = 0; i < cloud->size(); ++i) {
/*temp = cloud->points[i].getVector3fMap(); // DEBUG!
if (i%640 < 300 && i%640 > 200 && i < 640*200 && i > 640*100) {
temp -= 0.06*normal;
}*/
// check signed distance to floor
floor_dist = normal.dot(cloud->points[i].getVector3fMap()) + d;
//floor_dist = normal.dot(temp) + d; // DEBUG
// if enough below, consider a stair point
if (floor_dist < below_threshold) {
temp = cloud->points[i].getVector3fMap(); // RELEASE
point.getVector3fMap() = -(d/normal.dot(temp))*temp + normal*0.11;
floor_cloud->points.push_back(point);
}
else { // add as a normal obstacle or clearing point
obstacle_cloud->points.push_back(cloud->points[i]);
}
}
sensor_msgs::PointCloud2 floor_msg;
pcl::toROSMsg(*floor_cloud, floor_msg);
floor_msg.header = msg->header;
floor_pub.publish(floor_msg);
sensor_msgs::PointCloud2 obstacle_msg;
pcl::toROSMsg(*obstacle_cloud, obstacle_msg);
obstacle_msg.header = msg->header;
obstacle_pub.publish(obstacle_msg);
}
开发者ID:nilsbore,项目名称:strands_movebase,代码行数:73,代码来源:mirror_floor_points.cpp
示例5: cloud
void ColorizeRandomForest::extract(const sensor_msgs::PointCloud2 pc)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
std::vector<int> indices;
pcl::fromROSMsg(pc, *cloud);
cloud->is_dense = false;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName (std::string("z"));
pass.setFilterLimits (0.0, 1.5);
pass.filter (*cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.025);
ec.setMinClusterSize (200);
ec.setMaxClusterSize (100000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr noncloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
int cluster_num = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
JSK_ROS_INFO("Calculate time %d / %ld", cluster_num , cluster_indices.size());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud->points[*pit]);
cloud_cluster->is_dense = true;
cluster_num ++ ;
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
ne.setInputCloud (cloud_cluster);
ne.setSearchMethod (tree);
ne.setRadiusSearch (0.02);
ne.compute (*cloud_normals);
for (int cloud_index = 0; cloud_index < cloud_normals->points.size(); cloud_index++) {
cloud_normals->points[cloud_index].x = cloud_cluster->points[cloud_index].x;
cloud_normals->points[cloud_index].y = cloud_cluster->points[cloud_index].y;
cloud_normals->points[cloud_index].z = cloud_cluster->points[cloud_index].z;
}
int result_counter=0, call_counter = 0;
pcl::PointXYZRGBNormal max_pt,min_pt;
pcl::getMinMax3D(*cloud_normals, min_pt, max_pt);
for (int i = 0 ; i < 30; i++) {
double lucky = 0, lucky2 = 0;
std::string axis("x"), other_axis("y");
int rand_xy = rand()%2;
if (rand_xy == 0) {
lucky = min_pt.x - pass_offset_ + (max_pt.x - min_pt.x - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
lucky2 = min_pt.y + (max_pt.y - min_pt.y) * 1.0 * rand() / RAND_MAX;
} else {
lucky = min_pt.y - pass_offset_ + (max_pt.y - min_pt.y - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
lucky2 = min_pt.x + (max_pt.x - min_pt.x) * 1.0 * rand() / RAND_MAX;
axis = std::string("y");
other_axis = std::string("x");
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals_pass (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
pcl::IndicesPtr indices_x(new std::vector<int>());
pass.setInputCloud (cloud_normals);
pass.setFilterFieldName (axis);
float small = std::min(lucky, lucky + pass_offset_);
float large = std::max(lucky, lucky + pass_offset_);
pass.setFilterLimits (small, large);
pass.filter (*cloud_normals_pass);
pass.setInputCloud (cloud_normals_pass);
pass.setFilterFieldName (other_axis);
float small2 = std::min(lucky2, lucky2 + pass_offset2_);
float large2 = std::max(lucky2, lucky2 + pass_offset2_);
pass.setFilterLimits (small2, large2);
pass.filter (*cloud_normals_pass);
std::vector<int> tmp_indices;
pcl::removeNaNFromPointCloud(*cloud_normals_pass, *cloud_normals_pass, tmp_indices);
if(cloud_normals_pass->points.size() == 0)
continue;
pcl::FPFHEstimationOMP<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::FPFHSignature33> fpfh;
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ());
fpfh.setNumberOfThreads(8);
fpfh.setInputCloud (cloud_normals_pass);
fpfh.setInputNormals (cloud_normals_pass);
fpfh.setSearchMethod (tree);
fpfh.setRadiusSearch (radius_search_);
fpfh.compute (*fpfhs);
//.........这里部分代码省略.........
开发者ID:rkoyama1623,项目名称:jsk_recognition,代码行数:101,代码来源:colorize_segmented_RF_nodelet.cpp
示例6: float
//.........这里部分代码省略.........
vtkSmartPointer<vtkMatrix4x4> matrixRotModel = trans_rot_pose->GetMatrix ();
vtkSmartPointer<vtkMatrix4x4> matrixTranslation = translation->GetMatrix ();
mapper->SetInputConnection (translation_filter->GetOutputPort ());
mapper->Update ();
//render view
vtkSmartPointer<vtkActor> actor_view = vtkSmartPointer<vtkActor>::New ();
actor_view->SetMapper (mapper);
renderer->SetActiveCamera (cam_tmp);
renderer->AddActor (actor_view);
renderer->Modified ();
//renderer->ResetCameraClippingRange ();
render_win->Render ();
//back to real scale transform
vtkSmartPointer<vtkTransform> backToRealScale = vtkSmartPointer<vtkTransform>::New ();
backToRealScale->PostMultiply ();
backToRealScale->Identity ();
backToRealScale->Concatenate (matrixScale);
backToRealScale->Concatenate (matrixTranslation);
backToRealScale->Inverse ();
backToRealScale->Modified ();
backToRealScale->Concatenate (matrixTranslation);
backToRealScale->Modified ();
Eigen::Matrix4f backToRealScale_eigen;
backToRealScale_eigen.setIdentity ();
for (int x = 0; x < 4; x++)
for (int y = 0; y < 4; y++)
backToRealScale_eigen (x, y) = float (backToRealScale->GetMatrix ()->GetElement (x, y));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
cloud->points.resize (resolution_ * resolution_);
if (gen_organized_)
{
cloud->width = resolution_;
cloud->height = resolution_;
cloud->is_dense = false;
double coords[3];
float * depth = new float[resolution_ * resolution_];
render_win->GetZbufferData (0, 0, resolution_ - 1, resolution_ - 1, &(depth[0]));
for (int x = 0; x < resolution_; x++)
{
for (int y = 0; y < resolution_; y++)
{
float value = depth[y * resolution_ + x];
if (value == 1.0)
{
cloud->at (y, x).x = cloud->at (y, x).y = cloud->at (y, x).z = std::numeric_limits<float>::quiet_NaN ();
}
else
{
worldPicker->Pick (x, y, value, renderer);
worldPicker->GetPickPosition (coords);
cloud->at (y, x).x = static_cast<float> (coords[0]);
cloud->at (y, x).y = static_cast<float> (coords[1]);
cloud->at (y, x).z = static_cast<float> (coords[2]);
cloud->at (y, x).getVector4fMap () = backToRealScale_eigen
* cloud->at (y, x).getVector4fMap ();
}
}
开发者ID:PointCloudLibrary,项目名称:pcl,代码行数:67,代码来源:render_views_tesselated_sphere.cpp
示例7: get_point_cloud
typename pcl::PointCloud<PointT>::Ptr
get_point_cloud(int distance, bool colored)
{
//get rgb and depth data
while(!device -> getDepth(depth_map)){}
while(!device -> getRGB(rgb)){}
int depth_width = 640;
int depth_height = 480;
//create the empty Pointcloud
boost::shared_ptr<pcl::PointCloud<PointT>> cloud (new pcl::PointCloud<PointT>);
//initialize the PointCloud height and width
//cloud->height = std::max (image_height, depth_height);
//cloud->width = std::max (image_width, depth_width);
//allow infinite values for points coordinates
cloud->is_dense = false;
//set camera parameters for kinect
float focal_x_depth = 585.187492217609;//5.9421434211923247e+02;
float focal_y_depth = 585.308616340665;//5.9104053696870778e+02;
float center_x_depth = 322.714077555293;//3.3930780975300314e+02;
float center_y_depth = 248.626108676666;//2.4273913761751615e+02;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
#pragma omp parallel for
for (unsigned int y = 0; y < depth_height; ++y)
for ( unsigned int x = 0; x < depth_width; ++x){
PointT ptout;
uint16_t dz = depth_map[y*depth_width + x];
if (abs(dz) < distance){
// project
Eigen::Vector3d ptd((x - center_x_depth) * dz / focal_x_depth, (y - center_y_depth) * dz/focal_y_depth, dz);
// assign output xyz
ptout.x = ptd.x()*0.001f;
ptout.y = ptd.y()*0.001f;
ptout.z = ptd.z()*0.001f;
if(colored){
uint8_t r = rgb[(y*depth_width + x)*3];
uint8_t g = rgb[(y*depth_width + x)*3 + 1];
uint8_t b = rgb[(y*depth_width + x)*3 + 2];
ptout.rgba = pcl::PointXYZRGB(r, g, b).rgba; //assign color
//ptout.rgba = pcl::PointXYZRGB(0, 0, 0).rgba;
} else
ptout.rgba = pcl::PointXYZRGB(0, 0, 0).rgba;
#pragma omp critical
cloud->points.push_back(ptout); //assigns point to cloud
}
}
cloud->height = 1;
cloud->width = cloud->points.size();
return (cloud);
}
开发者ID:GiorgioMire,项目名称:my_pcl_reader_freenect,代码行数:62,代码来源:freenect_grabber.hpp
示例8: lock
void TorusFinder::segment(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
if (!done_initialization_) {
return;
}
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
pcl::PointCloud<pcl::PointNormal>::Ptr cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloud_msg, *cloud);
jsk_recognition_utils::ScopedWallDurationReporter r
= timer_.reporter(pub_latest_time_, pub_average_time_);
if (voxel_grid_sampling_) {
pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::VoxelGrid<pcl::PointNormal> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
sor.filter (*downsampled_cloud);
cloud = downsampled_cloud;
}
pcl::SACSegmentation<pcl::PointNormal> seg;
if (use_normal_) {
pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
seg_normal.setInputNormals(cloud);
seg = seg_normal;
}
seg.setOptimizeCoefficients (true);
seg.setInputCloud(cloud);
seg.setRadiusLimits(min_radius_, max_radius_);
if (algorithm_ == "RANSAC") {
seg.setMethodType(pcl::SAC_RANSAC);
}
else if (algorithm_ == "LMEDS") {
seg.setMethodType(pcl::SAC_LMEDS);
}
else if (algorithm_ == "MSAC") {
seg.setMethodType(pcl::SAC_MSAC);
}
else if (algorithm_ == "RRANSAC") {
seg.setMethodType(pcl::SAC_RRANSAC);
}
else if (algorithm_ == "RMSAC") {
seg.setMethodType(pcl::SAC_RMSAC);
}
else if (algorithm_ == "MLESAC") {
seg.setMethodType(pcl::SAC_MLESAC);
}
else if (algorithm_ == "PROSAC") {
seg.setMethodType(pcl::SAC_PROSAC);
}
seg.setDistanceThreshold (outlier_threshold_);
seg.setMaxIterations (max_iterations_);
seg.setModelType(pcl::SACMODEL_CIRCLE3D);
if (use_hint_) {
seg.setAxis(hint_axis_);
seg.setEpsAngle(eps_hint_angle_);
}
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.segment(*inliers, *coefficients);
JSK_NODELET_INFO("input points: %lu", cloud->points.size());
if (inliers->indices.size() > min_size_) {
// check direction. Torus should direct to origin of pointcloud
// always.
Eigen::Vector3f dir(coefficients->values[4],
coefficients->values[5],
coefficients->values[6]);
if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
dir = - dir;
}
Eigen::Affine3f pose = Eigen::Affine3f::Identity();
Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
coefficients->values[1],
coefficients->values[2]);
Eigen::Quaternionf rot;
rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
PCLIndicesMsg ros_inliers;
ros_inliers.indices = inliers->indices;
ros_inliers.header = cloud_msg->header;
pub_inliers_.publish(ros_inliers);
PCLModelCoefficientMsg ros_coefficients;
ros_coefficients.values = coefficients->values;
ros_coefficients.header = cloud_msg->header;
pub_coefficients_.publish(ros_coefficients);
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
tf::poseEigenToMsg(pose, torus_msg.pose);
torus_msg.small_radius = 0.01;
torus_msg.large_radius = coefficients->values[3];
pub_torus_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
//.........这里部分代码省略.........
开发者ID:Horisu,项目名称:jsk_recognition,代码行数:101,代码来源:torus_finder_nodelet.cpp
示例9: main
int
main (int argc, char** argv)
{
if (argc < 4)
{
std::cerr << "please specify the point cloud and the command line arg '-r' or '-c' or '-s' or '-v' or '-av' or '-rg' + param\n" << std::endl;
std::cerr << "radius filter: param ==> ray + min_neighbours " << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -r 100 10 \n" << std::endl;
std::cerr << "codnitional filter: param ==> min_dist + max_dist along the axis" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -c 0 1000 x\n" << std::endl;
std::cerr << "statistical filter: param ==> num_of_neigh + std_dev" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -s 50 1\n" << std::endl;
std::cerr << "voxel grid downsampling: param ==> leaf_size" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -v 10\n" << std::endl;
std::cerr << "voxel grid (approximated) downsampling filter: param ==> leafsize" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -av 10\n" << std::endl;
std::cerr << "region growing: param ==> point_dist" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -rg 10\n" << std::endl;
exit(0);
}
pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr cloud_filtered (new pcl::PointCloud<PointType>);
if (pcl::io::loadPCDFile (argv[1], *cloud))
return 0;
cloud->height = 1;
cloud->width = cloud->size();
cloud->is_dense=0;
do {
pcl::PointCloud<PointType>::Ptr cloud_filtered (new pcl::PointCloud<PointType>);
std::stringstream string;
size_t begin;
std::string buff;
string << "output/";
//if (system("mkdir output")) int a=0;
buff.assign(argv[1]);
// Check if the filename is a path
begin=buff.find_last_of("/\\");
if ( begin!=std::string::npos )
buff=buff.substr(begin+1);
begin=buff.find(".pcd");
if ( begin!=std::string::npos )
buff.assign ( buff,0,begin );
else {
std::cout << "No valid .pcd loaded" << std::endl;
return 0;
}
string << buff;
buff.assign(argv[2]);
begin=buff.find("-");
if ( begin!=std::string::npos )
buff.assign ( buff, begin+1, buff.length() );
else {
std::cout << "No valid method defined" << std::endl;
return 0;
}
string << "_" << buff;
buff.clear();
if (argc > 3)
buff.assign(argv[3]);
if ( buff.length() > 0 )
string << "_" << buff;
buff.clear();
if (argc > 4)
buff.assign(argv[4]);
if ( buff.length() > 0 )
string << "_" << buff;
buff.clear();
if (argc > 5)
buff.assign(argv[5]);
if ( buff.length() > 0 )
string << "_" << buff;
string << ".pcd";
if (!filterIt(argc, argv, cloud, cloud_filtered))
return 0;
//cloud_filtered->is_dense = 0;
// If ordered, remove the NaN pointsd
if (0) {
std::cout << "Cloud is organized and NaN points are now removed" << std::endl;
std::vector<int> unused;
cloud_filtered->is_dense = 0;
pcl::removeNaNFromPointCloud (*cloud_filtered, *cloud_filtered, unused);
}
//pcl::io::savePCDFile<PointType>(string.str(), cloud_filtered.operator*());
std::cerr << "Cloud points before filtering: " << cloud->points.size() << std::endl;
std::cerr << "Cloud points after filtering: " << cloud_filtered->points.size() << std::endl;
//.........这里部分代码省略.........
开发者ID:kasertim,项目名称:urban-environment-point-cloud,代码行数:101,代码来源:radiusRem.cpp
示例10: PCL_ERROR
void
pcl::RadiusOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
output.is_dense = true;
// If fields x/y/z are not present, we cannot filter
if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
{
PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
if (search_radius_ == 0.0)
{
PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
// Send the input dataset to the spatial locator
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*input_, *cloud);
// Initialize the spatial locator
if (!tree_)
{
if (cloud->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
else
tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
}
tree_->setInputCloud (cloud);
// Allocate enough space to hold the results
std::vector<int> nn_indices (indices_->size ());
std::vector<float> nn_dists (indices_->size ());
// Copy the common fields
output.is_bigendian = input_->is_bigendian;
output.point_step = input_->point_step;
output.height = 1;
output.data.resize (input_->width * input_->point_step); // reserve enough space
removed_indices_->resize (input_->data.size ());
int nr_p = 0;
int nr_removed_p = 0;
// Go over all the points and check which doesn't have enough neighbors
for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
{
int k = tree_->radiusSearch ((*indices_)[cp], search_radius_, nn_indices, nn_dists);
// Check if the number of neighbors is larger than the user imposed limit
if (k < min_pts_radius_)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step],
output.point_step);
nr_p++;
}
output.width = nr_p;
output.height = 1;
output.data.resize (output.width * output.point_step);
output.row_step = output.point_step * output.width;
removed_indices_->resize (nr_removed_p);
}
开发者ID:hitsjt,项目名称:StanfordPCL,代码行数:75,代码来源:radius_outlier_removal.cpp
示例11: main
int
main (int argc, char** argv)
{
srand (time (NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Generate pointcloud data
cloud->width = 1000;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
}
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud (cloud);
pcl::PointXYZ searchPoint;
searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
// K nearest neighbor search
int K = 10;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x
<< " " << cloud->points[ pointIdxNKNSearch[i] ].y
<< " " << cloud->points[ pointIdxNKNSearch[i] ].z
<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
std::cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << std::endl;
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].y
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].z
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}
return 0;
}
开发者ID:HSOFEUP,项目名称:pcl_samples,代码行数:74,代码来源:kdtree_search.cpp
示例12: cloud
void ImageProcessing::segEuclideanClusters(const char *filename) {
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
reader.read(filename, *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud(cloud);
vg.setLeafSize(0.01f, 0.01f, 0.01f);
vg.filter(*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
int i = 0, nr_points = (int) cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points) {
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0) {
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Get the points associated with the planar surface
extract.filter(*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.02); // 2cm
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud_filtered);
ec.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr all_clusters(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB aPoint;
int j = 0;
Color myColor;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
myColor = Color::random(); //one color for each cluster.
//adding all points of one cluster
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++) {
cloud_cluster->points.push_back(cloud_filtered->points[*pit]); //*
aPoint.x = cloud_cluster->points.back().x;
aPoint.y = cloud_cluster->points.back().y;
aPoint.z = cloud_cluster->points.back().z;
aPoint.r = myColor.red;
aPoint.g = myColor.green;
aPoint.b = myColor.blue;
all_clusters->points.push_back(aPoint);
}
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
j++;
}
//writer.write<pcl::PointXYZRGB> ("clustered_cloud.pcd", *cloud_cluster, false); //*
pcl::visualization::CloudViewer viewer("Cluster viewer");
viewer.showCloud(all_clusters);
//.........这里部分代码省略.........
开发者ID:cognitiveRobot,项目名称:albot2mapping,代码行数:101,代码来源:ImageProcessing.cpp
示例13: main
int main (int argc, char** argv)
{
if (argc != 3)
return (0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
if (pcl::io::loadPLYFile (argv[1], *cloud) == -1)
return (-1);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::io::loadPLYFile(argv[2], *cloud2);
pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
feature_extractor.setInputCloud (cloud);
feature_extractor.compute ();
std::vector <float> moment_of_inertia;
std::vector <float> eccentricity;
pcl::PointXYZ min_point_AABB;
pcl::PointXYZ max_point_AABB;
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
float major_value, middle_value, minor_value;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
Eigen::Vector3f mass_center;
feature_extractor.getMomentOfInertia (moment_of_inertia);
feature_extractor.getEccentricity (eccentricity);
feature_extractor.getAABB (min_point_AABB, max_point_AABB);
feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
feature_extractor.getEigenValues (major_value, middle_value, minor_value);
feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
feature_extractor.getMassCenter (mass_center);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (2.0, 0);
viewer->initCameraParameters ();
viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
viewer->addPointCloud<pcl::PointXYZ> (cloud2, "object cloud");
viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
Eigen::Quaternionf quat (rotational_matrix_OBB);
viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_
|
请发表评论