PCL Series--How to gradually register a pair of point clouds

Welcome to visit our blog PCL Series

PCL Series--read in PCD format File Operation PCL Series--writing point cloud Data to PCD format File PCL series--stitching two point clouds PCL series--extracting Narf key points from depth image (Rangeimage) PCL series--how to visualize depth image PCL series-- How to use the iterative nearest point method (ICP) Registration PCL Series--How to gradually register a pair of point clouds PCL series--three-dimensional reconstruction of Poisson reconstruction PCL series--three-dimensional reconstruction of greedy triangular projection algorithm PCL series--three-dimensional reconstruction of moving cube algorithm 

description

In this tutorial, we will learn how to match multiple point cloud images. The registration method is: Point cloud 22 registration, calculate their transformation matrix, and then calculate the total transformation matrix. The two point cloud registration uses the non-linear ICP algorithm, which is a variant of the ICP algorithm, using Levenberg-marquardt optimization. 

Action

Create a new file Pairwise_incremental_registration.cpp in VS2010, and then copy the following code to the file. Refer to the previous article to configure the properties of the item. Sets the Include directory and Library directory and additional dependencies.

#include <boost / make_shared.hpp> // shared pointer
// point / point cloud
#include <pcl / point_types.h>
#include <pcl / point_cloud.h>
#include <pcl / point_representation.h>
// pcd file input / output
#include <pcl / io / pcd_io.h>
// Filtering
#include <pcl / filters / voxel_grid.h>
#include <pcl / filters / filter.h>
//feature
#include <pcl / features / normal_3d.h>
// Registration
#include <pcl / registration / icp.h> // ICP method
#include <pcl / registration / icp_nl.h>
#include <pcl / registration / transforms.h>
// Visualization
#include <pcl / visualization / pcl_visualizer.h>

//Namespaces
using pcl :: visualization :: PointCloudColorHandlerGenericField;
using pcl :: visualization :: PointCloudColorHandlerCustom;

// define type alias
typedef pcl :: PointXYZ PointT;
typedef pcl :: PointCloud <PointT> PointCloud;
typedef pcl :: PointNormal PointNormalT;
typedef pcl :: PointCloud <PointNormalT> PointCloudWithNormals;

// Global variables
// Visualization object
pcl :: visualization :: PCLVisualizer * p;
// Left viewport and right viewport, the visualization window is divided into left and right parts
int vp_1, vp_2;

// Define a structure for processing point clouds
struct PCD
{
  PointCloud :: Ptr cloud; // Point cloud pointer
  std :: string f_name; // File name
    //Constructor
  PCD (): cloud (new PointCloud) (); // initialize
};


// define a new point expression <x, y, z, curvature> coordinate + curvature
class MyPointRepresentation: public pcl :: PointRepresentation <PointNormalT> // Inheritance
{
  using pcl :: PointRepresentation <PointNormalT> :: nr_dimensions_;
public:
  MyPointRepresentation ()
  {
    // Specify the number of dimensions
    nr_dimensions_ = 4;
  }

  // Overload function copyToFloatArray to define your own feature vector
  virtual void copyToFloatArray (const PointNormalT & p, float * out) const
  {
    // <x, y, z, curvature> coordinates xyz and curvature
    out [0] = p.x;
    out [1] = p.y;
    out [2] = p.z;
    out [3] = p.curvature;
  }
};



// In the left view of the window, simply display the source point cloud and target point cloud
void showCloudsLeft (const PointCloud :: Ptr cloud_target, const PointCloud :: Ptr cloud_source)
{
  p-> removePointCloud ("vp1_target"); // Remove a point cloud from the screen based on the given ID. Parameter is ID
  p-> removePointCloud ("vp1_source"); //
  PointCloudColorHandlerCustom <PointT> tgt_h (cloud_target, 0, 255, 0); // Target point cloud green
  PointCloudColorHandlerCustom <PointT> src_h (cloud_source, 255, 0, 0); // source point cloud red
  p-> addPointCloud (cloud_target, tgt_h, "vp1_target", vp_1); // Load point cloud
  p-> addPointCloud (cloud_source, src_h, "vp1_source", vp_1);
  PCL_INFO ("Press q to begin the registration. \ N"); // Show prompt information on the command line
  p-> spin ();
}


