Concatenate the points of two Point Clouds

In this tutorial we will learn how to concatenate the points of two different point clouds. The constraint imposed here is that the type and number of fields in the two datasets have to be equal. We will also learn how to concatenate the fields (e.g., dimensions) of two different point clouds. The constraint imposed here is that the number of points in the two datasets has to be equal.

The code

First, create a file, let’s say, concatenate_clouds.cpp in your favorite editor, and place the following code inside it:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

int
  main (int argc, char** argv)
{
  if (argc != 2)
  {
    std::cerr << "please specify command line arg '-f' or '-p'" << std::endl;
    exit(0);
  }
  pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c;
  pcl::PointCloud<pcl::Normal> n_cloud_b;
  pcl::PointCloud<pcl::PointNormal> p_n_cloud_c;

  // Fill in the cloud data
  cloud_a.width  = 5;
  cloud_a.height = cloud_b.height = n_cloud_b.height = 1;
    cloud_a.points.resize (cloud_a.width * cloud_a.height);
  if (strcmp(argv[1], "-p") == 0)
  {
    cloud_b.width  = 3;
    cloud_b.points.resize (cloud_b.width * cloud_b.height);
  }
  else{
    n_cloud_b.width = 5;
    n_cloud_b.points.resize (n_cloud_b.width * n_cloud_b.height);
  }

  for (std::size_t i = 0; i < cloud_a.size (); ++i)
  {
    cloud_a[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_a[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_a[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  if (strcmp(argv[1], "-p") == 0)
    for (std::size_t i = 0; i < cloud_b.size (); ++i)
    {
      cloud_b[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }
  else
    for (std::size_t i = 0; i < n_cloud_b.size (); ++i)
    {
      n_cloud_b[i].normal[0] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b[i].normal[1] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b[i].normal[2] = 1024 * rand () / (RAND_MAX + 1.0f);
    }
  std::cerr << "Cloud A: " << std::endl;
  for (std::size_t i = 0; i < cloud_a.size (); ++i)
    std::cerr << "    " << cloud_a[i].x << " " << cloud_a[i].y << " " << cloud_a[i].z << std::endl;

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

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

The explanation

Now, let’s break down the code piece by piece.

In lines:

  pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c;
  pcl::PointCloud<pcl::Normal> n_cloud_b;
  pcl::PointCloud<pcl::PointNormal> p_n_cloud_c;

  // Fill in the cloud data
  cloud_a.width  = 5;
  cloud_a.height = cloud_b.height = n_cloud_b.height = 1;
    cloud_a.points.resize (cloud_a.width * cloud_a.height);
  if (strcmp(argv[1], "-p") == 0)
  {
    cloud_b.width  = 3;
    cloud_b.points.resize (cloud_b.width * cloud_b.height);
  }
  else{
    n_cloud_b.width = 5;
    n_cloud_b.points.resize (n_cloud_b.width * n_cloud_b.height);
  }

  for (std::size_t i = 0; i < cloud_a.size (); ++i)
  {
    cloud_a[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_a[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_a[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  if (strcmp(argv[1], "-p") == 0)
    for (std::size_t i = 0; i < cloud_b.size (); ++i)
    {
      cloud_b[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }
  else
    for (std::size_t i = 0; i < n_cloud_b.size (); ++i)
    {
      n_cloud_b[i].normal[0] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b[i].normal[1] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b[i].normal[2] = 1024 * rand () / (RAND_MAX + 1.0f);
    }

we define the five Point Clouds for use in concatenating clouds: three inputs (cloud_a, cloud_b and n_cloud_b), two outputs (cloud_c and p_n_cloud_c). Then we fill in the data for the two input point clouds we are using (for points cloud_a and cloud_b, for fields cloud_a and n_cloud_b).

Then, lines:

  std::cerr << "Cloud A: " << std::endl;
  for (std::size_t i = 0; i < cloud_a.size (); ++i)
    std::cerr << "    " << cloud_a[i].x << " " << cloud_a[i].y << " " << cloud_a[i].z << std::endl;

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

display the content of cloud_a and either cloud_b or n_cloud_b (depending on the command line argument) to screen.

If we are trying to concatenate points then the code below:

    cloud_c  = cloud_a;
    cloud_c += cloud_b;

creates cloud_c by concatenating the points of cloud_a and cloud_b together.

Otherwise if we are attempting to concatenate fields then the code below:

    pcl::concatenateFields (cloud_a, n_cloud_b, p_n_cloud_c);

creates p_n_cloud_c by concatenating the fields of cloud_a and cloud_b together.

Finally either:

    std::cerr << "Cloud C: " << std::endl;
    for (std::size_t i = 0; i < cloud_c.size (); ++i)
      std::cerr << "    " << cloud_c[i].x << " " << cloud_c[i].y << " " << cloud_c[i].z << " " << std::endl;

or

    std::cerr << "Cloud C: " << std::endl;
    for (std::size_t i = 0; i < p_n_cloud_c.size (); ++i)
      std::cerr << "    " <<
        p_n_cloud_c[i].x << " " << p_n_cloud_c[i].y << " " << p_n_cloud_c[i].z << " " <<
        p_n_cloud_c[i].normal[0] << " " << p_n_cloud_c[i].normal[1] << " " << p_n_cloud_c[i].normal[2] << std::endl;

is used to show the content of cloud_c or p_n_cloud_c to the screen depending on if we concatenated the points or fields of the PointClouds.

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(concatenate_clouds)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (concatenate_clouds concatenate_clouds.cpp)
target_link_libraries (concatenate_clouds ${PCL_LIBRARIES})

After you have made the executable, you can run it. Simply do:

$ ./concatenate_clouds -p

to concatenate points or do:

$ ./concatenate_clouds -f

to concatenate fields.

You will see something similar to if concatenating points:

Cloud A:
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762
Cloud B:
    0.183749 0.968809 0.512055
    -0.998983 -0.463871 0.691785
    0.716053 0.525135 -0.523004
Cloud C:
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762
    0.183749 0.968809 0.512055
    -0.998983 -0.463871 0.691785
    0.716053 0.525135 -0.523004

and similar to this if concatenating fields:

Cloud A:
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762
Cloud B:
    0.183749 0.968809 0.512055
    -0.998983 -0.463871 0.691785
    0.716053 0.525135 -0.523004
    0.439387 0.56706 0.905417
    -0.579787 0.898706 -0.504929
Cloud C:
    0.352222 -0.151883 -0.106395 0.183749 0.968809 0.512055
    -0.397406 -0.473106 0.292602 -0.998983 -0.463871 0.691785
    -0.731898 0.667105 0.441304 0.716053 0.525135 -0.523004
    -0.734766 0.854581 -0.0361733 0.439387 0.56706 0.905417
    -0.4607 -0.277468 -0.916762 -0.579787 0.898706 -0.504929