SLAM Evaluation 之轨迹对齐论文翻译"Closed-Form Solution of Absolute Orientation Using Orthonormal Matrices"

                Closed-Form Solution of Absolute Orientation

                             Using Orthonormal Matrices

                           用正交矩阵求绝对方向的闭式解

原文下载地址:
http://graphics.stanford.edu/~smr/ICP/comparison/horn-hilden-orientation-josa88.pdf
作者: B.K.P. Horn

Abstract: Finding the relationship between two coordinate systems using pairs of measurements of the coordinates of a number of points in both systems is a classic photogrammetric task. It finds applications in stereophotogrammetry and in robotics. We present here a closed-form solution to the least-squares problem for three or more points. Currently, various empirical, graphical and numerical iterative methods are in use. Derivation of a closed-form solution can be simplified by using unit quaternions to represent rotation, as was shown in an earlier paper1. Since orthonormal matrices are more widely used to represent rotation, we now present a solution using 3 × 3 matrices. Our method requires the computation of the square-root of a symmetric matrix. We compare the new result with an alternate method where orthonormality is not directly enforced. In this other method a best fit linear transformation is found and then the nearest orthonormal matrix chosen for the rotation.
摘要：使用两个系统中多个点的坐标测量对来找到两个坐标系之间的关系是一个经典的摄影测量任务。它在立体摄影测量和机器人技术中得到应用。我们在这里提出了一个封闭形式的解决方案，解决了三个或更多点的最小二乘问题。目前，使用各种经验，图形和数值迭代方法。通过使用单位四元数来表示旋转，可以简化封闭形式解决方案的派生，如前面的论文1所示。由于正交矩阵更广泛地用于表示旋转，我们现在提出使用3×3矩阵的解决方案。我们的方法需要计算对称矩阵的平方根。我们将新结果与不直接强制执行正交性的替代方法进行比较。在该另一种方法中，找到最佳拟合线性变换，然后选择最接近的正交矩阵用于旋转。

We note that the best translational offset is the difference between the centroid of the coordinates in one system and the rotated and scaled centroid of the coordinates in the other system. The best scale is equal to the ratio of the root-mean-square deviations of the coordinates in the two systems from their respective centroids. These exact results are to be preferred to approximate methods based on measurements of a few selected points.
我们注意到，最佳平移偏移是一个系统中坐标的质心与另一个系统中坐标的旋转和缩放质心之间的差异。最佳比例等于两个系统中坐标与各自质心的均方根偏差之比。这些精确结果优选用于基于几个选定点的测量值的近似方法。

1. Origin of the Problem.

Suppose that we are given the coordinates of a number of points as measured in two different Cartesian coordinate systems (see Fig. 1). The photogrammetric problem of recovering the transformation between the two systems from these measurements is referred to as that of absolute orientation2,3,4. It occurs in several contexts, foremost in relating a stereo model developed from pairs of aerial photographs to a geodetic coordinate system. It also is of importance in robotics, where measurements in a camera coordinate system must be related to coordinates in a system attached to a mechanical manipulator. Here one speaks of the determination of the “hand-eye” transform5.
假设我们给出了在两个不同的笛卡尔坐标系中测量的多个点的坐标（见图1）。从这些测量中恢复两个系统之间的变换的摄影测量问题被称为绝对定向的摄影测量问题[2,3,4]。它发生在几个环境中，最重要的是将基于航空照片对开发而来的立体模型与大地坐标系相关联。它在机器人技术中也很重要，其中摄像机坐标系中的测量必须与连接到机械操纵器的系统中的坐标相关联。这里有人称作“手眼”变换关系的确定5。
            

1.A Previous Work.

The problem of absolute orientation is usually treated in an empirical, graphic or numerical, iterative fashion2,3,4. Thompson6 gives a solution to this problem when exactly three points are measured. His method, as well as the simpler one of Schut7, depends on selective neglect of the extra constraints available when all coordinates of three points are known, as dicussed later. Schut uses unit quaternions and arrives at a set of linear equations. A simpler solution, that does not require the solution of a system of linear equations, was presented in a precursor of this paper1. These three methods all suffer from the defect that they cannot handle more than three points. Perhaps more importantly, they do not even use all of the information available from the three points.
绝对取向的问题通常以经验，图形或数字，迭代方式处理[2,3,4]。当准确测量三个点时，Thompson[6]可以解决这个问题。他的方法，以及Schut[7]中较简单的方法，取决于当三个点的所有坐标都已知时可选择地忽略可用的额外约束，如后面所述。 Schut使用单位四元数并得到一组线性方程。一个更简单的解决方案，不需要线性方程组的解决方案，在本文的前言1中提出。这三种方法都存在缺点，即它们不能处理三个以上的点。也许更重要的是，他们甚至没有使用三点中的所有可用信息。