// In the right view of the window, simply display the source point cloud and target point cloud
void showCloudsRight (const PointCloudWithNormals :: Ptr cloud_target, const PointCloudWithNormals :: Ptr cloud_source)
{
  p-> removePointCloud ("source"); // Remove a point cloud from the screen based on the given ID. Parameter is ID
  p-> removePointCloud ("target");
  PointCloudColorHandlerGenericField <PointNormalT> tgt_color_handler (cloud_target, "curvature"); // Target point cloud color handle
  if (! tgt_color_handler.isCapable ())
      PCL_WARN ("Cannot create curvature color handler!");
  PointCloudColorHandlerGenericField <PointNormalT> src_color_handler (cloud_source, "curvature"); // source point cloud color handle
  if (! src_color_handler.isCapable ())
      PCL_WARN ("Cannot create curvature color handler!");
  p-> addPointCloud (cloud_target, tgt_color_handler, "target", vp_2); // Load the point cloud
  p-> addPointCloud (cloud_source, src_color_handler, "source", vp_2);
  p-> spinOnce ();
}


// Read a series of PCD files (hopefully registered point cloud files)
// number of argc parameters (from main ())
// list of argv parameters (from main ())
// result vector of parameter model point cloud dataset
void loadData (int argc, char ** argv, std :: vector <PCD, Eigen :: aligned_allocator <PCD>> & models)
{
  std :: string extension (".pcd"); // Declares and initializes the string type extension, which indicates the file extension name
  // read the pcd file by traversing the file name
  for (int i = 1; i <argc; i ++) // traverse all file names (skip program names)
  {
    std :: string fname = std :: string (argv [i]);
    if (fname.size () <= extension.size ()) // Whether the length of the file name meets the requirements
      continue;

    std :: transform (fname.begin (), fname.end (), fname.begin (), (int (*) (int)) tolower); // apply an operation (lowercase) to the specified range Each element
    // Check if the file is a pcd file
    if (fname.compare (fname.size ()-extension.size (), extension.size (), extension) == 0)
    {
      // read the point cloud and save it to models
      PCD m;
      m.f_name = argv [i];
      pcl :: io :: loadPCDFile (argv [i], * m.cloud); // Read point cloud data
      // Remove NaN points in the point cloud (xyz are NaN)
      std :: vector <int> indices; // Save the index of the removed point
      pcl :: removeNaNFromPointCloud (* m.cloud, * m.cloud, indices); // Remove NaN points in the point cloud

      models.push_back (m);
    }
  }
}


