Attention

You are viewing an older version of the documentation. The latest version is 2.2.

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 will cover the standard ICP.

Iterative Closest Point

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

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. Transform the source points using the obtained transformation.

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

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

Note

This tutorial can be run both inside and outside a Docker* image. We assume that the pcl-oneapi-tutorial Deb package has been 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;