Oswal and Balasubramanian[8] developed a least-squares method that can handle more than three points, but their method does not enforce the orthonormality of the rotation matrix. Instead, they simply find the bestfit linear transform. An iterative method is then used to “square up” the result—bringing it closer to being orthonormal. Their method for doing this is iterative (and without mathematical justification). In addition, the result obtained is not the solution of the original least-squares problem.
We study their approach in section 4 using a closed-form solution for the nearest orthonormal matrix derived in section 3.F. This is apparently not entirely novel, since an equivalent problem has been treated in the psychological literature15−21 (as recently pointed out to us by Takeo Kanade). The existing methods, however, cannot deal with a singular matrix. We extend our method to deal with the case where the rank deficiency of the matrix is one. This is an important extension, since the matrix will be singular when either of the sets of measurements are coplanar, as will always happen when there are only three measurements.
Oswal和Balasubramanian[8]开发了一种可以处理三个以上点的最小二乘法，但是他们的方法并没有强制执行旋转矩阵的正交性。相反，他们只是找到最佳拟合线性变换。然后使用迭代方法“平方化”结果 - 使其更接近正交。他们这样做的方法是迭代的（没有数学证明）。另外，获得的结果不是原始最小二乘问题的解。
我们在第4节中研究了他们的方法，使用封闭形式的解决方案，得到了最接近3.F中的的正交矩阵。这显然不完全是新颖的，因为在心理学文献[15-21]中已经处理了同等的问题（最近由Takeo Kanade向我们指出）。然而，现有方法不能处理奇异矩阵。我们扩展我们的方法来处理矩阵的秩缺陷为1的情况。这是一个重要的扩展，因为当任何一组测量值共面时矩阵将是奇异的，当只有三次测量时总会发生这种情况。

The main result presented here, however, is the closed-form solution to the least-squares problem of absolute orientation. Our new result can be applied in the special case when one or the other of the sets of measurements happens to be coplanar. This is important, because sometimes only three points are available, and three points are, of course, always coplanar. The solution we present differs from the schemes discussed at the beginning of this section in that it does not selectively neglect information provided by the measurements—it uses all of it.
然而，这里给出的主要结果是绝对方向的最小二乘问题的封闭形式解。当一组或另一组测量恰好是共面时，我们的新结果可以应用于这种特殊情况。这很重要，因为有时只有三个点可用，当然，三个点总是共面的。我们提出的解决方案与本节开头讨论的方案的不同之处在于它不会选择性地忽略测量提供的信息 - 它使用了所有这些信息。

We should point out that a version of this problem has been solved by Farrel & Stuelpnagel[9] (as C.J. Standish pointed out to us after reading a draft of our paper). Their solution only applies, however, when neither of the sets of measurements is coplanar. We also learned recently that S. Arun, T. Huang and S.D. Blostein independently developed a solution to an equivalent problem[10]. They use singular value decomposition of an arbitrary matrix instead of the eigenvalue-eigenvector decomposition of a symmetric matrix inherent in our approach.
我们应该指出，Farrel＆Stuelpnagel[9]已经解决了这个问题的一个版本（正如C.J. Standish在阅读了我们的论文草稿后向我们指出的那样）。然而，他们的解决方案仅适用于两组测量均不共面的情况。我们最近还了解到S. Arun，T。Huang和S.D. Blostein独立开发了一个解决等效问题的解决方案[10]。它们使用任意矩阵的奇异值分解，而不是我们方法中固有的对称矩阵的特征值 - 特征向量分解。

1.B Minimum Number of Points.

The transformation between two Cartesian coordinate systems can be thought of as the result of a rigid-body motion and can thus be decomposed into a rotation and a translation. In stereophotogrammetry, in addition, the scale may not be known. There are obviously three degrees of freedom to translation. Rotation has another three (direction of the axis about which the rotation takes place plus the angle of rotation about this axis). Scaling adds one more degree of freedom. Three points known in both coordinate systems provide nine constraints (three coordinates each), more than enough to allow determination of the seven unknowns, as shown, for example, in reference 1. By discarding two of the constraints, seven equations in seven unknowns can be developed that allow one to recover the parameters.

1.C Least Sum of Squares of Errors.

In practice, measurements are not exact, and so greater accuracy in determining the transformation parameters will be sought after by using more than three points. We no longer expect to be able to find a transformation that maps the measured coordinates of points in one system exactly into the measured coordinates of these points in the other. Instead, we minimize the sum of the squares of the residual errors. Finding the best set of transformation parameters is not easy. In practice, various empirical, graphical and numerical procedures are in use2,3,4. These are all itera-tive in nature. That is, given an approximate solution, such a method is applied repeatedly until the remaining error becomes negligible.