// simply register a pair of point cloud data and return the result
// parameter cloud_src source point cloud
// parameter cloud_tgt target point cloud
// parameter output point cloud
// parameter final_transform pairwise transformation matrix
// Parameter downsample whether to downsample
void pairAlign (const PointCloud :: Ptr cloud_src, const PointCloud :: Ptr cloud_tgt, PointCloud :: Ptr output, Eigen :: Matrix4f & final_transform, bool downsample = false)
{
  //
  // For consistency and speed, downsampling
  // \ note enable this for large datasets
  PointCloud :: Ptr src (new PointCloud); // Create Point Cloud Pointer
  PointCloud :: Ptr tgt (new PointCloud);
  pcl :: VoxelGrid <PointT> grid; // VoxelGrid gathers a given point cloud on a local 3D grid, and downsamples and filters the point cloud data
  if (downsample) // downsampling
  {
    grid.setLeafSize (0.05, 0.05, 0.05); // Set the leaf size of the voxel grid
        // downsampling source point cloud
    grid.setInputCloud (cloud_src); // Set the input point cloud
    grid.filter (* src); // Downsampling and filtering, and stored in src
        // downsampling target point cloud
    grid.setInputCloud (cloud_tgt);
    grid.filter (* tgt);
  }
  else // No downsampling
  {
    src = cloud_src; // Copy directly
    tgt = cloud_tgt;
  }

  // Calculate the normal and curvature of the surface
  PointCloudWithNormals :: Ptr points_with_normals_src (new PointCloudWithNormals); // Create a source point cloud pointer (note that the type of points includes coordinates and normal vectors)
  PointCloudWithNormals :: Ptr points_with_normals_tgt (new PointCloudWithNormals); // Create a target point cloud pointer (note that the type of point contains coordinates and normal vectors)
  pcl :: NormalEstimation <PointT, PointNormalT> norm_est; // This object is used to calculate the normal vector
  pcl :: search :: KdTree <pcl :: PointXYZ> :: Ptr tree (new pcl :: search :: KdTree <pcl :: PointXYZ> ()); // Create a kd tree, a search method for calculating normal vectors
  norm_est.setSearchMethod (tree); // Set search method
  norm_est.setKSearch (30); // Set the number of nearest neighbors
  norm_est.setInputCloud (src); // Set the input cloud
  norm_est.compute (* points_with_normals_src); // Calculate normal vector and store it in points_with_normals_src
  pcl :: copyPointCloud (* src, * points_with_normals_src); // Copy the point cloud (coordinates) to points_with_normals_src (including coordinates and normal vectors)
  norm_est.setInputCloud (tgt); // These three lines calculate the normal vector of the target point cloud, as above
  norm_est.compute (* points_with_normals_tgt);
  pcl :: copyPointCloud (* tgt, * points_with_normals_tgt);

  // Create an instance of a custom point expression
  MyPointRepresentation point_representation;
  // Weighted curvature dimension to keep balance with coordinate xyz
  float alpha [4] = {1.0, 1.0, 1.0, 1.0};
  point_representation.setRescaleValues (alpha); // Set the scale values (used when vectorizing points)

  // Create a non-linear ICP object and set parameters
  pcl :: IterativeClosestPointNonLinear <PointNormalT, PointNormalT> reg; // Create a non-linear ICP object (ICP variant, optimized using Levenberg-Marquardt)
  reg.setTransformationEpsilon (1e-6); // Set the maximum allowable error (iterative optimization)
  // ***** Note: Adjust this parameter according to the size of your database
  reg.setMaxCorrespondenceDistance (0.1); // Set the maximum distance between corresponding points (0.1m), during the registration process, ignore points greater than the threshold
  reg.setPointRepresentation (boost :: make_shared <const MyPointRepresentation> (point_representation)); // Set point expression
    // Set the source point cloud and target point cloud
  //reg.setInputSource (points_with_normals_src); // version does not match, use the following statement
    reg.setInputCloud (points_with_normals_src); // Set the input point cloud (point cloud to be transformed)
  reg.setInputTarget (points_with_normals_tgt); // Set the target point cloud
    reg.setMaximumIterations (2); // Set the number of iterations for internal optimization

  // Run the same optimization in a loop and visualize the results
  Eigen :: Matrix4f Ti = Eigen :: Matrix4f :: Identity (), prev, targetToSource;
  PointCloudWithNormals :: Ptr reg_result = points_with_normals_src; // Used to store results (coordinates + normal vector)

  for (int i = 0; i <30; ++ i) // iteration
  {
    PCL_INFO ("Iteration Nr.% D. \ N", i); // The command line displays the number of iterations
    // Save the point cloud for visualization
    points_with_normals_src = reg_result; //
    //estimate
    //reg.setInputSource (points_with_normals_src);
        reg.setInputCloud (points_with_normals_src); // Reset the input point cloud (point cloud to be transformed), because the transformation has already occurred after the last iteration
    reg.align (* reg_result); // Align (register) two point clouds

    Ti = reg.getFinalTransformation () * Ti; // Cumulative (per iteration) transformation matrix
        // If the error between this transformation and the last transformation is smaller than the threshold, further refine the method by reducing the maximum corresponding point distance
    if (fabs ((reg.getLastIncrementalTransformation ()-prev) .sum ()) <reg.getTransformationEpsilon ())
      reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance ()-0.001); // Reduce the maximum distance between corresponding points (set above)
    prev = reg.getLastIncrementalTransformation (); // The error of the last transformation
    // Display the current registration status, in the right view of the window, simply display the source point cloud and target point cloud
    showCloudsRight (points_with_normals_tgt, points_with_normals_src);
  }

  targetToSource = Ti.inverse (); // Calculate the transformation matrix from the target point cloud to the source point cloud
  pcl :: transformPointCloud (* cloud_tgt, * output, targetToSource); // Transform the target point cloud back to the source point cloud frame

  p-> removePointCloud ("source"); // Remove a point cloud from the screen based on the given ID. Parameter is ID
  p-> removePointCloud ("target");
  PointCloudColorHandlerCustom <PointT> cloud_tgt_h (output, 0, 255, 0); // Set the point cloud display color, the same below
  PointCloudColorHandlerCustom <PointT> cloud_src_h (cloud_src, 255, 0, 0);
  p-> addPointCloud (output, cloud_tgt_h, "target", vp_2); // Add point cloud data, the same below
  p-> addPointCloud (cloud_src, cloud_src_h, "source", vp_2);

    PCL_INFO ("Press q to continue the registration. \ N");
  p-> spin ();

  p-> removePointCloud ("source");
  p-> removePointCloud ("target");

  // add the source to the transformed target
  * output + = * cloud_src; // The number of points in the spliced point cloud image (points) is the sum of the points of the two point clouds

  final_transform = targetToSource; // Final transformation. Transformation matrix from target point cloud to source point cloud
 }


 // **************** Entry function ************************
