ICP

Implementation of the photogeometric ICP algorithm in OpenCL

Published on June 17, 2015

Categories: robotics, registration

Tags: c++, opencl, icp, registration, filtering, nearest-neighbor, point-cloud

Website: https://github.com/nlamprian/ICP

ICP is an implementation of the Photogeometric Iterative Closest Point (ICP) algorithm in OpenCL. ICP performs real-time frame-to-frame 3D registration, utilizing both the geometry and the texture of a scene. This way, the geometric features can guide the registration process in situations where there are faintly textured regions, and the color can be used in those places with non-salient surface topology. The implemented framework is based on the paper Real-time RGB-D mapping and 3D modeling on the GPU using the random ball cover data structure by Neumann et al.

Currently, there are two options for the rotation estimation step. One that uses rotation matrices and estimates the rotation by performing Singular Value Decomposition on the CPU. The other uses unit quaternions and estimates the rotation based on the Power Method. The rest of the computational load is executed exclusively on the GPU. Both resulting pipelines are able to perform one ICP iteration in about 1.1 millisecond, for input sets of |F|=|M|=16384 landmarks and |R|=256 representative points.

It’s quite easy to do 3D registration in your own applications. You configure and initialize an ICP object, write the sets of landmarks on the device buffers, possibly offer an initial estimation of the transformation, execute the kernels, and get back the estimated homogeneous transformation. Below is a dummy example to showcase the workflow. For more details, take a look at the example in the repository.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
#include <CLUtils.hpp>
#include <ICP/algorithms.hpp>

using namespace clutils;
using namespace cl_algo;

int main(int argc, char **argv) {
  // Setup OpenCL environment
  CLEnv env;
  env.addContext(0);
  env.addQueue(0, 0);
  env.addProgram(0, kernel_files_rbc);
  env.addProgram(1, kernel_files_icp);

  // Configure kernel execution parameters
  CLEnvInfo<1> infoRBC(0, 0, 0, { 0 }, 0);
  CLEnvInfo<1> infoICP(0, 0, 0, { 0 }, 1);
  const ICP::ICPStepConfigT RC = ICP::ICPStepConfigT::POWER_METHOD;
  const ICP::ICPStepConfigW WC = ICP::ICPStepConfigW::WEIGHTED;
  ICP::ICP<RC, WC> icp(env, infoRBC, infoICP);
  icp.init(nm, nr, a, c, max_iter, angle_thres, translation_thres);

  // Copy point clouds to device
  icp.write(ICP::ICPStep<RC, WC>::Memory::D_IN_F, dataF);
  icp.write(ICP::ICPStep<RC, WC>::Memory::D_IN_M, dataM);

  icp.buildRBC();  // Build RBC data structure
  icp.run();  // Refine transformation

  // Retrieve estimated transformation
  cl_float *T = (cl_float *) icp.read();

  return 0;
}

You can find the complete documentation at icp.nlamprian.me. The source code is available on GitHub.