Improving Rigid-Body Registration Based on Points Affected by Bias and Noise

Abstract

The task of registering two coordinate frames is frequently accomplished by measuring the same set of points in both frames. Noise and possible bias in the measured locations degrade the quality of registration. It was shown that the performance of registration may be improved by filtering out noise from repeated measurements of the points, calculating small corrections to the mean locations and restoring rigid-body condition. In the current study, we investigate experimental conditions in which improvement in registration can still be achieved without cumbersome collection of repeated measurements. We show that for sufficiently small noise relative to bias, the corrections calculated from a single measurement of points can be used and still lead to the improved registration.

1 Introduction

Registration is a commonly performed procedure when points measured in one coordinate frame have to be accessed in another frame. Each registration requires identification of common features with known spatial location in both frames. This may be a challenging task when spatial relations between features are not preserved, and thus non-rigid registration techniques must be used [1,2,3,4]. In many manufacturing applications (like part assembly) corresponding 3D points can be identified and rigid-body registration can be used. We call the first frame from which the points are transformed, the working frame, and the second frame the destination frame. In the ideal situation when the rigid-body condition (RBC) is satisfied, the distance between any two points $\left( {\varvec{X}_{i} ,\varvec{X}_{j} } \right)$ in the working frame and the distance between the same points in the destination frame $\left( {\varvec{Y}_{i} ,\varvec{Y}_{j} } \right)$ are equal

$$ L_{i,j} = ||\varvec{X}_{i} - \varvec{X}_{j} || - ||\varvec{Y}_{i} - \varvec{Y}_{j} || \equiv 0 $$

(1)

Then, the rotation $\varvec{R}$ and translation $\varvec{\tau}$ mapping one set of $J \ge 3$ points $\left\{ \varvec{X} \right\}_{J}$ exactly on another set $\left\{ \varvec{Y} \right\}_{J}$ can be determined. These common points, measured in both frames and used for registration, are called fiducials. In reality, due to noise and possible bias in the measured data, RBC does not hold (i.e., $L_{i,j} \ne 0$) and $\left\{ \varvec{X} \right\}_{J}$ cannot be mapped exactly to $\left\{ \varvec{Y} \right\}_{J}$. Then, the transformation $\left\{ {\varvec{R},\varvec{\tau}} \right\}$ can be found by minimizing the following Fiducial Registration Error

$$ FRE\left( {\varvec{R},\varvec{\tau}} \right) = \frac{1}{J}\sum\nolimits_{j = 1}^{J} {\text{||}\varvec{RX}_{j} +\varvec{\tau}- \varvec{Y}_{j} ||^{2} .} $$

(2)

Transformation $\left\{ {\varvec{R},\varvec{\tau}} \right\}$ can be determined and applied to the target points $\left\{ {\varvec{T}_{X} } \right\}$ which are usually measured only in the working frame. If a corresponding set of targets $\left\{ {\varvec{T}_{Y} } \right\}$ is available in the destination frame, then the Target Registration Error $TRE\left( {\varvec{T}_{X} } \right)$ can be calculated as

$$ TRE\left( {\varvec{T}_{X} } \right) = ||\varvec{RT}_{X} +\varvec{\tau}- \varvec{T}_{Y} ||. $$

(3)

The quality of the registration may be reported by providing the values of $TRE$ for a representative set of targets measured in both frames. Thus, reduction of $TRE$ should improve the performance of many tasks in manufacturing. For example, peg-in-hole testing (commonly used to benchmark a robot’s performance [5,6,7] for insertion tasks) depends on the error in hole location [8,9,10,11,12].

