Adding your own custom PointT type — Point Cloud Library 0.0 documentation (original) (raw)

The current document explains not only how to add your own PointT point type, but also what templated point types are in PCL, why do they exist, and how are they exposed. If you’re already familiar with this information, feel free to skip to the last part of the document.

Contents

Note

The current document is valid only for PCL 0.x and 1.x. Note that at the time of this writing we are expecting things to be changed in PCL 2.x.

PCL comes with a variety of pre-defined point types, ranging from SSE-aligned structures for XYZ data, to more complex n-dimensional histogram representations such as PFH (Point Feature Histograms). These types should be enough to support all the algorithms and methods implemented in PCL. However, there are cases where users would like to define new types. This document describes the steps involved in defining your own custom PointT type and making sure that your project can be compiled successfully and ran.

Why PointT types

PCL’s PointT legacy goes back to the days where it was a library developed within ROS. The consensus then was that a _Point Cloud_is a complicated n-D structure that needs to be able to represent different types of information. However, the user should know and understand what types of information need to be passed around, in order to make the code easier to debug, think about optimizations, etc.

One example is represented by simple operations on XYZ data. The most efficient way for SSE-enabled processors, is to store the 3 dimensions as floats, followed by an extra float for padding:

1struct PointXYZ 2{ 3 float x; 4 float y; 5 float z; 6 float padding; 7};

As an example however, in case an user is looking at compiling PCL on an embedded platform, adding the extra padding can be a waste of memory. Therefore, a simpler PointXYZ structure without the last float could be used instead.

Moreover, if your application requires a PointXYZRGBNormal which containsXYZ 3D data, RGBA information (colors), and surface normals estimated at each point, it is trivial to define a structure with all the above. Since all algorithms in PCL should be templated, there are no other changes required other than your structure definition.

What PointT types are available in PCL?

To cover all possible cases that we could think of, we defined a plethora of point types in PCL. The following might be only a snippet, please seepoint_types.hppfor the complete list.

This list is important, because before defining your own custom type, you need to understand why the existing types were created they way they were. In addition, the type that you want, might already be defined for you.

union { float data[4]; struct { float x; float y; float z; }; };

union { float data[4]; struct { float x; float y; float z; }; }; union { struct { float intensity; }; float data_c[4]; };

Note

The nested union declaration provides yet another way to look at the RGBA data–as a single precision floating point number. This is present for historical reasons and should not be used in new code.

union { float data[4]; struct { float x; float y; float z; }; }; union { union { struct { std::uint8_t b; std::uint8_t g; std::uint8_t r; std::uint8_t a; }; float rgb; }; std::uint32_t rgba; };

struct { float x; float y; };

union { float data[4]; struct { float x; float y; float z; }; }; union { struct { float strength; }; float data_c[4]; };

union { float data_n[4]; float normal[3]; struct { float normal_x; float normal_y; float normal_z; }; } union { struct { float curvature; }; float data_c[4]; };

union { float data[4]; struct { float x; float y; float z; }; }; union { float data_n[4]; float normal[3]; struct { float normal_x; float normal_y; float normal_z; }; }; union { struct { float curvature; }; float data_c[4]; };

Note

Despite the name, this point type does contain the alpha color channel.

union { float data[4]; struct { float x; float y; float z; }; }; union { float data_n[4]; float normal[3]; struct { float normal_x; float normal_y; float normal_z; }; } union { struct { union { union { struct { std::uint8_t b; std::uint8_t g; std::uint8_t r; std::uint8_t a; }; float rgb; }; std::uint32_t rgba; }; float curvature; }; float data_c[4]; };

union { float data[4]; struct { float x; float y; float z; }; }; union { float data_n[4]; float normal[3]; struct { float normal_x; float normal_y; float normal_z; }; } union { struct { float intensity; float curvature; }; float data_c[4]; };

union { float data[4]; struct { float x; float y; float z; }; }; union { struct { float range; }; float data_c[4]; };

union { float data[4]; struct { float x; float y; float z; }; }; union { struct { float vp_x; float vp_y; float vp_z; }; float data_c[4]; };

struct { float j1, j2, j3; };

struct { float r_min, r_max; };

struct { std::uint8_t boundary_point; };

struct { union { float principal_curvature[3]; struct { float principal_curvature_x; float principal_curvature_y; float principal_curvature_z; }; }; float pc1; float pc2; };

struct { float histogram[125]; };

struct { float histogram[33]; };

struct { float histogram[308]; };

struct { float x, y, z, roll, pitch, yaw; float descriptor[36]; };

struct { int x, y; BorderTraits traits; };

struct { union { float gradient[3]; struct { float gradient_x; float gradient_y; float gradient_z; }; }; };

template struct Histogram { float histogram[N]; };

struct { union { float data[4]; struct { float x; float y; float z; }; }; float scale; };

union { float data[4]; struct { float x; float y; float z; }; }; union { float data_n[4]; float normal[3]; struct { float normal_x; float normal_y; float normal_z; }; }; union { struct { std::uint32_t rgba; float radius; float confidence; float curvature; }; float data_c[4]; };

How are the point types exposed?

Because of its large size, and because it’s a template library, including many PCL algorithms in one source file can slow down the compilation process. At the time of writing this document, most C++ compilers still haven’t been properly optimized to deal with large sets of templated files, especially when optimizations (-O2 or -O3) are involved.

