Code Explained

Basics

std::vector<PointT, Eigen::aligned_allocator<PointT> > points;

• pcl::PointCloud<PointT> uses std::vector internally as the container to store points, and applies the aligned allocator that Eigen inherited from std::allocator.

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

• pcl::XX::Ptr uses shared_ptr internally as the smart pointer to keep track of the number of references and automatically manage the memory. The shared_ptr is a class essentially.

• pcl::XX::Ptr cloud (new XX) takes the pointer created by new and passes to the constructor as an argument. The constructor also initializes other variables such as the one used to keep track of the number of references.

pcl::XX::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::XX::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>() );

• new Thing(); is explicit that you want a constructor called whereas new Thing; is taken to imply you don't mind if the constructor isn't called. See this reference page and this example.

• For example, new Thing(); may initialize all values to 0, but new Thing; may not.

// correct
pcl::PointCloud<pcl::PointXYZRGB> source_cloud_transformed;
sac_ia.align (source_cloud_transformed);

// incorrect
pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_cloud_transformed (new pcl::PointCloud<pcl::PointXYZRGB>);
sac_ia.align (source_cloud_transformed);

// because align function takes in only raw pointers rather than smart pointers

• In this example, new should not be used, since we want to leave the constructor to the align function. Otherwise the compilation error is the following.

error: no matching function for call to ‘pcl::SampleConsensusInitialAlignment<pcl::PointXYZRGB, pcl::PointXYZRGB, pcl::FPFHSignature33>::align(pcl::PointCloud<pcl::PointXYZRGB>::Ptr&)’

sac_ia.align (source_cloud_transformed);

no known conversion for argument 1 from ‘pcl::PointCloud<pcl::PointXYZRGB>::Ptr {aka boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >}’ to ‘pcl::Registration<pcl::PointXYZRGB, pcl::PointXYZRGB, float>::PointCloudSource& {aka pcl::PointCloud<pcl::PointXYZRGB>&}’

Correspondence Estimation & KdTreeFLANN

// pcl/registration/correspondence_estimation.h
typename pcl::search::KdTree<PointTarget>::Ptr tree_;
CorrespondenceEstimation::determineCorrespondences ()
{
  tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
}

// pcl/search/kdtree.h & pcl/search/impl/kdtree.hpp
template<typename PointT, class Tree = pcl::KdTreeFLANN<PointT> >
class KdTree: public Search<PointT>
boost::shared_ptr<Tree> tree_;
int nearestKSearch ()
{
  return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances));
}

// pcl/kdtree/kdtree_flann.h & pcl/kdtree/impl/kdtree_flann.hpp
template <typename PointT, typename Dist = ::flann::L2_Simple<float> >
class KdTreeFLANN : public pcl::KdTree<PointT>
typedef ::flann::Index<Dist> FLANNIndex;
boost::shared_ptr<FLANNIndex> flann_index_;
int nearestKSearch ()
{
  flann_index_->knnSearch (::flann::Matrix<float> (&query[0], 1, dim_), 
                           k_indices_mat, k_distances_mat,
                           k, param_k_);
}

// flann/flann.hpp
template<typename Distance>
class Index
typedef NNIndex<Distance> IndexType;
IndexType* nnIndex_;
int knnSearch()
{
  return nnIndex_->knnSearch(queries, indices, dists, knn, params);
}

// flann/algorithms/nn_index.h
template <typename Distance>
class NNIndex : public IndexBase
int knnSearch()
{
  // actual implementation using OpenMP
  // wrapped by functions and compilation directives
}

• The default precision used in FLANN is 0.8, according to the autotuned parameter settings here.

KdTree Point Representation

PCL uses the PointRepresentation class as a middle layer to convert various point types (including common ones as well as point types created for feature descriptors) into unified float vectors, to pass into the Kd-tree.

For example, the PointXYZRGBA point type can have 3 float fields (12 bytes) for 3D position, and 4 unit8 fields (4 bytes) for RGBA color channels. However, KdTree can only accept uniform vectors with all fields to be float, and will measure distance equally in each dimension. Therefore, there is need to adapt the two interfaces. By default, it will only take the first 3 dimensions/fields in the point type (the 3D position of the point).

If there is need to incorporate more fields into the kdtree search, then we need to implement a customized PointRepresentation class for this purpose. For example for the XYZRGB point type, how to map from uint8 to float, how to normalize the range, and how to assign weights to distance measure need to be considered carefully. In addition, it might be good to implement a customized distance measure as needed, because by default the kdtree will use L2_Simple for input float vectors.

