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

ICP is an algorithm employed to minimize the difference between two clouds of points. The standard, not joint nor generalized, ICP has been optimized using the Intel® oneAPI Base Toolkit.

See registration_api for details.

This tutorial shows how to use these optimizations inside a Docker* image. For the same functionality outside of Docker* images, see PCL Optimizations Outside of Docker* Images.

Prepare the environment:

cd <edge_insights_for_amr_path>/Edge_Insights_for_Autonomous_Mobile_Robots_<version>/AMR_containers
./run_interactive_docker.sh eiforamr-full-flavour-sdk:2023.1 root -c full_flavor
mkdir one_api_registration && cd one_api_registration

Create the file oneapi_icp_example.cpp:
```
vim oneapi_icp_example.cpp
```

Place the following inside the file:

#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)
{
  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices;
  p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_file_indices.size () != 2)
  {
    print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd\n", argv[0]);
    return (-1);
  }

  // Load the files
  print_info ("Loading %s as source and %s as target...\n", argv[p_file_indices[0]], argv[p_file_indices[1]]);
  PointCloud<PointXYZ>::Ptr src, tgt;
  src.reset (new PointCloud<PointXYZ>);
  tgt.reset (new PointCloud<PointXYZ>);
  if (loadPCDFile (argv[p_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_file_indices[1]], *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);
}
/* ]--- */

Create a CMakeLists.txt file:
```
vim CMakeLists.txt
```

Place the following inside the file:

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
set(target oneapi_icp_example)
set(CMAKE_CXX_COMPILER dpcpp)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_FLAGS "-Wall -Wpedantic -Wno-unknown-pragmas -Wno-pass-failed -Wno-unneeded-internal-declaration -Wno-unused-function -Wno-gnu-anonymous-struct -Wno-nested-anon-types -Wno-extra-semi -Wno-unused-local-typedef -fsycl -fsycl-unnamed-lambda -ferror-limit=1")
project(registration)

find_package(PCL 1.12 REQUIRED)
find_package(PCL-ONEAPI 1.12 REQUIRED)

add_executable (${target} oneapi_icp_example.cpp)

include_directories(${PCL_INCLUDE_DIRS} ${PCL-ONEAPI_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS} ${PCL-ONEAPI_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS} ${PCL-ONEAPI_DEFINITIONS})

target_link_libraries (${target} sycl pcl_oneapi_registration pcl_oneapi_search pcl_oneapi_kdtree pcl_io)

Source the Intel® oneAPI Base Toolkit environment:

export PATH=/home/eiforamr/workspace/lib/pcl/share/pcl-1.12:/home/eiforamr/workspace/lib/pcl/share/pcl-oneapi-1.12:$PATH
source /opt/intel/oneapi/setvars.sh

Build the code:

cd /home/eiforamr/workspace/one_api_registration/
mkdir build && cd build
cmake ../
make -j

Download the test data from GitHub*:

wget https://raw.githubusercontent.com/NVIDIA-AI-IOT/cuPCL/main/cuOctree/test_P.pcd
wget https://raw.githubusercontent.com/NVIDIA-AI-IOT/cuPCL/main/cuOctree/test_Q.pcd
# if the binaries is not downloaded try setting the proxies first and try again:
export http_proxy="http://<http_proxy>:port"
export https_proxy="http://<https_proxy>:port"

Run the binary:

./oneapi_icp_example test_P.pcd test_Q.pcd

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*.

  PointCloud<PointXYZ>::Ptr src, tgt;
  src.reset (new PointCloud<PointXYZ>);
  tgt.reset (new PointCloud<PointXYZ>);
  if (loadPCDFile (argv[p_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_file_indices[1]], *tgt) == -1)
  {
    print_error ("Error reading the input files!\n");
    return (-1);
  }

  PointCloud<PointXYZ> output;

Declare the Intel® oneAPI Base Toolkit's ICP, and set the input configuration parameters.

  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;
  std::cerr << transform << std::endl;
  // Write transformed data to disk
  savePCDFileBinary ("source_transformed.pcd", output);

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

Contents

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

Code Explanation#