To speed up user code that includes and links against PCL, we are using_explicit template instantiations_, by including all possible combinations in which all algorithms could be called using the already defined point types from PCL. This means that once PCL is compiled as a library, any user code will not require to compile templated code, thus speeding up compile time. The trick involves separating the templated implementations from the headers which forward declare our classes and methods, and resolving at link time. Here’s a fictitious example:

1// foo.h 2 3#ifndef PCL_FOO_ 4#define PCL_FOO_ 5 6template 7class Foo 8{ 9 public: 10 void 11 compute (const pcl::PointCloud &input, 12 pcl::PointCloud &output); 13} 14 15#endif // PCL_FOO_

The above defines the header file which is usually included by all user code. As we can see, we’re defining methods and classes, but we’re not implementing anything yet.

1// impl/foo.hpp 2 3#ifndef PCL_IMPL_FOO_ 4#define PCL_IMPL_FOO_ 5 6#include "foo.h" 7 8template void 9Foo::compute (const pcl::PointCloud &input, 10 pcl::PointCloud &output) 11{ 12 output = input; 13} 14 15#endif // PCL_IMPL_FOO_

The above defines the actual template implementation of the methodFoo::compute. This should normally be hidden from user code.

1// foo.cpp 2 3#include "pcl/point_types.h" 4#include "pcl/impl/instantiate.hpp" 5#include "foo.h" 6#include "impl/foo.hpp" 7 8// Instantiations of specific point types 9PCL_INSTANTIATE(Foo, PCL_XYZ_POINT_TYPES));

And finally, the above shows the way the explicit instantiations are done in PCL. The macro PCL_INSTANTIATE does nothing else but go over a given list of types and creates an explicit instantiation for each. From pcl/include/pcl/impl/instantiate.hpp:

// PCL_INSTANTIATE: call to instantiate template TEMPLATE for all // POINT_TYPES

#define PCL_INSTANTIATE_IMPL(r, TEMPLATE, POINT_TYPE)
BOOST_PP_CAT(PCL_INSTANTIATE_, TEMPLATE)(POINT_TYPE)

#define PCL_INSTANTIATE(TEMPLATE, POINT_TYPES)
BOOST_PP_SEQ_FOR_EACH(PCL_INSTANTIATE_IMPL, TEMPLATE, POINT_TYPES);

Where PCL_XYZ_POINT_TYPES is (from pcl/include/pcl/impl/point_types.hpp):

// Define all point types that include XYZ data #define PCL_XYZ_POINT_TYPES
(pcl::PointXYZ)
(pcl::PointXYZI)
(pcl::PointXYZRGBA)
(pcl::PointXYZRGB)
(pcl::InterestPoint)
(pcl::PointNormal)
(pcl::PointXYZRGBNormal)
(pcl::PointXYZINormal)
(pcl::PointWithRange)
(pcl::PointWithViewpoint)
(pcl::PointWithScale)

Basically, if you only want to explicitly instantiate Foo forpcl::PointXYZ, you don’t need to use the macro, as something as simple as the following would do:

1// foo.cpp 2 3#include "pcl/point_types.h" 4#include "pcl/impl/instantiate.hpp" 5#include "foo.h" 6#include "impl/foo.hpp" 7 8template class Foopcl::PointXYZ;

Note

For more information about explicit instantiations, please see C++ Templates - The Complete Guide, by David Vandervoorde and Nicolai M. Josuttis.

How to add a new PointT type

To add a new point type, you first have to define it. For example:

1struct MyPointType 2{ 3 float test; 4};

Then, you need to make sure your code includes the template header implementation of the specific class/algorithm in PCL that you want your new point type MyPointType to work with. For example, say you want to usepcl::PassThrough. All you would have to do is:

#define PCL_NO_PRECOMPILE #include <pcl/filters/passthrough.h> #include <pcl/filters/impl/passthrough.hpp>

// the rest of the code goes here

If your code is part of the library, which gets used by others, it might also make sense to try to use explicit instantiations for your MyPointType types, for any classes that you expose (from PCL our outside PCL).

Note

Starting with PCL-1.7 you need to define PCL_NO_PRECOMPILE before you include any PCL headers to include the templated algorithms as well.

Note

The invocation of POINT_CLOUD_REGISTER_POINT_STRUCT must be in the global namespace and the name of the new point type must be fully qualified.

Example

The following code snippet example creates a new point type that contains XYZ data (SSE padded), together with a test float.

1#define PCL_NO_PRECOMPILE 2#include <pcl/memory.h> 3#include <pcl/pcl_macros.h> 4#include <pcl/point_types.h> 5#include <pcl/point_cloud.h> 6#include <pcl/io/pcd_io.h> 7 8struct EIGEN_ALIGN16 MyPointType // enforce SSE padding for correct memory alignment 9{ 10 PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding 11 float test; 12 PCL_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 13}; 14 15POINT_CLOUD_REGISTER_POINT_STRUCT (MyPointType, // here we assume a XYZ + "test" (as fields) 16 (float, x, x) 17 (float, y, y) 18 (float, z, z) 19 (float, test, test) 20) 21 22 23int 24main (int argc, char** argv) 25{ 26 pcl::PointCloud cloud; 27 cloud.resize (2); 28 cloud.width = 2; 29 cloud.height = 1; 30 31 cloud[0].test = 1; 32 cloud[1].test = 2; 33 cloud[0].x = cloud[0].y = cloud[0].z = 0; 34 cloud[1].x = cloud[1].y = cloud[1].z = 3; 35 36 pcl::io::savePCDFile ("test.pcd", cloud); 37}