# Fitting one set of points to another by a rigid motion

I'm not really sure how to explain this problem clearly, so please bear with me. I have a basis of 3 orthonormal unit vectors and a position, a standard 4x4 transform matrix in computer graphics.

Also I have several points (offsets) in that space that I transform to world space. The points are then perturbed slightly and now I wish to find the new basis that is the closest to representing the perturbed points.

It's not exactly like finding principle components, because I want it to respect the original offsets. If that makes sense. Like springs from each new point to their respective starting positions. I think the answer lies in solving a least-squares-problem, but I looked into it an now my head hurts.

Can somebody explain it simply for me. I would prefer a closed form solution, but an iterative one would be okay too. Thanks very much

• More details of your problem need to be stated. Apparently you have a rigid transfomation ("3 orthonormal unit vectors and a position") consisting of an orthogonal transformation and a translation. Probably you are asking how to modify the orthogonal transformation, leaving the translation portion fixed, so that the newly perturbed points are best approximated (by applying the adjusted rigid transformation to the original points). Spell out details: name the transforms, the translation, the points. Most importantly state what criterion (least squares?) determines the best fit.
– hardmath
Apr 16, 2013 at 15:55
• Yes sorry, I'm not sure about some of the terminology and the best way to describe it. Actually, I do want to modify position too. Orthogonal basis + position is an affine transformation right? So I have an affine transformation, A, which transforms a small number of points, and I want to find A' so that the transformation 'follows' the points as best as possible after they are perturbed. I hope that is clearer. Apr 17, 2013 at 0:16
• This is a "classic" problem with a "classical" name, the orthogonal Procrustes problem, my favorite application of SVD (singular value decomposition).
– hardmath
Apr 17, 2013 at 2:29
• Yeah, give me a bit to think about writing it up nicely.
– hardmath
Apr 17, 2013 at 3:12
• I know that this is about 2 years too late, but I have some code up on Github that uses the SVD to solve this kind of problem. Maybe it will be of use to you? Here is the link Dec 18, 2015 at 13:48

Inge Söderkvist (2009) has a nice write-up of solving the Rigid Body Movement Problem by singular value decomposition (SVD).

Suppose we are given 3D points $\{x_1,\ldots,x_n\}$ that after perturbation take positions $\{y_1,\ldots,y_n\}$ respectively. We seek a rigid "motion", i.e. a rotation $R$ and translation $d$ combined, applied to points $x_i$ that minimizes the sum of squares of "errors":

$$\sum_{i=1}^n || Rx_i + d - y_i ||^2$$

Think of the points $x_i,y_i$ as columns, and of rotation $R$ as a $3\times 3$ orthogonal matrix. Strictly speaking we require $\det(R)=1$ because as a continuous motion a rotation preserves the "orientation" of the points, i.e. will not involve reflecting them.

The first step is to subtract off the respective means $\overline{x},\overline{y}$ from points $x_i,y_i$, which has the effect of "eliminating" (for now) the unknown translation $d$. That is, the problem becomes:

$$\min_{R\in SO(3)} || RA - B ||_F$$

where $A = [x_1 - \overline{x},\ldots,x_n - \overline{x}]$ and $B = [y_1 - \overline{y},\ldots,y_n - \overline{y}]$. Here $SO(3)$ denotes the special orthogonal group of rotation matrices in 3D we are allowed to choose $R$ from, and the matrix norm $F$ here denotes the Frobenius norm, i.e. the square root of the sum of squares of matrix entries (like a Euclidean norm, but on matrix entries).

Now $A,B$ are both $3\times n$ matrices. The minimization above is an orthogonal Procrustes problem allowing only rotation matrices. The solution $R$ is given by taking the singular value decomposition of the "covariance" matrix $C = BA^T$:

$$C = U S V^T$$

where $U,V$ are orthogonal matrices and $S = \text{diag}(\sigma_1,\sigma_2,\sigma_3)$ is the diagonal matrix of singular values $\sigma_1 \ge \sigma_2 \ge \sigma_3 \ge 0$. Numerical linear algebra packages like Matlab and Octave will compute the SVD for you.

Once we have the SVD, define $R = U \text{diag}(1,1,\pm 1) V^T$ where the sign in the middle factor is chosen to make $\det(R) = 1$. Ordinarily a real world application will have $\det(UV^T) = 1$, and thus the sign chosen would be positive. If not, it means the best orthogonal (distance preserving) fit to the new points involves a reflection, and it suggests checking the data for mistakes.

Finally we define the translation $d = \overline{y} - R\overline{x}$. Done!

The variant of orthogonal Procrustes problems where only rotations are allowed is also the subject of another Wikipedia article on the Kabsch algorithm. Notation in the Wikipedia articles differs from ours in multiplying by $R$ on the right, rather than (as here) on the left.

• Fantastic! Thank you so much for taking the time to write that up. Very useful indeed. Apr 17, 2013 at 11:28
• Is there a standard way to solve the "weighted" version of this problem, where the noises on the various points have different variances? Mar 17, 2017 at 22:10
• @nibot: Have a look at T. Vikland's dissertation (2006), which presents some exposition and published articles on the weighted orthogonal Procrustes problem (WOPP): "The solution to a WOPP can not be computed as easily as for an OPP. Additionally, a WOPP can have several local minima."
– hardmath
Mar 18, 2017 at 14:12
• There's another similar and useful writeup on the SVD approach from ETHZ: igl.ethz.ch/projects/ARAP/svd_rot.pdf Apr 29, 2020 at 19:19