What is the accuracy of SVD in 3d transformations

Question

I have a triangle $x$ with points $x_1,x_2,x_3\in\mathbb{R}^3$ that were measured at one location. The triangle was then transformed to $\bar x$ where the points were measured again as $\bar{x}_1,\bar{x}_2,\bar{x}_3$. The measurements have some small margin of error for every axis, so the transformation is not absolutely rigid but almost.

I am using Accord.NET to calculate the transformation matrix $A$ from the triangles $x$ and $\bar x$ like described in this post. To verify the result, I test if $Ax=\hat x$ is equal to $\bar x$.

For my "simple" small whole-number manual test, $\hat x$ is equal to $\bar x$. But for a real world example, $\hat x$ is pretty far off as it seems.

Now I wonder if there is an error in my interpretation of the aforementioned posting, or in my code, or if this is within the margin of computational error when using SVD.

This is my code:

var pc = previous.Center.ToArray(); // centroid as double[3]
var nc = next.Center.ToArray(); // centroid as double[3]

var px = previous.ToArray(); // triangle as double[3,3]
var nx = next.ToArray(); // triangle as double[3,3]

var py = Elementwise.Subtract(px, Matrix.Transpose(Matrix.Create(pc, pc, pc)));
var ny =  Elementwise.Subtract(nx, Matrix.Transpose(Matrix.Create(nc, nc, nc)));

var H = Matrix.DotWithTransposed(py, ny);
var svd = new Decompositions.SingularValueDecomposition(H);

var U = svd.LeftSingularVectors;
var V = svd.RightSingularVectors;

var R = Matrix.DotWithTransposed(V, U);
var t = Elementwise.Subtract(pc, Matrix.Dot(R, nc));

return new Matrix3D(
  R[0, 0], R[0, 1], R[0, 2], 0,
  R[1, 0], R[1, 1], R[1, 2], 0,
  R[2, 0], R[2, 1], R[2, 2], 0,
  t[0], t[1], t[2], 1);

Calculating $A$ from $$ x_1 = (1, 1, 1), x_2 = (1, 4, 0), x_3 = (5, 2, 0) $$ and $$ \bar x_1 = (7, 7, 0), \bar x_2 = (4, 7, 1), \bar x_3 = (7, 3, 1) $$ $Ax$ yields $\bar x$ pretty much exactly.

But calculating $A$ from a real-world example which would be $$ x_1 = (5776.461, 3325.486, 1511.157)\\ x_2 = (5774.713, 3218.453, 1442.178)\\ x_3 = (5706.623, 3332.699, 1398.096) $$ and $$ \bar x_1 = (5775.979, 3326.12, 1511.192)\\ \bar x_2 = (5774.248, 3218.872, 1442.361)\\ \bar x_3 = (5706.302, 3332.975, 1398.004) $$ $Ax$ yields $$ \hat x_1 = (5778.82, 3320.916, 1512.8)\\ \hat x_2 = (5777.015, 3214.044, 1443.574)\\ \hat x_3 = (5708.845, 3328.376, 1399.839) $$

Is that because of the similarity from $x$ and $\bar x$, or because of the non-rigid transformation due to measurement errors, within error margin of the algorithm or an error in my code?

UPDATE

Matrix3D is a .NET framework class that has a transposed layout compared to the matrix layout in the given answer. Therefore, you have to transpose $R$ before copying the values to the Matrix3D object.

Answer 1