Registration can be improved by obtaining repeated measurements of fiducials $\left\{ \varvec{X} \right\}_{J}^{\left( n \right)}$, $\left\{ \varvec{Y} \right\}_{J}^{\left( n \right)}$ and targets $\left\{ {\varvec{T}_{X} } \right\}^{\left( n \right)}$, $n \le N$. Then, noise could be filtered out by using the mean locations of points and virtually noise-free registration transformation could be obtained. Subsequent use of such transformation would map the mean locations of targets from the working frame almost exactly onto the true locations of targets in the destination frame. However, removal of noise does not remove non-homogenous bias from the measured 3D points and the mean locations $\left\{ {\bar{\varvec{X}}} \right\}_{J}$ and $\left\{ {\bar{\varvec{Y}}} \right\}_{J}$ cannot satisfy RBC in (1). To deal with this problem, a method was developed which calculated bias in fiducials from their repeated measurements [13]. The method yields a set of dependent linear equations, and to solve it, a separate set of $J$ points without bias has to be measured. This requirement severely restricts the use of the method as the location of bias-free points may not be known or available in practical applications. Another approach was developed in [14] where it was shown that the rigid-body condition can be restored for fiducials by calculating a small correction $\varvec{\varepsilon}_{j}$ to each $\bar{\varvec{X}}_{j}$. Then, the resulting transformation $\left\{ {\varvec{R}_{c} ,\varvec{\tau}_{c} } \right\}$ mapped exactly each corrected fiducial $\bar{\varvec{X}}_{j} -\varvec{\varepsilon}_{j}$ from the working frame to $\bar{\varvec{Y}}_{j}$ in the destination frame and reduced $FRE$ in (2) to zero. Correction to the mean location of target $\bar{\varvec{T}}_{X}$ in the working frame was then interpolated from corrections of nearby fiducials, leading to a reduction of $TRE\left( {\bar{\varvec{T}}_{X} } \right)$ in most cases.

It was hypothesized in [14] that repeated measurements may not be necessary if bias is the dominant cause of deviation from the rigid-body condition (i.e., the magnitude of noise is sufficiently smaller than bias). We test this hypothesis in this study: we do not use the mean of repeated measurements and instead apply the procedure directly to single measurements of fiducials and targets. Based on the outcome of our experiments we conclude that the procedure of restoration of rigid-body condition is still feasible for unfiltered, noisy data, and it can lead to improved registration when noise in the measured locations of points is sufficiently smaller (approximately three times) than bias.

2 Restoration of Rigid-Body Condition

If (1) does not hold and $L_{i,j} \ne 0$ then the corrections $\left\{\varvec{\varepsilon}\right\}_{J}$ in locations of $\left\{ \varvec{X} \right\}_{J}$ are sought that would bring $L_{i,j}$ back to zero. Corrections are calculated in the working frame for $\left\{ \varvec{X} \right\}_{J}$ since they will be needed to estimate corrections for targets $\left\{ {\varvec{T}_{X} } \right\}$ which are measured in working frame. Thus, we seek for vectors $\left\{\varvec{\varepsilon}\right\}_{J}$ such that

$$ ||\left( {\varvec{X}_{i} -\varvec{\varepsilon}_{i} } \right) - \left( {\varvec{X}_{j} -\varvec{\varepsilon}_{j} } \right)||^{2} = ||\varvec{Y}_{i} - \varvec{Y}_{j} ||^{2} , $$

(4)

for $1 \le i < j \le J$. The solution $\left\{\varvec{\varepsilon}\right\}_{J} = \left\{ {\varvec{\varepsilon}'} \right\}_{J} + \left\{ {\varvec{\varepsilon}''} \right\}_{J}$ is obtained in a two-step iterative procedure described in [14] where $\left\{ {\varvec{\varepsilon}'} \right\}_{J}$ and $\left\{ {\varvec{\varepsilon}''} \right\}_{J}$ are the corrections calculated in the first and the second iteration, respectively.

Once the corrections to fiducials are calculated, a registration between the corrected fiducial locations in the working frame $\varvec{X}_{j} -\varvec{\varepsilon}_{j}$ and the corresponding fiducials $\varvec{Y}_{j}$ in the destination frame can be performed, and the transformation $\left\{ {\varvec{R}_{\varvec{c}} ,\varvec{\tau}_{\varvec{c}} } \right\}$ can be determined (since the rigid-body condition is now restored for fiducials, corresponding $FRE = 0$). Then, a correction to the target $\varvec{T}_{X}$ in the working frame can be calculated by linear interpolation of corrections from $M$ nearby fiducials as

