An iterative closest point algorithm for marker-free 3D shape registration of continuum robots

TL;DR

This paper addresses marker-free estimation of the backbone shape of continuum robots from multi-view images, a key problem for validation and control. It introduces an optimization-based framework that projects a parametric 3D backbone curve into camera views and uses an ICP-like correspondence to align observed pixels with the model, avoiding full 3D rendering. The backbone is represented using a moving-frame curvature parameterization to enable arc-length-consistent sampling, improving efficiency. On concentric-tube CR data, the One-Step variant achieves sub-millimeter accuracy on artificial data (average max error ~0.665 mm) and about 0.939 mm on real images, demonstrating practical feasibility with open-source Python implementations, though real-time performance remains a goal for future work.

Abstract

Continuum robots have emerged as a promising technology in the medical field due to their potential of accessing deep sited locations of the human body with low surgical trauma. When deriving physics-based models for these robots, evaluating the models poses a significant challenge due to the difficulty in accurately measuring their intricate shapes. In this work, we present an optimization based 3D shape registration algorithm for estimation of the backbone shape of slender continuum robots as part of a pho togrammetric measurement. Our approach to estimating the backbones optimally matches a parametric three-dimensional curve to images of the robot. Since we incorporate an iterative closest point algorithm into our method, we do not need prior knowledge of the robots position within the respective images. In our experiments with artificial and real images of a concentric tube continuum robot, we found an average maximum deviation of the reconstruction from simulation data of 0.665 mm and 0.939 mm from manual measurements. These results show that our algorithm is well capable of producing high accuracy positional data from images of continuum robots.

Figures (8)

• Figure 1: Matching two circular arcs of the same length with $\theta$ as the radius. The cost function is computed as the sum of all reconstruction errors. The reconstruction error is the distance of each cannula pixel to the closest reconstruction pixel. Initial matching errors like in the initial guess decrease over the iteration steps, so that in the end, in this example, the estimate is ideal.
• Figure 2: A 3D curve traced by a moving frame. The frame follows its own z-axis and rotates around the axes with the curvatures $u_x$, $u_y$, and $u_z$.
• Figure 3: Generation of artificial images from simulation results.
• Figure 4: The One-Step algorithm and Multi-Step algorithm with $p=8$ fit the reconstruction to the pixel with maximum distance to ground truth of around 1mm, whereas the Multi-Step version with $p=2$ converges to a local minimum.
• Figure 5: The One-Step algorithm fits the reconstruction to the pixel, whereas the Multi-Step with $p=2$ version converges to a local minimum. For $p=8$ the Multi-Step algorithm matches the robot's shape well.

Theorems & Definitions (2)

• Theorem 2.1: Convergence of the ICP curve-fitting
• proof