PCL: connect fields or data in two point clouds to form a new point cloud

Keywords: PCL

1. Learn how to connect two different point clouds into A point cloud. Before operation, make sure that the fields in two data sets are of the same type and the same dimension. At the same time, learn how to connect the fields of two different point clouds (such as color normals). The mandatory constraint of this operation is that the number of points in two data sets must be the same. For example, point cloud A is N point XYZ point, point cloud B is N point. For the RGB point, connect two fields to form A point cloud C is N points xyzrgb type

#include <iostream>
#include <pcl/io/pcd_io.h>      //io module 
#include <pcl/point_types.h>   //data type

int
  main (int argc, char** argv)
{
  if (argc != 2) //Prompt to enter two parameters - f or - p if the executable is executed
  {
    std::cerr << "please specify command line arg '-f' or '-p'" << std::endl;
    exit(0);
  }
  //State three pcl::PointXYZ point cloud data types, namely cloud_a, cloud_b, cloud_c
  pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c;
  //Store the Normal point cloud needed for connection, Normal (float n x, float N Y, float N Z)
  pcl::PointCloud<pcl::Normal> n_cloud_b;
   //Store the point cloud after connecting XYZ and normal
  pcl::PointCloud<pcl::PointNormal> p_n_cloud_c;

  // Create point cloud data
  //Set the number of cloud a to 5
  cloud_a.width  = 5;
  cloud_a.height = cloud_b.height = n_cloud_b.height = 1; //Set to unordered point cloud
    cloud_a.points.resize (cloud_a.width * cloud_a.height); //Total
  if (strcmp(argv[1], "-p") == 0)   //Determine whether the connection is a + B = C (point cloud connection)
  {
    cloud_b.width  = 3;
    cloud_b.points.resize (cloud_b.width * cloud_b.height);
  }
  else{
    n_cloud_b.width = 5; //If XYZ and normal are connected, 5 normals are generated (connection between fields)
    n_cloud_b.points.resize (n_cloud_b.width * n_cloud_b.height);
  }
//The following loop generates unordered point clouds to fill the two types of point cloud data defined above
  for (size_t i = 0; i < cloud_a.points.size (); ++i)
  {  //Cloud a generates three points (each point has three randomly filled values of X Y Z)
    cloud_a.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_a.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_a.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  if (strcmp(argv[1], "-p") == 0)
    for (size_t i = 0; i < cloud_b.points.size (); ++i)
    {   //If a+b=c is connected, Cloud B uses three points as xyz data
      cloud_b.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }
  else
    for (size_t i = 0; i < n_cloud_b.points.size (); ++i)
    {  //If xyz+normal=xyznormal is connected, then n Cloud B uses 5 points as normal data
      n_cloud_b.points[i].normal[0] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b.points[i].normal[1] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b.points[i].normal[2] = 1024 * rand () / (RAND_MAX + 1.0f);
    }
/*******************************************************************
Five point cloud objects used to connect point cloud are defined: three inputs (cloud? A cloud? B and N cloud? B)
Two outputs (cloud a and Cloud B or cloud a and N Cloud B) are then populated with data for two input points cloud a and Cloud B  

********************************************************************/
//Output Cloud A
  std::cerr << "Cloud A: " << std::endl;
  for (size_t i = 0; i < cloud_a.points.size (); ++i)
    std::cerr << "    " << cloud_a.points[i].x << " " << cloud_a.points[i].y << " " << cloud_a.points[i].z << std::endl;
//Output Cloud B
  std::cerr << "Cloud B: " << std::endl;
  if (strcmp(argv[1], "-p") == 0)
    for (size_t i = 0; i < cloud_b.points.size (); ++i)
      std::cerr << "    " << cloud_b.points[i].x << " " << cloud_b.points[i].y << " " << cloud_b.points[i].z << std::endl;
  else//Output n Cloud B
    for (size_t i = 0; i < n_cloud_b.points.size (); ++i)
      std::cerr << "    " << n_cloud_b.points[i].normal[0] << " " << n_cloud_b.points[i].normal[1] << " " << n_cloud_b.points[i].normal[2] << std::endl;

  // Copy the point cloud data
  if (strcmp(argv[1], "-p") == 0)
  {
    cloud_c  = cloud_a;
    cloud_c += cloud_b;//Connect cloud a and Cloud B to create cloud C and output
    std::cerr << "Cloud C: " << std::endl;
    for (size_t i = 0; i < cloud_c.points.size (); ++i)
      std::cerr << "    " << cloud_c.points[i].x << " " << cloud_c.points[i].y << " " << cloud_c.points[i].z << " " << std::endl;
  }
  else
  {  //Connect fields connect cloud a and N Cloud B fields to create P cloud C)
    pcl::concatenateFields (cloud_a, n_cloud_b, p_n_cloud_c);
    std::cerr << "Cloud C: " << std::endl;
    for (size_t i = 0; i < p_n_cloud_c.points.size (); ++i)
      std::cerr << "    " <<
        p_n_cloud_c.points[i].x << " " << p_n_cloud_c.points[i].y << " " << p_n_cloud_c.points[i].z << " " <<
        p_n_cloud_c.points[i].normal[0] << " " << p_n_cloud_c.points[i].normal[1] << " " << p_n_cloud_c.points[i].normal[2] << std::endl;
  }
  return (0);
}

Posted by Sware on Fri, 25 Oct 2019 10:14:46 -0700