We investigate the performance of pose measuring systems which determine an object’s pose from measurement of a few fiducial markers attached to the object. Such systems use point-based, rigid body registration to get the orientation matrix. Uncertainty in the fiducials’ measurement propagates to the uncertainty of the orientation matrix. This orientation uncertainty then propagates to points on the object’s surface. This propagation is anisotropic, and the direction along which the uncertainty is the smallest is determined by the eigenvector associated with the largest eigenvalue of the orientation data’s covariance matrix. This eigenvector in the coordinate frame defined by the fiducials remains almost fixed for any rotation of the object. However, the remaining two eigenvectors vary widely and the direction along which the propagated uncertainty is the largest cannot be determined from the object’s pose. Conditions that result in such a behavior and practical consequences of it are presented.
1. Introduction
The pose of a rigid object is defined by six degrees of freedom (6DOF): three angles describing the object’s orientation matrix R and three components describing the object’s position τ (e.g., center of mass or center of bounding box). Accounting for the uncertainty of the measured pose is of great importance in many applications (e.g., propagating uncertainty along different joints in a robot arm or fusing measurements from multiple sensors) and it has been studied for a long time [1]. The methodology used in these studies is based on a 4 × 4 homogenous transformation matrix, related exponential mapping, and Lie algebra [2].
In this paper, our focus is on a different aspect of pose uncertainty. We are interested in how uncertainty of a single static measurement of a rigid body pose propagates to any Point of Interest (POI) associated with the object (e.g., a point on its surface). When the Computer Aided Design (CAD) model of an object is known, the location of any POI can be calculated using 6DOF data acquired by pose measuring systems [3]. In assembly applications where rigid parts need to be mated using autonomous robotic systems [4–8], uncertainty in pose has to be propagated to the POI. For example, in a peg-in-hole experiment (commonly used to test a robot’s performance [9–12]), uncertainty in the hole location directly affects the test outcome [13–17]. Thus, we acquire repeated measurements of a rigid object’s pose, obtained in the same experimental conditions, to investigate the uncertainty of a given POI.
In most practical applications, the six components of pose are not directly measured but are derived from other raw measurements. Many pose measuring systems report 6DOF data of an object based on the measurement of the 3D positions of a few points. These points, also known as fiducial markers, are rigidly attached to (or around) the measured object. Some systems may not require the use of markers as they may be trained to use some characteristic features of the measured object (e.g., well defined corner points). For systems which use 3D points, a homogenous transformation R,τ is found using point-based rigid body registration and minimizing the following error function called the Fiducials Registration Error (FRE) (1)FRER,τ=1J∑n=1JRXn+τ-Yn2,where XJ is a set of J fiducials measured in one coordinate frame (working frame) and YJ is a set of corresponding fiducials measured in the second frame (destination frame). Pose measuring systems track the movement of the working frame and the transformation R,τ defines the object’s 6DOF pose relative to a starting reference frame (coordinate frame associated with the instrument). Point-based rigid body registration not only is implemented in pose measuring systems but is commonly used in many field applications where 3D points are measured in one frame but have to be accessed in another frame where they are required. These points (targets) need to be transformed from the working frame to the destination frame using the previously determined transformation R,τ.
Noise present in the measurement of the fiducials propagates to the transformation R,τ. Such noisy transformation (when applied to a target) yields random deviation of the transformed target from its nominal true location r and is quantified by the Target Registration Error (TREr) defined as the root mean square of distances between these two points. The development of a closed form equation for TRE has been the aim of extensive research for many years [18–22]. The main conclusions from these efforts can be summarized as follows: (1)TREr depends on the location of target r relative to the three main axes of the moment of inertia derived from the spatial configuration of fiducials XJ; (2)TREr can be expressed as the sum of two components: one related to uncertainty in position τ and the second related to uncertainty in the orientation data R; (3) both components of TREr depend on the magnitude of the noise (TREr increases for noisier fiducial measurements); (4) the orientation component is anisotropic; that is, it depends on the direction in space while the positional component is isotropic.
For the class of pose measuring systems which use point-based registration to track the pose of a rigid object, propagation of orientation uncertainty to a given POI is equivalent to the propagation of the fiducials’ uncertainty to a target point and, therefore, should inherit the above-mentioned characteristics of TREr. The anisotropic distribution of the orientation uncertainty was reported for the pose measuring system using a stereo camera to track spherical, reflective markers attached to an object [23]. It was found that the distribution is bimodal on a unit sphere with the smallest uncertainty located at poles defined by the eigenvector e3 associated with the largest eigenvalue of the covariance matrix of the orientation data. It was hypothesized that such a distribution offers an opportunity for better planning of robotic operations by ensuring that a given POI is in the region of small uncertainty. However, to take advantage of such a strategy, the direction of eigenvector e3 must stay fixed in the CAD coordinate frame regardless of the object’s orientation.
In this paper, we expanded the study in [23] to determine the conditions for which the observed behavior (stability of e3) holds by acquiring static measurements of several poses using a different pose measuring system (a large-scale tracking system iGPS). For each pose, the covariance matrix of the orientation data was determined. While the matrices were different for different poses, we found that the direction of the eigenvector e3 exhibited very small variations compared to the directions of the other two eigenvectors, e1 and e2, which showed larger variations. This behavior was reproduced in computer simulations and, to the best of our knowledge, it has not been reported in the literature. Analysis of existing theoretical expressions for TREr in point-based registration reveals the reason for such unusual behavior of pose measuring systems which employ point-based registration to calculate 6DOF data. We found that misalignment between the directions of the anisotropic noise of fiducials and the directions of the axes of the moment of inertia characterizing the configuration of the fiducials is responsible for the observed phenomenon. It appears that the direction of e3 is almost independent of the misalignment whereas the directions of e1 and e2 were dependent on the misalignment. Furthermore, our study shows that e3 is well aligned with the eigenvector bmin of the moment of inertia matrix corresponding to the smallest eigenvalue.
The location of any POI is fixed relative to the locations of fiducials. Therefore, for the class of pose measuring systems discussed in this paper, if a vector pointing to a POI is aligned with bmin, this POI will be in the region of small propagated uncertainty, regardless of the object’s orientation. Prior knowledge of such behavior may be useful in robotic operations when tight tolerances are required. A procedure for determining the placement of fiducials so that the smallest uncertainty is propagated to a given POI is introduced. The optimal placement of fiducials has been studied earlier for rigid body registration. Two main applied approaches were (1) use of theoretical models of TREr [24] (some of them based on isotropic noise [25, 26]); (2) numerical search for the optimal placement using covariance matrices of experimental noise, evaluating transformations R,τ and then corresponding to TREr [22, 27]. While these studies showed implicit directional dependence of TREr and its reduction, they did not alert practitioners that the uncertainty of a given POI on the rotated rigid object may depend on the object’s orientation nor provide clear guidance on how to ensure that this uncertainty will be close to the smallest possible value, regardless of object’s orientation. This paper attempts to provide this missing information.
In the next section, some background information and relevant equations are reviewed, followed by a brief description of the experimental setup and data postprocessing. This is followed by a presentation of the results, discussion, and conclusions.
2. Previous Research
In this section a brief review of the theoretical work relevant to our experiments is presented. Section 2.1 presents a brief review of point-based rigid body registration. This is followed by a discussion of the propagation of noise from the fiducials used to register two sets of points XJ and YJ to the registration parameters R,τ and then to the transformed target point; an analytical formula for TREr based on anisotropic, homogenous Gaussian noise perturbing the fiducials is provided. In Section 2.2, the propagation of orientation uncertainty of a 6DOF rigid object to an individual point on its surface is discussed.
2.1. TRE in Point-Based Rigid Body Registration
Given two sets of J fiducials XJ and YJ measured in the working and destination frames, respectively, the rotation R and translation τ which minimize the error function in (1) can be obtained in the following way. First, the origins of both frames are moved to the respective centroids Xavg and Yavg, that is, the locations of fiducials in the translated frames are X~n=Xn-Xavg and Y~n=Yn-Yavg, n=1,…,J. Then, the covariance matrix covXY3×3 is determined as (2)covXY=1JX~1,…,X~JY~1,…,Y~JT,where ⋯T is the transposed matrix. The rotation matrix R can be calculated as in [28] (3)R=VDUT,where the matrices U and V are obtained from the Singular Value Decomposition (SVD) of the covariance matrix(4)covXY=USVT,D=10001000detVUT.Once the rotation matrix R is determined, the related translation vector τ is calculated as (5)τ=Yavg-RXavg.This transformation R,τ minimizes FRE in (1) in the least-squares sense, and this procedure is implemented in many commercial software packages.
However, noise in the measured fiducials XJ and YJ affects the registration transformation, and it needs to be propagated to the target TX transformed to the destination frame, namely, RTX+τ. Intuitively, it is obvious that the statistical properties of the target error TREr will depend on the characteristics of the noise perturbing the fiducial locations as well as on the location of the target relative to the configuration of the fiducials. Based on the seminal papers by Sibson [29] and Fitzpatrick et al. [18], most theoretical studies and supporting computer simulations split the registration R,τ to two transformations: a “big” deterministic one R0,τ0 and a small noisy one ΔR,Δτ, that is, the two frames are first initially aligned using the big transformation and the fine tuning is done by the small rotation and translation. Thus, any point x in the working frame is transformed to y in the destination frame as(6)y=ΔRR0x+τ0+Δτ.The rationale behind such an approach was put forward by Sibson who observed that the distribution of TRE was completely determined by stochastic noise in the fiducials and not by the big transformation R0,τ0. This observation is an extension of the well-known property that a variance of a 3D point perturbed by Gaussian noise is the same in all coordinate frames related by any translation τ, that is, xi→xi-τ. As stated in [18], “Neither this reorientation nor the special positioning of the origin above is necessary to effect a solution [...], nor for any part of the derivation that follows. However, they do reduce the complexity considerably, and they can be easily undone at the end.” The big rotation R0 can be found from SVD of the covariance matrices covXX and covYY of fiducials XJ and YJ as(7)R0=UYUXTand the big translation τ0 can by calculated by substituting R=R0 in (5). Since both matrices covXX and covYY are symmetric and have positive diagonal elements, their SVD decomposition yields(8)covXX=UXΛ2UXTand similarly for covYY. Matrix Λ2 is diagonal matrix(9)Λ2=Λ12000Λ22000Λ32.Matrix covXX is closely related to the matrix of the moment of inertia M as (10)M=tracecovXXI3×3-covXX,where I3×3 is the identity matrix. Thus, Λ2 defines the moments of inertia relative to the three major axes, and the orientation of the axes is determined by matrix UX in the working frame and UY in the destination frame. When a coordinate system is aligned with the axes of the moment of inertia (customarily done in theoretical analysis of TREr in point-based rigid body registration) then matrix M takes a simple diagonal form(11)M=Λ22+Λ32000Λ12+Λ32000Λ12+Λ22.It should be stressed that the moment of inertia characterizes the configuration of the fiducials in space, not the noise affecting the locations of the fiducials. In general, when the distance between fiducials is a few orders of magnitude larger than the noise, the moment of inertia relative to the major axes remains constant, that is, ΛX2=ΛY2, and for this reason, we drop the subscript in Λ2.
While noise does not affect the moment of inertia, it has a great impact on the Target Registration Error (TREr). Different forms for estimating TREr were developed for different characteristics of fiducial noise, starting from the simplest isotropic, homogenous, Gaussian noise (the same for all fiducials) to the most complex, anisotropic, nonhomogenous Gaussian with nonzero mean (i.e., nonzero bias). No closed form solution has yet been developed for the most complex case. An analytical expression was provided for Gaussian, zero mean, homogenous, and anisotropic noise characterized by covariance matrix Ψ; see equation (51) in [30]. For such noise model, TREr was evaluated from the variance varr(12a)varr=traceΨJ+r2α2u,where u is the unit vector pointing towards the target r, that is, r=ru and (12b)α2u=∑i=13∑j≠i3uj2Λj2Ψi,i+Λi2Ψj,jΛi2+Λj22+∑i=13∑j≠i3∑k≠i,k≠j3ujukΨj,kΛi2Λi2+Λj2Λk2+Λi2is the variance of the angular error (deviation of the directional vector uϑ,φ from its nominal, noise free direction) and uϑ,φ is parametrized by two spherical angles ϑ and φ as(13)uϑ,φ=u1,u2,u3=cosϑcosφ,cosϑsinφ,sinϑ.Equation (12a) contains two terms: the first is isotropic and is related to the uncertainty in translation τ in (5); the second term is anisotropic as it depends on angles ϑ,φ and is related to uncertainty in the rotation R in (3). The isotropic term is inversely proportional to the number of fiducials J, and for most target locations which are not very close to the origin of the coordinate frame, the term related to orientation uncertainty in R will be dominant.
We note that the orientation of the noise matrix Ψ (i.e., the coordinate frame formed by its eigenvectors) and the orientation of the moment of inertia matrix M are completely unrelated and their relative orientation depends on the experimental conditions.
2.2. Propagation of Orientation Uncertainty of Rigid Body to a POI
Let vector U define the location of a POI in the CAD coordinate frame and let u be a unit vector parallel to U such that U=Uu. If Rj is the orientation matrix of a rigid object and tj its location obtained from the jth measurement, then Uj is the location of the POI on the rotated object in the coordinate frame of the pose measuring instrument,(14)Uj=Uwj+tj,where wj is a unit vector pointing to a rotated POI in the coordinate frame of the instrument (15)wj=Rjuand it can be parametrized by two spherical angles wjϑj,φj as in (13). We are interested in propagating the uncertainty of Rj to the uncertainty of wj. We assume that(16)Rj=RavgΔRj,where Ravg is the averaged orientation obtained from N repeated measurements acquired in the same experimental conditions, ΔRj is a small random rotation (noise), and j=1,…,N. In axis-angle representation aj,ρj, the smallness of the rotation is gauged by small values of angle ρj and this leads to the following expression for ΔRj in linear approximation(17)ΔRjaj,ρj≈I+0-qjzqjyqjz0-qjx-qjyqjx0,where I3×3 is the identity matrix, aj is a unit vector defining the axis of rotation, and (18)qj=ρjaj.A covariance matrix Cq of the orientation data can be calculated as (19)Cq=1Nq1,…,qNq1,…,qNT.
Repeated measurements of the orientation matrix Rj in (15) yield a corresponding set of vectors wj which are tightly distributed around the average direction wavg. If μ denotes the angle between wjϑj,φj and wavg, then its distribution can be described by the Fisher-Bingham-Kent (FBK) distribution [31–33] as (20)Gσ,βμ=μσ-2exp-μ22σ2Kσ,βμ,where σ is the angular uncertainty and Kσ,β is the Kent correction to the Fisher distribution(21)Kσ,βμ=12πσ-21-2βσ21+2βσ2×∫02πexpβμ2cos2ηdη.This correction takes into account the nonzero eccentricity parameter β which describes the shape of the elliptical contour of a constant probability on the ϑ,φ plane (Kσ,β→1 for symmetric circle contour when β→0). Larger values of uncertainty σ correspond to larger deviations of vector wj from the mean direction wavg. For pose measuring systems which use point-based rigid body registration, the angular uncertainty σ is equivalent to the angular uncertainty α from (12a) and (12b) when homogenous, anisotropic model of Gaussian noise characterizes the experimental conditions. However, the analysis in this subsection and as discussed here, the angular uncertainty σ is more general than the uncertainty α discussed in Section 2.1 because it is applicable to any sequence of noisy rotations ΔRj, no matter what sensors and raw measurements were used to get the rotation matrices. Equations (12a) and (12b) are applicable only to the class of pose measuring systems which utilize point-based rigid body registration.
3. Data Collection and Processing3.1. Experimental Setup
A commercially available, large-scale tracking system (iGPS) was used to collect 6DOF data [34]. The manufacturer specified positional uncertainty is 250 μm. The system consists of a network of eight transmitters placed outside of the working volume (3 m × 3 m × 1.8 m) to track vector bars within the work volume. The transmitters were mounted on 3.05 m high steel columns anchored to the concrete floor. The columns were evenly distributed around the perimeter of the lab space [15 m × 16 m × 10 m (high)], and the working volume was in the center of the lab. Two vector bars were used in the experiment, and each vector bar contains two detectors which define a vector in space (the detectors in a vector bar were separated by 101.6 mm). The two vector bars rigidly mounted to an aluminum rail were used to create a local coordinate frame: in commercial applications, a rigid object remains fixed in the local frame which is tracked by the system.
Four different local frames were created and used to obtain measurements for four configurations of the vector bars; see Figure 1. Both vector bars were parallel to each other, and the distance between them was 375.7 mm for configurations (a–c) and 902.2 mm for configuration (d). The line connecting the two bars for configuration (a) is parallel to that for (b) and similarly for configurations (c) and (d); the line in (a) and (b) is perpendicular to the line in (c) and (d). Each local frame was used to measure M different static poses (M = 12, 27, 12, 20 for frames (a–d), respectively), and at each pose, N repeated measurements in the same experimental conditions were acquired (N ≥ 50,000).
Four configurations (a, b, c, d) of the vector bars used in the experiments to create four different local frames.
The system outputs 6DOF data ∠X,∠Y,∠Z,x,y,z where the first three components are the angles of rotation. From the three angles, a rotation matrix R is constructed as (22)R∠X,∠Y,∠Z=RX∠XRY-∠YRZ∠Z,where RX,Y,Z are matrices of the basic rotations around the axes of a fixed coordinate frame of the tracking system. The last three components of the 6DOF data are coordinates of the origin of the local frame defined by the user (a lower detector in the vector bar labeled as 0 in Figure 1). In addition to the 6DOF pose of the frame, the Cartesian coordinates of the four detectors constituting the two vector bars are also available. They were used as the locations of four fiducials for point-based rigid body registration in some of the computer simulations.
3.2. Data Postprocessing
For each mth pose, the averaged orientation Ravgm was calculated from the repeated measurements or computer generated Rj, j=1,…,N. There are different ways of calculating the average rotation matrix, and in this study, we used the mean rotation in the Euclidean sense. Specifically, Ravg was found as the orthogonal projection of a matrix R-=1/N∑j=1NRj; see equation (3.7) in [35]. Such a matrix retains the property of a rotation matrix and the expected properties of means of numbers, namely, invariance under permutation, biinvariance, and invariance under transposition. It also minimizes the error function based on the Frobenius norm; see [35] for details. Once Ravg was determined, the matrix of small random rotation was determined as(23)ΔRjaj,ρj=RavgTRjfrom which the axis aj and the angle ρj were extracted. Axis-angle representation of any rotation matrix has the following symmetry: (24)Ra,-ρ=R-a,ρ.In our calculations, we restricted the angle ρj to always be positive and allowed the axis aj to flip its direction to maintain the right-handedness of the coordinate frame. Once aj,ρj were known, the covariance matrix of the orientation data Cmq was calculated using (18) and (19) and its eigenvalues Λ1,m,Λ2,m,Λ3,m (where Λ1<Λ2<Λ3) and corresponding eigenvectors e1,m,e2,m,e3,m were evaluated for m=1,…,M. Note that the inverse of large rotation RavgT was already applied in (23) and, therefore, eigenvectors e1,m,e2,m,e3,m of the covariance matrix Cmq are determined in the CAD coordinate frame. In addition, the covariance matrix of positional data CmY and its eigenvectors and eigenvalues were determined where Y is the positional part of N repeated pose measurements in the instrument frame.
Histograms of the angular deviations μj of the instantaneous unit vector wjϑj,φj from the corresponding unit mean vector wavg were created for repeated registrations Rj and selected vectors u as follows from (15). Additionally, 2D histograms of the spherical angles ϑj,φj parametrizing the unit vectors wj were constructed. For each set of unit vectors wj, the corresponding parameters σ and β for the FBK distribution were determined as in [32]. These parameters were then used to generate the plot of the theoretical distribution Gσ,βμ from (20). This theoretical FBK distribution was then compared with the histogram of the angles of deviation μj obtained from the experimental data.
The evaluation of the angular uncertainty σ was repeated many times to get the distribution of σ on a unit sphere. For each average mth orientation, a grid of elevation and azimuth angles ϑi,φl was created, i=1,…,I, l=1,…,L, where I=180 and L=360, corresponding to angular increments of 1°. For each pair of angles, a unit vector ui,lϑi,φl parametrized as in (13) was defined, and all Rj rotations acquired for the mth pose were applied to ui,l using (15). From the resulting set of unit vectors wjϑi,φl, the corresponding angular uncertainty σϑi,φl was calculated as in [32]. The procedure was repeated for each vector ui,lϑi,φl in the I×L grid.
To show a link between the propagation of an object’s pose uncertainty to a POI and the propagation of uncertainty from fiducials to the target in registration problem, the distribution of the angular uncertainty αϑi,φl predicted by (12a) and (12b) was determined using the same I×L grid of vectors ui,lϑi,φl. For each dataset corresponding to Ravgm, the average locations of the four fiducials Y4 were calculated. Then, the moment of inertia matrix M in (10) was evaluated and from its SVD decomposition, Λ2 in (9) was obtained and used in (12a) and (12b). In addition to Λ2, the noise covariance Ψ is required in (12a) and (12b). Since equation (12a) and (12b) can handle only homogenous noise, we arbitrary selected the covariance matrix Ψ of the first fiducial Y1. Once Ψ and Λ2 were determined, the angular uncertainty αϑi,φl was calculated for each vector uϑi,φl in the I×L grid.
As mentioned earlier, the orientation of matrix M is unrelated to the orientation of matrix Ψ. In (12a) and (12b), this is reflected by the fact that the coordinate frame can be rotated so that M is diagonal (only Λ2 is used in the equation) while noise Ψ is not (see off-diagonal elements Ψj,k in the triple summation in (12a) and (12b)). To investigate the effect of relative misalignment between the two matrices, we performed SVD decomposition of the noise matrix (25)Ψ=UψΨ0UψTand then replaced the original matrix with the rotated one (26)Ψω=ΩΨ0ΩT,where rotation matrix Ωa,ω is determined by arbitrary axis a and angle of rotation ω. For ω=0, Ω=I, Ψ=Ψ0, and both matrices M and Ψ are perfectly aligned. Larger angle ω corresponds to larger misalignment between M and Ψ. For each ω, the corresponding Ψω is used in (12a) and (12b) and the distribution of αϑi,φl is recalculated.
4. Results
Figure 2 shows the elements of the covariance matrices Cmq calculated using (19) for each average orientation Ravgm, m ≤ M = 27 for vector bars in configuration (b). Figure 3 shows the spatial orientation of eigenvectors e1,m,e2,m,e3,m corresponding to the ordered eigenvalues Λ1,m,Λ2,m,Λ3,m of Cmq for configurations (a) and (b). Both graphs in Figure 3 are displayed from the same view angles. Eigenvectors corresponding to the largest eigenvalue Λ3 are shown as thick solid lines. Similar distributions of the eigenvectors were obtained for data acquired for vector bars in configurations (c) and (d) and in computer simulations with arbitrary configurations of vector bars.
Elements of the covariance matrix Cmq for orientations Ravgm, m≤M=27. Left column, plots (a–c): variances of the orientation data q (diagonal elements of covariance matrix); right column, plots (d–f): covariance coefficients of the orientation data q.
Spatial distribution of the three eigenvectors e1,m,e2,m,e3,m corresponding to the three eigenvalues Λ1,m,Λ2,m,Λ3,m of the covariance matrix Cmq for orientation Ravgm, m≤M: (a) M = 12, vector bars in configuration (a); (b) M = 27, vector bars in configuration (b). Both graphs are from the same view direction and the axes are unitless. Dotted lines denote eigenvectors e1,m associated with the smallest eigenvalue Λ1,m while solid lines are used to plot eigenvectors e3,m associated with the largest eigenvalue Λ3,m. The thick dark gray line indicates the direction of eigenvector bmin of moment of inertia matrix M for corresponding configuration of vector bars.
The anisotropy of noise perturbing the locations of fiducials was checked by evaluating the ratio of eigenvalues Λ3,m/Λ1,m of the covariance matrices of positional data CmY for all datasets and the median value was equal to 3.27.
The distributions of the angular uncertainty σ are shown in Figure 4 together with the directions of the corresponding eigenvectors e1,m,e2,m,e3,m of covariance matrix Cmq for m=1 and m=2 (elements of Cmq are plotted in Figure 2). Histograms of the deviation angles μ and the associated FBK distributions Gσ,βμ given by (20) are shown in Figure 5 for the same noisy orientation data used to create the plot in Figure 4(b). The angular uncertainty σ and eccentricity β were determined for the directions aligned with eigenvector e1 corresponding to σmax and eigenvector e3 corresponding to σmin in Figure 4(b). The values of σ were 0.37,0.21 [mrad] and 2.86,4.96 [mrad−2] for β.
Directional distribution of the angular uncertainty σ in [mrad]. Both plots are shown from the same view angle. Directions of the three eigenvectors associated with eigenvalues Λ1,Λ2,Λ3 of the covariance matrix Cmq (elements of which are displayed in Figure 2) are also plotted for: (a) m = 1; (b) m = 2.
Histogram of angles μ (indicated by markers) and distribution Gσ,βμ (lines) for two directions of uϑ,φ where the angular uncertainty σϑ,φ shown in Figure 4(b) is equal to (a) σmax; (b) σmin.
Figure 6 displays histograms of angles ϑj,φj parametrizing unit vectors wjϑj,φj in (15) for noisy rotations Rj used to create plots in Figures 4(b) and 5. Average vector wavgϑavg,φavg is aligned with the direction where σmax or σmin are located in Figure 4(b), Δϑj=ϑj-ϑavg and Δφj=φj-φavg.
Histogram of angles ϑj,φj parametrizing unit vectors wϑj,φj plotted in a log scale, where −inf indicates empty bins. The average unit vector wavgϑavg,φavg is aligned with the direction associated with (a) eigenvalue Λ1 from Figure 4(b) (related distribution Gσ,β shown in Figure 5(a)); (b) eigenvalue Λ3 from Figure 4(b) (related distribution Gσ,β shown in Figure 5(b)).
Finally, the plots in Figure 7 show examples of the distribution of angular uncertainty αϑ,φ calculated using (12b) for increasing misalignment angle ω between the moment of inertia matrix M and the noise matrix Ψ. The plots were created for Λ2=104×diag7.55,1.02,3.06 [mm2], diagonal noise matrix (variances) in (26) Ψ0=10-3×diag1.6,2.2,3.5 [mm2], and the axis of rotation Ωa,ω in (26) is set arbitrary to a=0.0384,0.7319,0.6804.
Distribution of uncertainty α in [mrad] for increasing misalignment angle ω in [rad]: (a) ω=0.087; (b) ω=0.785; (c) ω=1.571; (d) ω=2.356. Fixed eigenvectors for eigenvalues of matrix M (λ1<λ2<λ3) are plotted in black; the two major axes defined by locations of αmin and αmax and the third axis perpendicular to them are plotted in gray.
5. Discussion
The average orientations Ravgm were acquired so that they differ substantially from each other. As expected, such wide variations in the poses result in large variations of the corresponding covariance matrices Cmq. The three variances of the orientation data (diagonal elements of Cmq) and the three covariance coefficients (off-diagonal elements) shown in Figure 2 depend on the orientation Ravgm. The graphs in Figure 2 present data for vector bars in configuration (b), but similar variability in the elements of the covariance matrices Cmq was also observed for data acquired for configurations (a), (c), and (d). Despite this variability, the eigenvector e3,m which corresponds to the largest eigenvalue Λ3,m of the covariance matrix Cmq exhibits surprisingly weak dependence on the actual mth orientation for data acquired for a given configuration of vector bars; see solid lines in Figures 3(a) and 3(b). This is in striking contrast to the remaining two eigenvectors (marked with dashed and dotted lines). Note that eigenvectors e3,m are closely aligned with the direction of the eigenvector bmin corresponding to the smallest eigenvalue of the inertia matrix M given by (10).
The theoretical FBK distribution Gσ,βμ as defined by (20) agrees very well with the experimental histogram of deviation angles μ as seen in Figure 5. Further evidence supporting this agreement can be seen in Figure 6. Kent postulated (see (1.6) in [32]) that the relation βσ2→c (where 0≤c<1/2) holds for small noise. Indeed, the larger uncertainty σ and matching smaller eccentricity β in Figure 6(a) should be compared with the smaller σ and larger β in Figure 6(b). Recall that smaller eccentricity β describes a more rounded distribution. The difference between the distributions shown in Figures 6(a) and 6(b) will impact assembly tasks in manufacturing when tight tolerances are required.
The theoretical equations (12a) and (12b) derived for target registration error TREr for point-based, rigid body registration are useful in explaining the propagation of pose uncertainty to a given POI (i.e., a target point). The target point remains fixed relative to the fiducials, that is, the target point relative to the major axes of the moment of inertia is fixed and independent of any imposed rotation of the object. However, when anisotropic noise affects the measurement of the fiducials, different “big” rotations disregarded in Sibson’s analysis of isotropic noise [29] will cause different orientations of the noise matrix relative to the fiducials. This will cause the angular uncertainty αϑ,φ to be dependent on these rotations (note off-diagonal elements of noise matrix Ψj,k in (12b) are affected by increasing misalignment angle ω in (26)), and the consequences of such dependence could be seen in Figure 7. The rotation of the gray axes in Figure 7 follows the pattern observed in the experiment; see Figure 3. Note in Figure 7 that the eigenvector bmin of matrix M corresponding to its smallest eigenvalue λ1 is well aligned with the direction defined by the two poles where α=αmin, similarly to poles defined by σmin and e3 in Figures 4 and 3. Exact matching between the theoretical α provided in (12a) and (12b) and experimental uncertainty σ is not expected since equation (12a) and (12b) was derived for anisotropic, homogenous noise (the same for all fiducials) while noise in experiment was anisotropic and nonhomogenous.
The surprising stability of eigenvector e3, pointing to σmin in the CAD frame and the close alignment of e3 and bmin, have a very useful implication. The direction of bmin depends solely on the selection of the fiducials, implying that the direction of e3 also depends on the selection of fiducials’ locations. Thus, if the location of a given POI associated with a rigid object is critical, it would be beneficial to place fiducials around an object in such a way that the axis of the smallest moment of inertia is parallel to the vector pointing towards this POI in the CAD frame. The principle for finding such configuration of fiducials is outlined in Appendix.
6. Conclusions
Many pose measuring systems derive pose from the measurement of fiducial markers attached to an object. The uncertainty of the fiducials’ locations propagates to the uncertainty of the object’s orientation which, in turn, propagates in an anisotropic way to individual points on the object’s surface. The angular distribution of the orientation uncertainty propagated to a POI depends generally on the object’s orientation. However, the orientation uncertainty in the regions close to the poles defined by the eigenvector corresponding to the smallest eigenvalue of the moment of inertia matrix of fiducials is almost independent of the object’s orientation. These regions are also characterized by the smallest propagated orientation uncertainty. Thus, strategic placement of fiducials around an object ensures that the orientation uncertainty propagated to a given POI is the smallest, regardless of the object’s orientation.
Appendix
The problem of finding optimal locations of fiducials for a given POI does not have a unique solution. In practical applications, the geometry of the rotated object and type of markers tracked by the instrument may impose additional constraints on possible fiducial locations. The general strategy for optimal placement is schematically shown in Figure 8 where gray diamonds depict an arbitrary configuration of J fiducials in the CAD frame.
Optimal placement of fiducials for a given Point of Interest P. Gray diamonds indicate an initial configuration of J fiducials, bmin shows the direction of the principal axis corresponding to the smallest moment of inertia for the initial configuration, and two extra fiducials FJ+1 and FJ+2 are placed on line OP at a distance d on each side of center O. For sufficiently large d, the principal axis bmin of the new configuration for J+2 fiducials will be closely aligned with OP line.
In this general configuration, the principal axis of the smallest moment of inertia is denoted by bmin, P is the location of a POI, and χ is the angle between bmin and P. We assume that the centroid O of J fiducials is located at the origin of the CAD frame; if not, the origin needs to be moved to coincide with O prior to determining line OP. To align bmin with line OP, two extra fiducials FJ+1 and FJ+2 are added and placed on line OP at a distance d on each side of center O. The added pair of fiducials does not change the location of centroid O and does not contribute to the moment of inertia about line OP. However, with increasing d, the two moments of inertia of the J+2 points about the two axes perpendicular to OP will increase. Thus, for a sufficiently large value of d, the moment of inertia about line OP will be the smallest and then bmin, that is, the principal axis of the smallest moment of inertia evaluated for J+2 points, will be closely aligned with line OP. This close alignment should cause the orientation uncertainty propagated to a given POI to be close to the minimum value.
To illustrate this procedure, numerical simulations were performed. Six different randomly selected configurations of J points (4≤J≤7) scattered within a cube of length 700 mm were used. For each configuration, the covariance matrix covXX from (8) was determined and used to calculate the moment of inertia matrix M from (10) and its principal axes and the corresponding moments of inertia. Eigenvector corresponding to the smallest eigenvalue was determined as bmin, and the initial distance dinit, defined as the square root of the largest eigenvalue of M, was calculated using (11) for the initial configuration of J points(A.1)dinit=Λ22+Λ32.Then, the covariance matrix of one of a positional data was chosen as a representative characteristic of noise affecting locations of fiducials and its version Ψ in the rotated frame aligned with eigenvectors of M matrix was evaluated. Then, the nondiagonal noise matrix Ψ and the three eigenvalues Λ12,Λ22,Λ32 of covXX were substituted in (12b) to get the extreme values of the theoretical orientation uncertainty α, that is, αmaxumax and αminumin. Finally, a Point of Interest P was selected. In the worst-case scenario, line OP was aligned with umax, that is, P had the largest uncertainty, and the misalignment angle χ between bmin and OP was close to 90°; in the general case, an arbitrary point P was selected, and the corresponding angle χ was less than 90°. Then, a pair of points FJ+1 and FJ+2 was added and the distance d varied from zero to 3dinit. For each d, the moment of inertia matrix M was determined using all J+2 points, and the corresponding eigenvector bmind, angle χd, and the smallest theoretical uncertainty αmind were evaluated.
Figure 9 shows representative graphs from the six simulations. Parameter gd, displayed in Figure 9(b), is the normalized uncertainty defined as (A.2)gd=αmind-αmin0αmax0-αmin0.For the worst-case selection of POI P, g0=1, while for a generic location of P, 0<g0<1. Negative values of gd indicate that the smallest propagated uncertainty αmin for the optimal configuration of J+2 fiducials (which contains a pair of added fiducials separated by the distance 2d) is smaller than the smallest uncertainty αmin for the initial configuration of J fiducials. The uncertainty of the location of P (including both positional and rotational uncertainty propagated from the 6DOF pose) was calculated using (12a), and to suppress the trivial dependence of variance on length of P, a constant P=350 mm was used in all six simulations.
Results from computer simulations. Six random configurations of fiducials, lines (1–3) correspond to the worst-case selection of Point of Interest P: (a) angle χ; (b) normalized uncertainty g evaluated in (A.2); (c) standard deviation of P evaluated as square root of variance calculated in (12a).
As seen in Figures 9(b) and 9(c), the required distance d should be close to dinit. If the two extra fiducials cannot be placed at the distance dinit from O due to geometrical constrains, then the general procedure outlined above needs to be modified; for example, two pairs of fiducials (FJ+1,FJ+2 and FJ+3,FJ+4) could be placed on line OP at equal distances d1,3<dinit and d2,4<dinit.
Disclosure
Certain trade names and company products are mentioned in the text to adequately describe the experimental procedure. In no case does it imply NIST recommendation or endorsement, nor does it imply that the products are necessarily the best available for the purpose.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
SuS.-F.LeeC. S. G.Manipulation and Propagation of Uncertainty and Verification of Applicability of Actions in Assembly Tasks1992226137613892-s2.0-002694538310.1109/21.199463Zbl0825.93492BarfootT. D.FurgaleP. T.Associating uncertainty with three-dimensional poses for use in estimation problems20143036796932-s2.0-8490215359510.1109/TRO.2014.2298059SaksenaA.NammotoT.KinugawaJ.KosugeK.Automatic path planning for 3-D assembly system using configuration space and CAD modelProceedings of the 12th IEEE International Conference on Mechatronics and Automation, ICMA 2015August 2015China135513612-s2.0-8495523791210.1109/ICMA.2015.7237682JiangX.KonnoA.UchiyamaM.A vision-based endpoint trajectory and vibration control for flexible manipulatorsProceedings of the 2007 IEEE International Conference on Robotics and Automation, ICRA'07April 2007Italy342734322-s2.0-3634903277410.1109/ROBOT.2007.364002BrignoneL.HowarthM.A geometrically validated approach to autonomous robotic assemblyProceedings of the IROS 2002: IEEE/RSJ International Conference on Intelligent Robots and SystemsLausanne, Switzerland1626163110.1109/IRDS.2002.1043988MezouarY.PratsM.MartinetP.External Hybrid Vision/Force Control, presented at the Int. Conference on Advanced Robotics ICRAProceedings of the External Hybrid Vision/Force Control, presented at the Int. Conference on Advanced Robotics ICRA2007Jeju, KoreaPratsM.MartinetP.Del PobilA. P.LeeS.Robotic execution of everyday tasks by means of external vision/force control2008132532662-s2.0-4644913693310.1007/s11370-007-0008-xHuangS.YamakawaY.SenooT.IshikawaM.A direct visual servo scheme based on simplified interaction matrix for high-speed manipulationProceedings of the 2012 IEEE International Conference on Robotics and Biomimetics, ROBIO 2012December 2012China195019552-s2.0-8487648591010.1109/ROBIO.2012.6491253ParkH.BaeJ.-H.ParkJ.-H.BaegM.-H.ParkJ.Intuitive peg-in-hole assembly strategy with a compliant manipulatorProceedings of the 2013 44th IEEE International Symposium on Robotics, ISR 2013October 2013Republic of Korea2-s2.0-8489321954910.1109/ISR.2013.6695699QiaoH.TsoS. K.Three-step precise robotic peg-hole insertion operation with symmetric regular polyhedral objects19993715354135632-s2.0-003320763110.1080/002075499190176LiuZ.XieY.XuJ.ChenK.Laser tracker based robotic assembly system for large scale peg-hole partsProceedings of the 4th Annual IEEE International Conference on Cyber Technology in Automation, Control and Intelligent Systems, IEEE-CYBER 2014June 2014China5745782-s2.0-8491059342410.1109/CYBER.2014.6917527ParkD. I.ParkC.DoH.ChoiT.KyungJ. H.Assembly phase estimation in the square peg assembly processProceedings of the the Int. Conference on Control, Automation and Systems2012Jeju, KoreaChhatparS. R.BranickyM. S.Particle filtering for localization in robotic assemblies with position uncertaintyProceedings of the IEEE IRS/RSJ International Conference on Intelligent Robots and Systems, IROS 2005August 2005can209521022-s2.0-7995801017110.1109/IROS.2005.1545286ChhatparS.BranickyM.Search strategies for peg-in-hole assemblies with position uncertaintyProceedings of the RSJ/IEEE International Conference on Intelligent Robots and SystemsMaui, HI, USA1465147010.1109/IROS.2001.977187KimJ. Y.KimW. S.ChoH. S.Misalignment estimation and compensation for robotic assembly with uncertainty20052333553642-s2.0-1884443163910.1017/S0263574704000979UsubamatovR.LeongK. W.Analyses of peg-hole jamming in automatic assembly machines20113143583622-s2.0-8005351617410.1108/01445151111172943UsubamatovR.AdamS. A.HarunA.Analyzing the jamming of parts on the shaft in assembly processes20123243403462-s2.0-8486756644110.1108/01445151211262401FitzpatrickJ. M.WestJ. B.MaurerC. R.Jr.Predicting error in rigid-body point-based registration199817569470210.1109/42.7360212-s2.0-0032173355WestJ. B.FitzpatrickJ. M.TomsS. A.MaurerC. R.Jr.MaciunasR. J.Fiducial point placement and the accuracy of point-based, rigid body registration20014848108172-s2.0-003506142710.1097/00006123-200104000-00023SeginerA.Rigid-body point-based registration: The distribution of the target registration error when the fiducial registration errors are given20111543974132-s2.0-7995877066110.1016/j.media.2011.01.001MoghariM. H.AbolmaesumiP.Distribution of target registration error for anisotropic and inhomogeneous fiducial localization error20092867998132-s2.0-6624913889610.1109/TMI.2009.2020751ShamirR. R.JoskowiczL.ShoshanY.Fiducial optimization for minimal target registration error in image-guided neurosurgery20123137257372-s2.0-8485799111510.1109/TMI.2011.2175939FranaszekM.ShahM.CheokG. S.SaidiK. S.The axes of random infinitesimal rotations and the propagation of orientation uncertainty20157268762-s2.0-8494955839810.1016/j.measurement.2015.04.020MaB.MoghariM. H.EllisR. E.AbolmaesumiP.Estimation of optimal fiducial target registration error in the presence of heteroscedastic noise20102937087232-s2.0-7774927364610.1109/TMI.2009.2034296WestJ. B.MaurerC. R.Jr.Designing optically tracked instruments for image-guided surgery20042355335452-s2.0-254247614010.1109/TMI.2004.825614WangM.SongZ.Improving target registration accuracy in image-guided neurosurgery by optimizing the distribution of fiducial points20095126312-s2.0-6534908315510.1002/rcs.227RiboldiM.BaroniG.SpadeaM. F.TagasteB.GaribaldiC.CambriaR.OrecchiaR.PedottiA.Genetic evolutionary taboo search for optimal marker placement in infrared patient setup20075219, article no. 006581558302-s2.0-3474889666310.1088/0031-9155/52/19/006ArunK. S.HuangT. S.BlosteinS. D.Least-squares fitting of two 3D point sets19879569870010.1109/tpami.1987.4767965SibsonR.Studies in the robustness of multidimensional scaling: perturbational analysis of classical scaling1979412217229MR547248WilesA. D.LikholyotA.FrantzD. D.PetersT. M.A statistical model for point-based target registration error with anisotropic fiducial localizer error20082733783902-s2.0-4014909593410.1109/TMI.2007.908124BinghamC.An antipodally symmetric distribution on the sphere197421201122510.1214/aos/1176342874MR0397988Zbl0297.62010KentJ. T.The Fisher-Bingham distribution on the sphere19824417180MR655376Zbl0485.62015WatsonG. A.Fitting enclosing cylinders to data in Rn200643218919610.1007/s11075-006-9054-2MR2292202SchmittR.NischS.SchönbergA.DemeesterF.RendersS.Performance evaluation of iGPS for industrial applicationsProceedings of the 2010 International Conference on Indoor Positioning and Indoor Navigation, IPIN 2010September 2010Switzerland2-s2.0-7865075032110.1109/IPIN.2010.5647630MoakherM.Means and averaging in the group of rotations2002241116MR192054810.1137/S0895479801383877Zbl1028.470142-s2.0-0037281553