// pcl/point_representation.h
template <typename PointT>
class PointRepresentation {
  protected:
    int nr_dimensions_;
    std::vector<float> alpha_;
    bool trivial_;
  public:
    PointRepresentation () : nr_dimensions_ (0), alpha_ (0), trivial_ (false) {}
    virtual bool 
      isValid (const PointT &p) const
    template <typename OutputType> void 
      vectorize (const PointT &p, OutputType &out) const
};

// extends PointRepresentation to define default behavior for common point types.
template <typename PointDefault>
class DefaultPointRepresentation : public PointRepresentation <PointDefault> {
  DefaultPointRepresentation () {
    nr_dimensions_ = sizeof (PointDefault) / sizeof (float);
    if (nr_dimensions_ > 3) nr_dimensions_ = 3;
    trivial_ = true;
  }
};

// intended to be used when defining the default behavior for feature descriptor
// types (i.e., copy each element of each field into a float array).
template <typename PointDefault>
class DefaultFeatureRepresentation : public PointRepresentation <PointDefault>

// extends PointRepresentation to allow for sub-part selection on the point.
template <typename PointDefault>
class CustomPointRepresentation : public PointRepresentation <PointDefault>

In order to use customized point representation, we need to replace point_representation_ in KdTreeFLANN with our customized one. By default it is set to DefaultPointRepresentation initialized in its parent KdTree class.

// pcl/search/kdtree.h
template<typename PointT, class Tree = pcl::KdTreeFLANN<PointT> >
class KdTree: public Search<PointT>

// pcl/kdtree/kdtree_flann.h
template <typename PointT, typename Dist = ::flann::L2_Simple<float> >
class KdTreeFLANN : public pcl::KdTree<PointT> {
  using KdTree<PointT>::point_representation_;
};

// pcl/kdtree/kdtree.h
template <typename PointT>
class KdTree {
  KdTree (bool sorted = true) : input_(), indices_(), 
    epsilon_(0.0f), min_pts_(1), sorted_(sorted), 
    point_representation_ (new DefaultPointRepresentation<PointT>) {};
};

KdTree with Customized Distance Measure

By default, PCL has only compiled L2_Simple as the distance measure, and set a header guard PCL_NO_PRECOMPILE to avoid the recompilation of the hpp files.

// in file pcl/search/kdtree.h
// ...
#ifdef PCL_NO_PRECOMPILE
#include <pcl/search/impl/kdtree.hpp>  // will be skipped by default
#else
#define PCL_INSTANTIATE_KdTree(T) template class PCL_EXPORTS pcl::search::KdTree<T>;
#endif

In this case, using distance measure other than the default L2_Simple will cause the following error.

undefined reference to `pcl::search::KdTree<pcl::PointXYZRGBNormal, pcl::KdTreeFLANN<pcl::PointXYZRGBNormal, flann::L1<float> > >::KdTree(bool)'

The solution is to enforce the compilation of the hpp file again.

// to use pcl::Search::KdTree with L1 distance
#include <pcl/search/impl/kdtree.hpp>
pcl::search::KdTree<PointT, pcl::KdTreeFLANN<PointT, flann::L1<float>>>::Ptr kdtree
  (new pcl::search::KdTree<PointT, pcl::KdTreeFLANN<PointT, flann::L1<float>>>);

// to use pcl::KdTreeFLANN with L1 distance
#include <pcl/kdtree/impl/kdtree_flann.hpp>
boost::shared_ptr<pcl::KdTreeFLANN<FeatureT, flann::L1<float> > > feature_tree 
  (new pcl::KdTreeFLANN<FeatureT, flann::L1<float> >);

Point Cloud Transformation

pcl::transformPointCloudWithNormals (*source_cloud, *source_cloud_transformed, transformation);

Recall that one of the use cases of transformation matrices is to change the reference frame. Suppose that point $p$ is fixed in space. Given $p_1$and $p_2$in frame 1 and frame 2 respectively, we have the relation $p_2 = T_{21} p_1$, where $T_{21}$is the transformation matrix between frame 1 and frame 2 (in this case, indicating the configuration of frame 1 with respect to frame 2).

Note that when we transform a point cloud, we actually apply the transformation matrix to each and every point in the cloud (not the origin, not the reference frame). Therefore, it follows the aforementioned relation $p_2 = T_{21} p_1$.

Accordingly in PCL, when we use pcl::transformPointCloud , the transformation matrix being used is actually $T_{21}$(not $T_{12}$), though with this matrix we can obtain the new point cloud 2 from point cloud 1. It is important to keep in mind this subscript, especially when subscript cancellation is needed in the following steps.