Concatenate the points or the fields of two Point Clouds

In this tutorial we will learn how to concatenating 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.

The code

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

 1#include <iostream>
 2#include <pcl/point_cloud.h> // for PointCloud
 3#include <pcl/point_types.h>
 4
 5int
 6  main ()
 7{
 8  pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c;
 9
10  // Fill in the cloud data
11  cloud_a.width  = 5;
12  cloud_b.width  = 3;
13  cloud_a.height = cloud_b.height = 1;
14  cloud_a.resize (cloud_a.width * cloud_a.height);
15  cloud_b.resize (cloud_b.width * cloud_b.height);
16
17  for (std::size_t i = 0; i < cloud_a.size (); ++i)
18  {
19    cloud_a[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
20    cloud_a[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
21    cloud_a[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
22  }
23
24  for (std::size_t i = 0; i < cloud_b.size (); ++i)
25  {
26    cloud_b[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
27    cloud_b[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
28    cloud_b[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
29  }
30
31  std::cerr << "Cloud A: " << std::endl;
32  for (std::size_t i = 0; i < cloud_a.size (); ++i)
33    std::cerr << "    " << cloud_a[i].x << " " << cloud_a[i].y << " " << cloud_a[i].z << std::endl;
34
35  std::cerr << "Cloud B: " << std::endl;
36  for (std::size_t i = 0; i < cloud_b.size (); ++i)
37    std::cerr << "    " << cloud_b[i].x << " " << cloud_b[i].y << " " << cloud_b[i].z << std::endl;
38
39  // Copy the point cloud data
40  cloud_c  = cloud_a;
41  cloud_c += cloud_b;
42
43  std::cerr << "Cloud C: " << std::endl;
44  for (std::size_t i = 0; i < cloud_c.size (); ++i)
45    std::cerr << "    " << cloud_c[i].x << " " << cloud_c[i].y << " " << cloud_c[i].z << " " << std::endl;
46
47  return (0);
48}

The explanation

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

In lines:

  pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c;

  // Fill in the cloud data
  cloud_a.width  = 5;
  cloud_b.width  = 3;
  cloud_a.height = cloud_b.height = 1;
  cloud_a.resize (cloud_a.width * cloud_a.height);
  cloud_b.resize (cloud_b.width * 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);
  }

  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);
  }

we define the three Point Clouds: two inputs (cloud_a and cloud_b), one output (cloud_c), and fill in the data for the two input point clouds.

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;
  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;

display the content of cloud_a and cloud_b to screen.

In line:

  // Copy the point cloud data
  cloud_c  = cloud_a;
  cloud_c += cloud_b;

we create cloud_c by concatenating the points of cloud_a and cloud_b together.

Finally:

  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;

is used to show the content of cloud_c.

Compiling and running the program

Add the following lines to your CMakeLists.txt file:

 1cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 2
 3project(concatenate_points)
 4
 5find_package(PCL 1.2 REQUIRED)
 6
 7include_directories(${PCL_INCLUDE_DIRS})
 8link_directories(${PCL_LIBRARY_DIRS})
 9add_definitions(${PCL_DEFINITIONS})
10
11add_executable (concatenate_points concatenate_points.cpp)
12target_link_libraries (concatenate_points ${PCL_LIBRARIES})

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

$ ./concatenate_points

You will see something similar to:

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