$$ \varvec{\varepsilon}\left( {\varvec{T}_{X} } \right) = \sum\nolimits_{m = 1}^{M} {w_{m}\varvec{\varepsilon}_{m} ,} $$

(5)

where $\varvec{\varepsilon}_{m}$ is the correction at $\varvec{X}_{m}$ fiducial and $w_{m}$ is a normalized weight. Corrected target locations

$$ \tilde{\varvec{T}}_{X} = \varvec{T}_{X} -\varvec{\varepsilon}\left( {\varvec{T}_{X} } \right), $$

(6)

can now be transformed to the destination frame using $\left\{ {\varvec{R}_{\varvec{c}} \text{,}\varvec{\tau}_{\varvec{c}} } \right\}$ and the transformed point is expected to be closer to the target point measured in the destination frame $T_{Y}$.

3 Experiment

Two series of experiments were performed. In the first one, three different metrology systems were used: two motion tracking systems (A and B) and a laser tracker (LT). Registration of A to LT and B to LT were investigated. In the second set of experiments, an industrial robot operated in compliance mode was used to measure the location of the end point of its tool and robot-world registration was investigated.

In the first series of experiments, 3D fiducial points were measured by systems A, B, and LT on a semi-regular grid of 5 × 5 × 5 points, placed in a work volume (approximately 3 m × 3 m × 1.8 m). In addition, $K = 16$ target points distributed randomly in the work volume were also acquired, as shown in Fig. 1a. Measurements of each point (fiducial and target) were repeated $N = 200$ times for System A and B; single measurement of each point was acquired with LT. However, the setting for LT was such that the average of 50 raw measurements was output (corresponding instrument noise was 1.7 μm) and therefore, for the purpose of this study, LT was considered as noise and bias free. Details of experimental set up are described in [15].

Fig. 1.

(a) 3D data: locations of 125 fiducials and 16 targets plotted in coordinate frame of LT. (b) 2D data: locations of 277 fiducials and 80 targets displayed in world coordinate frame. Large squares mark locations of five fiducials used to perform the registration of original, uncorrected data. Triangles mark locations of three fiducials used to perform the registration after RBC was restored in the set of fiducials.

In the second set of experiments, a Six Degree of Freedom (6DOF) collaborative robot arm (RA) operated in compliance mode was used. The arm was mounted on an optical breadboard plate (600 mm × 900 mm) firmly supported by an aluminum frame. The plate has a flatness of ±0.15 mm and has threaded holes which form a regular grid of squares 25 mm × 25 mm. A part of the plate accessible to the robot arm covered an area containing 17 × 21 holes. All available 357 holes were divided into two subsets: the first subset contained $J = 277$ points from which fiducials used in registration were selected; the second subset contained $K = 80$ target points, as shown in Fig. 1b.

A cylindrical adapter was screwed into each hole on the plate. The adapter had a hole into which a cylindrical peg, mounted to the end of the robot wrist, could be inserted. The peg was made of tool steel and had 12.67 mm diameter; the hole in the aluminum adapter had a nominal diameter of (12.67 + 0.051) mm. Once the peg was inserted into the adapter, the location of the endpoint of the peg in the robot’s coordinate frame was recorded. The adapter was screwed into each of 357 holes and a single measurement was acquired for each hole. For a limited number of holes randomly distributed on the plate, $N = 16$ repeated measurements were obtained. RA data contain 3D Cartesian coordinates of the endpoint of the peg and three angles defining the orientation of the peg in the robot’s frame. However, for this project, the angles and z-coordinate were ignored and only 2D data $\left( {x,y} \right)$ were further processed.

4 Data Processing