At times information is available that permits one to obtain so good an initial guess of the transformation parameters, that a single step of the iteration brings one close enough to the true solution of the least-squares problem for all practical purposes; but this is rare.

1.D Closed-Form Solution.

In this paper we present a closed-form solution to the least-squares problem of absolute orientation, one that does not require iteration. One advantage of a closed-form solution is that it provides us in one step with the best possible tranformation, given the measurements of the points in the two coordinate systems. Another advantage is that one need not find a good initial guess, as one does when an iterative method is used. A solution of this problem was presented previously that uses unit quaternions to represent rotations1. The solution for the desired quaternion was shown to be the eigenvector of a symmetric 4 × 4 matrix associated with the largest positive eigenvalue. The elements of this matrix are simple combinations of sums of products of coordinates of the points. To find the eigenvalues, a quartic equation has to be solved whose coefficients are sums of products of elements of the matrix. It was shown that this quartic is particularly simple, since one of its coefficients is zero. It simplifies even more when only three points are used.

1.E Orthonormal Matrices.

While unit quaternions constitute an elegant representation for rotation, most of us are more familiar with the use of proper orthonormal matrices for this purpose. Working directly with matrices is difficult, because of the need to deal with six nonlinear constraints that ensure that the matrix is orthonormal. We nevertheless are able to derive a solution for the rotation matrix using direct manipulation of 3 × 3 matrices. This closed-form solution requires the computation of the positive semi-definite squareroot of a positive semi-definite matrix. We show in section 3.C how such a square-root can be found, once the eigenvalues and eigenvectors of the matrix are available. Finding the eigenvalues requires the solution of a cubic equation.

The method discussed here finds the same solution as does the method presented earlier that uses unit quaternions to represent rotation, since it minimizes the same error sum1. We present the new method only because the use of orthonormal matrices is so widespread. We actually consider the solution using unit quaternions to be more elegant.

1.F Symmetry of the Solution.

Let us call the two coordinate systems “left” and “right.” A desirable property of a solution method is that when applied to the problem of finding the best transformation from the left to the right system, it gives the exact inverse of the best transformation from the left to the right system. It was shown in reference 1 that the scale factor has to be treated in a particular way to guarantee that this happens. The method we develop here for directly computing the rotation matrix gives two apparently different results when applied to the problem of finding the best transformation from left to right and the problem of finding the best transformation from right to left. We show that these two results are in fact different forms of the same solution and that our method does indeed have the sought after symmetry property.

1.G The Nearest Orthonormal Matrix.

Since the constraint of orthonormality leads to difficulties, some in the past have chosen to find a 3 × 3 matrix that fits the data best in a leastsquares sense without constraint on its elements8. The result will typically not be orthonormal. If the data is fairly accurate, the matrix may be almost orthonormal. In this case, we might wish to find the “nearest” orthonormal matrix. That is, we wish to minimize the sum of the squares of differences between the elements of the matrix obtained from the measurements and an ideal orthonormal matrix. Iterative methods exist for finding the nearest orthonormal matrix.

A closed-form solution, shown in section 3.F, again involves squareroots of 3 × 3 symmetric matrices. The answer obtained this way is different, however, from the solution that minimizes the sum of the squares of the residual errors. In particular, it does not have the highly desirable symmetry property discussed above, and it requires the accumulation of a larger number of sums of products of coordinates of measured points.

2. Solution Methods

As we shall see, the translation and the scale factor are easy to determine once the rotation is known. The difficult part of the problem is finding the rotation. Given three noncollinear points, we can easily construct a useful triad in each of the left and the right coordinate systems1 (seeFig. 2). Take the line from the first to the second point to be the direction of the new x-axis. Place the new y-axis at right angles to the new x-axis in the plane formed by the three points. The new z-axis is then made orthogonal to the x- and y-axis with orientation chosen to satisfy the right-hand rule. This construction is carried out in both left and right systems. The rotation that takes one of these constructed triads into the other is also the rotation that relates the two underlying Cartesian coordinate systems. This rotation is easy to find, as is shown in reference 1.

This ad hoc method constitutes a “closed-form” solution for finding the rotation given three points. Note that it uses the information from the three points selectively. Indeed, if we renumber the points, we obtain a different rotation matrix (unless the data happens to be perfect). Also note that the method cannot be extended to deal with more than three points. Even with just three points we should really attack this problem by means of a least-squares method, since there are more constraints than unknown parameters. The least-squares solution for translation and scale will be given in subsections 2.B and 2.C. The optimum rotation will be found in section 4.

2.A Finding the Translation.

Let there by n points. The measured coordinates in the left and right coordinate system will be denoted by

                                                                  

respectively, where i ranges from 1 to n. We are looking for a transformation of the form