// Main function
int main (int argc, char ** argv)
{
  // Read data
  std :: vector <PCD, Eigen :: aligned_allocator <PCD>> data; // model
  loadData (argc, argv, data); // Read pcd file data, see the definition above

  // Check user data
  if (data.empty ())
  {
    PCL_ERROR ("Syntax is:% s <source.pcd> <target.pcd> [*]", argv [0]); // Syntax
    PCL_ERROR ("[*]-multiple files can be added. The registration results of (i, i + 1) will be registered against (i + 2), etc"); // Can use multiple files
    return (-1);
  }
  PCL_INFO ("Loaded% d datasets.", (Int) data.size ()); // Show how many point cloud files were read

  // Create a PCLVisualizer object
    p = new pcl :: visualization :: PCLVisualizer (argc, argv, "Pairwise Incremental Registration example"); // p is a global variable
  p-> createViewPort (0.0, 0, 0.5, 1.0, vp_1); // Create left viewport
  p-> createViewPort (0.5, 0, 1.0, 1.0, vp_2); // Create right viewport

    // Create point cloud pointer and transformation matrix
    PointCloud :: Ptr result (new PointCloud), source, target; // Create 3 point cloud pointers for the result, source point cloud, and target point cloud
  // Global transformation matrix, identity matrix, pairwise transformation
    // Comma expression, first create an identity matrix, and then assign the pairwise transformation to the global transformation
    Eigen :: Matrix4f GlobalTransform = Eigen :: Matrix4f :: Identity (), pairTransform;


    // traverse all point cloud files
  for (size_t i = 1; i <data.size (); ++ i)
  {
    source = data [i-1] .cloud; // source point cloud
    target = data [i] .cloud; // target point cloud
    showCloudsLeft (source, target); // In the left view area, simply display the source point cloud and target point cloud
    PointCloud :: Ptr temp (new PointCloud); // Create a temporary point cloud pointer
        // Show the name of the point cloud file being registered and their respective points
    PCL_INFO ("Aligning% s (% d points) with% s (% d points). \ N", data [i-1] .f_name.c_str (), source-> points.size (), data [i] .f_name.c_str (), target-> points.size ());
// ************************************************ ********
         // Register 2 point clouds, see the function definition above
         pairAlign (source, target, temp, pairTransform, true);
     // Transform the current pair of point cloud data into a global transform.
     pcl :: transformPointCloud (* temp, * result, GlobalTransform);
     // Update global transformation
     GlobalTransform = GlobalTransform * pairTransform;
         // ************************************************ ********

         // Save the paired registration results and transform to the first point cloud frame
     std :: stringstream ss; // These two sentences are generated file names
     ss << i << ".pcd";
     pcl :: io :: savePCDFile (ss.str (), * result, true); // Save paired registration results

   }
}

Rebuild the project. To change the project's debug directory, hold down SHIFT while you click the right mouse button to open the Cmd window in the current window. Enter Pairwise_incremental_registration.exe capture0001.pcd capture0002.pcd capture0003.pcd capture0004.pcd on the command line CAPTURE0005.PCD, execute the program. Get the results shown in the following figure.

ReferenceHow to incrementally register pairs of clouds