The differences defined in (1) were calculated for all possible pairs of fiducials $\left( {i,j} \right)$ where $1 \le i < j \le J$. For the series of experiments using LT, the number of such pairs was $J_{p} =$ 7 750, for the second series using RA, $J_{p} =$ 38 226. For Systems A and B, the differences $L_{i,j}$ were calculated for averaged locations $\left\{ {\bar{\varvec{X}}} \right\}_{J}$ and $L_{i,j}^{\left( n \right)}$ for each repeated measurement of fiducials in working frame $\left\{ \varvec{X} \right\}_{J}^{\left( n \right)}$, $n \le N$. For data acquired by RA, only one measurement of each point was acquired ($N = 1$). Standard deviations $\sigma_{L}$ of differences $L$ were calculated for instantaneous data acquired by RA and for averaged locations acquired by Systems A and B

$$ \sigma_{L} = \sqrt {1/J_{p} \,\,\sum\nolimits_{i = 1}^{i = j - 1} {\sum\nolimits_{j = 2}^{J} {\left( {L_{i,j} - \bar{L}} \right)^{2} } } } . $$

(7)

Three standard deviations were calculated: $\sigma_{L0}$ for the original, uncorrected locations of fiducials; $\sigma_{L1}$ for the fiducials after the first step of the procedure restoring RBC; $\sigma_{L2}$ after the second step when RBC was restored. In addition to the standard deviations, histograms of differences $L$ were built for each dataset and after each iteration of the procedure restoring RBC.

Restoration of RBC was performed for 3D data for Systems A and B registered to LT and for 2D data for robot frame registered to world. Corrections $\left\{\varvec{\varepsilon}\right\}_{J}$ were calculated for average $\left\{ {\bar{\varvec{X}}} \right\}_{J}$ and instantaneous $\left\{ \varvec{X} \right\}_{J}^{\left( n \right)}$ locations of the fiducials, yielding corresponding registration transformations $\left\{ {\varvec{R}_{c} ,\varvec{\tau}_{c} } \right\}$ and $\left\{ {\varvec{R}_{c}^{\left( n \right)} ,\varvec{\tau}_{c}^{\left( n \right)} } \right\}$. Once corrections to fiducials were determined, estimated corrections to average and instantaneous target $\varvec{\varepsilon}\left( {\bar{\varvec{T}}_{X} } \right)$ and $\varvec{\varepsilon}\left( {\varvec{T}_{X}^{\left( n \right)} } \right)$ were determined using (5). For 3D data (Systems A and B registered to LT) corrections were interpolated from four nearby fiducials which constituted the tetrahedron containing a given target. For 2D data, correction from eight fiducials surrounding a target on a plane were used in the interpolation. Then, corrected target $\tilde{\varvec{T}}_{X}$ in the working frame was calculated as in (6) and transformed to the destination frame.

Finally, two types of Target Registration Error were calculated. In the first error $TRE_{u} \left( {\varvec{T}_{X} } \right)$, uncorrected target $\varvec{T}_{X}$ was transformed by $\left\{ {\varvec{R}_{u} ,\varvec{\tau}_{u} } \right\}$ derived from uncorrected locations of fiducials. In the second error $TRE_{c} \left( {\tilde{\varvec{T}}_{X} } \right)$, corrected target $\tilde{\varvec{T}}_{X}$ was transformed by $\left\{ {\varvec{R}_{c} ,\varvec{\tau}_{c} } \right\}$ derived from corrected locations of fiducials (after RBC was restored). For $N$ repeated measurements, both types of errors were calculated for each dataset, i.e. $TRE_{c}^{\left( n \right)} \left( {\varvec{T}_{X}^{\left( n \right)} } \right)$ and $TRE_{c}^{\left( n \right)} \left( {\tilde{\varvec{T}}_{X}^{\left( n \right)} } \right)$, from which the corresponding median values were determined.

