Intel® oneAPI Base Toolkit's Iterative Closest Point (ICP)

The standard Iterative Closest Point (ICP) has been optimized using Intel® oneAPI Base Toolkit. Joint ICP and Generalized ICP are not currently optimized with Intel® oneAPI Base Toolkit. This tutorial covers the standard ICP.

Iterative Closest Point

Iterative closest point (ICP) is an algorithm utilized to minimize the difference between two point clouds.

The ICP steps are:

1. For each point in the source point cloud, match the closest point in the reference/target point cloud.

2. Estimate the combination of rotation and translation using a root mean square point-to-point distance metric minimization technique which will best align each source point to its match found in the previous step.

3. Apply the obtained transformation to source point cloud.

4. Iterate (re-associate the points, and so on).

For details regarding the PCL Registration module and its internal algorithmic details, please refer to the registration_api for details.

Note

This tutorial is applicable for execution both within inside and outside a Docker* image. It assumes that the pcl-oneapi-tutorials Deb package is installed, and the user has copied the tutorial directory from /opt/intel/pcl/oneapi/tutorials/ to a user-writable directory.

1. Prepare the environment:

   cd <path-to-oneapi-tutorials>/registration
   

   

2. oneapi_icp_example.cpp should be in the directory with following content:

   #include <pcl/oneapi/registration/icp.h>
   #include <pcl/console/parse.h>
   #include <pcl/point_types.h>
   #include <pcl/point_cloud.h>
   #include <pcl/point_representation.h>
   #include <pcl/io/pcd_io.h>
   
   
   using namespace pcl;
   using namespace pcl::io;
   using namespace pcl::console;
   
   /* ---[ */
   int
   main (int argc, char** argv)
   {
     std::cout << "Running on device: " << dpct::get_default_queue().get_device().get_info<sycl::info::device::name>() << "\n";
   
     // Load the files
     PointCloud<PointXYZ>::Ptr src, tgt;
     src.reset (new PointCloud<PointXYZ>);
     tgt.reset (new PointCloud<PointXYZ>);
     if (loadPCDFile ("test_P.pcd", *src) == -1 || loadPCDFile ("test_Q.pcd", *tgt) == -1)
     {
       print_error ("Error reading the input files!\n");
       return (-1);
     }
   
     PointCloud<PointXYZ> output;
     // Compute the best transformtion
     pcl::oneapi::IterativeClosestPoint<PointXYZ, PointXYZ> reg;
     reg.setMaximumIterations(20);
     reg.setTransformationEpsilon(1e-12);
     reg.setMaxCorrespondenceDistance(2);
   
     reg.setInputSource(src);
     reg.setInputTarget(tgt);
   
     // Register
     reg.align(output); //point cloud output of alignment i.e source cloud after transformation is applied.
   
     Eigen::Matrix4f transform = reg.getFinalTransformation();
   
     std::cerr << "Transform Matrix:" << std::endl;
     std::cerr << transform << std::endl;
     // Write transformed data to disk
     savePCDFileBinary ("source_transformed.pcd", output);
   }
   /* ]--- */
   

   

3. Source the Intel® oneAPI Base Toolkit environment:

   source /opt/intel/oneapi/setvars.sh
   

   

4. (Optional) Setup proxy setting to download test data:

   export http_proxy="http://<http_proxy>:port"
   export https_proxy="http://<https_proxy>:port"
   

   

5. Build the code:

   mkdir build && cd build
   cmake ../
   make -j
   

   

6. Run the binary:

   ./oneapi_icp_example
   

   

7. Expected results example:

   Transform Matrix:
   0.998899   0.0107221   0.0457259   0.0790768
   -0.00950837    0.999602  -0.0266773   0.0252976
   -0.0459936    0.026213    0.998599   0.0677631
           0           0           0           1
   

   

Code Explanation

Define two input point Clouds (src, tgt), declare the output point cloud, and load the test data from GitHub*.

  // Load the files
  PointCloud<PointXYZ>::Ptr src, tgt;
  src.reset (new PointCloud<PointXYZ>);
  tgt.reset (new PointCloud<PointXYZ>);
  if (loadPCDFile ("test_P.pcd", *src) == -1 || loadPCDFile ("test_Q.pcd", *tgt) == -1)
  {
    print_error ("Error reading the input files!\n");
    return (-1);
  }

Declare oneapi ICP, and set the input configuration parameters.

  // Compute the best transformtion
  pcl::oneapi::IterativeClosestPoint<PointXYZ, PointXYZ> reg;
  reg.setMaximumIterations(20);
  reg.setTransformationEpsilon(1e-12);
  reg.setMaxCorrespondenceDistance(2);

Set the two input point clouds for the ICP module, and call the method to align the two point clouds. The align method populates the output point cloud, passed as a parameter, with the src point cloud transformed using the computed transformation matrix.

  reg.setInputSource(src);
  reg.setInputTarget(tgt);

  // Register
  reg.align(output); //point cloud output of alignment i.e source cloud after transformation is applied.

Get the computed matrix transformation, print it, and save the transformed point cloud.

  Eigen::Matrix4f transform = reg.getFinalTransformation();

  std::cerr << "Transform Matrix:" << std::endl;