Let's go ahead and look at that code. I'm going to have to make some assumptions, like that Matrix.create takes as input three double[3] values and makes a matrix with those as the rows of the matrix (and that multiplying matrices by vectors is done as $Mv$, with $v$ a column vector, rather than as $vM$ (with $v$ a row vector).

var pc = previous.Center.ToArray(); // centroid as double[3]
var nc = next.Center.ToArray(); // centroid as double[3]

var px = previous.ToArray(); // triangle as double[3,3]
var nx = next.ToArray(); // triangle as double[3,3]

var py = Elementwise.Subtract(px, Matrix.Transpose(Matrix.Create(pc, pc, pc)));
var ny =  Elementwise.Subtract(nx, Matrix.Transpose(Matrix.Create(nc, nc, nc)));

OK, so you've moved both coordinate systems to the respective triangle centers (or moved both triangles to the centers of their respective coordinate systems, depending on which way you like to do things). I'm a little uncertain about the use of Transpose here: it's easy to imagine that this should not be there.

Assuming that it's correct, though, you now have, in matrices py and ny, the vectors representing the rays from the triangle centroid to each of the three vertices; let's call these "offset vectors". If there were only two of these, you could say that they were the "axis" vectors for two coordinate systems (one in the plane of each triangle). But now you construct a matrix $H$ via $$H = P \cdot N^t,$$ and I have to ask myself, "What is $H$ supposed to be???" We'll come back to this, but for now, it looks as if $H$ is supposed to be something that (more or less) converts the new offset vectors to the old offset vectors.

var H = Matrix.DotWithTransposed(py, ny);
var svd = new Decompositions.SingularValueDecomposition(H);

var U = svd.LeftSingularVectors;
var V = svd.RightSingularVectors;

var R = Matrix.DotWithTransposed(V, U);

Regardless, you then factor $H$ as $H = U D V^t$, and hoping that $D$ is nearly the identity, you compute $R = V U^t$, which would be the inverse of $H$ if only $D$ had been the identity. So $R$ is roughly the orthogonal matrix closest to $H^{-1}$. So $R$ should transform old offsets to new offsets. The problem is that $R$, applied to the new triangle center, won't give the previous center. (Actually, I'd say there are two problems, one of which is that if $R$ is supposed to be the previous-to-new transformation, you'd want to look at Elementwise.Subtract(nc, Matrix.Dot(R, pc)), i.e., apply $R$ to the previous center and see how close it comes to the NEW center. So the next line is presumably backwards:

var t = Elementwise.Subtract(pc, Matrix.Dot(R, nc))

return new Matrix3D(
  R[0, 0], R[0, 1], R[0, 2], 0,
  R[1, 0], R[1, 1], R[1, 2], 0,
  R[2, 0], R[2, 1], R[2, 2], 0,
  t[0], t[1], t[2], 1);

And now we see that we really are working with transformations that map column vectors (with a $1$ at the end to make everything work in homogeneous coordinates) to column vectors. OK.

I promised to return to the matter of the definition of $H$. Without writing down a lot of stuff, I can't be sure of this, but it sure feels as if $H$ should be $H = P N^{-1}$. Now that doesn't quite make sense, because in general the vectors in $N$ will not form a basis (and certainly won't if there's no error in the computations). Still, I have my doubts about using $N^t$ as an approximation of $N^{-1}$. I'm not so worried about scale, because you'll be dropping the diagonal matrix from the SVD in a moment, but rather about orthogonality... the less that the vectors in $P$ and $N$ form an orthonormal basis, the more peculiar this seems to me.

Regardless, I'm going to assume that if I worked through things, it'd make more sense, and suggest that the major error is in the line

var t = Elementwise.Subtract(pc, Matrix.Dot(R, nc))

which should probably be

var t = Elementwise.Subtract(nc, Matrix.Dot(R, pc))

Best of luck!

Answer 2

After long hours of comparing the results of my code to the same algorithm coded in Python, there appear to be two errors at work:

First, I assumed the Matrix3D object from the .NET framework utilizes the same layout like the transformation matrix in the linked answer. Apparently it does not, but is transposed. I only noticed this when I looked at the documentation seeing the translation vector is "lying" in the 4th row instead of "standing" in the 4th column. By try-and-error I transposed the matrix $R$ and the results got a lot better, but still somewhat off.

The final solution was the hint given in John Hughes' answer that the calculation of the translation vector is wrong, which it indeed is when referring to the paper which I found here (see section II, step 10).

So the correct code is

var pc = previous.Center.ToArray(); // centroid as double[3]
var nc = next.Center.ToArray(); // centroid as double[3]

var px = previous.ToArray(); // triangle as double[3,3]
var nx = next.ToArray(); // triangle as double[3,3]

var py = Elementwise.Subtract(px, Matrix.Transpose(Matrix.Create(pc, pc, pc)));
var ny =  Elementwise.Subtract(nx, Matrix.Transpose(Matrix.Create(nc, nc, nc)));

var H = Matrix.DotWithTransposed(py, ny);
var svd = new Decompositions.SingularValueDecomposition(H);

var U = svd.LeftSingularVectors;
var V = svd.RightSingularVectors;

var R = Matrix.DotWithTransposed(V, U);
var t = Elementwise.Subtract(nc, Matrix.Dot(R, pc));

// Windows.Media3D.Matrix3D has a transposed layout
// I chose to transpose R so the indices match conventional r/c notation
// instead of swapping row and col indices when copying R to the new object
R = Matrix.Transpose(R);

return new Matrix3D(
  R[0, 0], R[0, 1], R[0, 2], 0,
  R[1, 0], R[1, 1], R[1, 2], 0,
  R[2, 0], R[2, 1], R[2, 2], 0,
  t[0], t[1], t[2], 1);