For each of $J = 125$ fiducials acquired by System A and B, the variance $\sigma_{j}^{2}$ was calculated from $N = 200$ repeated measurements and then the averaged variance $\sigma^{2}$ for all $J$ fiducials was determined. For RA data, repeated $N = 16$ measurements were acquired only for a limited number of fiducial locations (52 out of 277) from which the average variance $\sigma^{2}$ was calculated.

5 Results

Table 1 contains the mean standard deviation $\sigma$, three standard deviations $\sigma_{L0}$, $\sigma_{L1}$ and $\sigma_{L2}$ calculated in (7), and parameter $\rho$ for three cases. For the two registrations using LT, the differences $L_{i,j}$ and corresponding standard deviations were calculated for the average locations of fiducials $\left\{ {\bar{\varvec{X}}} \right\}_{J}$ (i.e., noise was filtered out from their locations).

Table 1. Summary of experimental results.

For the RA data, the differences $L_{i,j}$ were calculated for instantaneous data, and the resulting standard deviations are affected by systematic bias as well as by random noise.

Histograms of the differences $L$ calculated for data acquired by Systems A, B and LT are shown in Fig. 2; histograms for data acquired by RA are shown in Fig. 3. Target registration errors calculated in (3) for data acquired with Systems A, B and LT are shown in Fig. 4. The results in Fig. 4 were obtained for three different post-processing methods, including $TRE$ calculated for the average as well as instantaneous locations of fiducials and targets. For data acquired with RA, only a single measurement of each fiducial or target was taken and, therefore, only two post-processing methods could be used and the results are presented in Fig. 5. The difference between both methods, i.e. $\Delta TRE = TRE_{c} - TRE_{u}$, is shown in Fig. 5b: $\Delta TRE < 0$ indicates improvement in registration.

Fig. 2.

Histogram of differences $L$ evaluated from data acquired by: (1) System A and LT, (2) System B and LT; (a) based on original, uncorrected locations of fiducials; (b) based on fiducials after the first step of RBC restoration. Grey plots (A) correspond to the average locations of fiducials $\left\{ {\bar{\varvec{X}}} \right\}_{J}$. Solid lines (B) show the lower limit of count at each bin over all $N$ histograms from repeated data; dashed lines (C) are for upper limit of count at each bin.

Fig. 3.

Histogram of differences $L$ evaluated from data acquired by RA on an optical breadboard plate: (a) based on original, uncorrected locations of fiducials; (b) based on fiducials after the first step of RBC restoration; (c) based on fiducials after the second, final step of RBC restoration.

Fig. 4.

Target registration error calculated using three methods: (1) $TRE_{u} \left( k \right)$ for uncorrected, average fiducials and targets; (2) $TRE_{c} \left( k \right)$ for corrected, average fiducials and targets; (3) corrected, instantaneous fiducials and targets – displayed is median of $N = 200$ instantaneous errors $TRE_{c}^{\left( n \right)} \left( k \right)$: (a) data acquired by System A and LT; (b) data acquired by System B and LT.

Fig. 5.

(a) Target registration error calculated using two methods: (1) $TRE_{u} \left( k \right)$ for uncorrected, single measurement of fiducials and targets; (2) $TRE_{c} \left( k \right)$ for corrected, single measurement of fiducials and targets; (b) the difference $\Delta TRE$ between both methods.

6 Discussion and Conclusions

The three registrations listed in Table 1 were performed in very different experimental conditions. System A has the smallest noise $\sigma$ and (when registered to LT) has the largest bias, as evidenced by $\sigma_{LO}$. System B has the largest noise and the smallest bias. The characteristics of the robot arm are between these two conditions: noise and bias of RA is between the corresponding limits of Systems A, B and LT.

Plots displayed in Figs. 2 and 3 clearly demonstrate that the two-step iterative procedure of restoring RBC in a set of fiducials converged very well. This applies to the restoration of RBC in the set of averaged fiducials $\left\{ {\bar{\varvec{X}}} \right\}_{J}$ (filled grey histograms in Fig. 2) and in instantaneous locations $\left\{ \varvec{X} \right\}_{J}^{\left( n \right)}$ (dashed lines in Fig. 2 and filled grey histograms in Fig. 3). The progress of the procedure to restore RBC in the fiducials can be also noted in data included in Table 1 where $\sigma_{L0 } \gg \sigma_{L1 } \gg \sigma_{L2 } \approx 0$.

Full restoration of RBC is limited only to a set of fiducials $\left\{ \varvec{X} \right\}_{J}$ and for other points, like targets $\left\{ {\varvec{T}_{X} } \right\}$, only partial restoration of RBC is possible. This is because targets are measured only in the working frame and corrections for their locations are estimated by extrapolating corrections of nearby fiducials. Therefore, target errors $TRE_{c} \left( {\varvec{T}_{X} } \right)$ are non-zero although fiducials error $FRE$ equals zero. For cases where repeated measurements in the working frame are available and noise could be filtered out from fiducials and targets, the procedure investigated in this study shows substantial improvement for most targets, as shown in Fig. 4 for cases (1) and (2). For example, for targets $k = 7,8$ in Fig. 4a, corresponding $TRE_{c} \left( k \right)$ are reduced by almost ten times in comparison with uncorrected $TRE_{u} \left( k \right)$. The overall improvement is larger for System A registered to LT (Fig. 4a) and smaller for System B registered to LT (Fig. 4b). For some targets ($k = 9,10$ in Fig. 4b), the procedure leads to slightly increased $TRE\left( k \right)$.

For instantaneous data containing both noise and bias, the outcome of the procedure may be very different. For System B registered to LT, the median of $TRE_{c}^{\left( n \right)}$ (evaluated after RBC has been restored for each n-th dataset) is much larger than $TRE_{c}$ calculated for average fiducials $\left\{ {\bar{\varvec{X}}} \right\}_{J}$ and targets $\left\{ {\bar{\varvec{T}}_{X} } \right\}$, see cases (3) and (2) in Fig. 4b. In fact, the median of $TRE_{c}^{\left( n \right)}$ is even larger than the uncorrected $TRE_{u} \left( k \right)$, see cases (3) and (1) in Fig. 4b. However, for System A registered to LT, there is practically no difference between the median of $TRE_{c}^{\left( n \right)}$ and corrected $TRE_{c}$ calculated for average $\left\{ {\bar{\varvec{X}}} \right\}_{J}$ and $\left\{ {\bar{\varvec{T}}_{X} } \right\}$, see cases (3) and (2) in Fig. 4a. Similarly, for instantaneous data acquired by RA and registered to world, the procedure of restoring RBC leads to noticeable reduction in $TRE_{c}$ when compared with uncorrected $TRE_{u}$, see cases (2) and (1) in Fig. 5a. In fact, in out of 80 target locations, the procedure failed only marginally in four cases, as indicated by $\Delta TRE > 0$ in Fig. 5b.

If random noise is the main cause of poor registration, reduction in $TRE\left( k \right)$ can be obtained by preplanning the placement of fiducials relative to the k-th target. This procedure is based on the fact that uncertainty propagated from fiducials via transformation $\left\{ {\varvec{R},\varvec{\tau}} \right\}$ to the target is anisotropic, as predicted in [16] and confirmed experimentally in [17]. Thus, optimally placed fiducials will ensure that the uncertainty propagated to a given target is the smallest. However, unlike the suppression of bias which can decrease $TRE$ of any target point, placement of fiducials must be optimized for each target independently.

In summary, the calculation of parameter $\rho$ as shown in Table 1 may help to determine if the procedure of restoring RBC leads to improvement in the registration of instantaneous data (i.e., data containing both noise and bias). For $\rho < 1$, the procedure should not be used as it will lead to worse performance of registration than for original, uncorrected data. For $\rho \;{ \gtrsim }\;2.5$, the use of the procedure is beneficial. The larger the parameter $\rho$, the better improvement in registration is expected and this, in turn, should improve performance of part assembly when tight tolerances are required.