Matlab calculate 3D similarity transformation. fit

Question

How can I calculate in MatLab similarity transformation between 4 points in 3D? I can calculate transform matrix from

T*X = Xp,

but it will give me affine matrix due to small errors in points coordinates. How can I fit that matrix to similarity one? I need something like fitgeotrans, but in 3D

Thanks

Answer 1

If I am interpreting your question correctly, you seek to find all coefficients in a 3D transformation matrix that will best warp one point to another. All you really have to do is put this problem into a linear system and solve. Recall that warping one point to another in 3D is simply:

A*s = t

s = (x,y,z) is the source point, t = (x',y',z') is the target point and A would be the 3 x 3 transformation matrix that is formatted such that:

A = [a00 a01 a02]
    [a10 a11 a12] 
    [a20 a21 a22]

Writing out the actual system of equations of A*s = t, we get:

a00*x + a01*y + a02*z = x'
a10*x + a11*y + a12*z = y'
a20*x + a21*y + a22*z = z'

The coefficients in A are what we need to solve for. Re-writing this in matrix form, we get:

[x y z 0 0 0 0 0 0]   [a00]   [x']
[0 0 0 x y z 0 0 0] * [a01] = [y']
[0 0 0 0 0 0 x y z]   [a02]   [z']
                      [a10] 
                      [a11]
                      [a12]
                      [a20]
                      [a21]
                      [a22]

Given that you have four points, you would simply concatenate rows of the matrix on the left side and the vector on the right

[x1 y1 z1 0 0 0 0 0 0]   [a00]   [x1']
[0 0 0 x1 y1 z1 0 0 0]   [a01]   [y1']
[0 0 0 0 0 0 x1 y1 z1]   [a02]   [z1']
[x2 y2 z2 0 0 0 0 0 0]   [a10]   [x2']
[0 0 0 x2 y2 z2 0 0 0]   [a11]   [y2']
[0 0 0 0 0 0 x2 y2 z2]   [a12]   [z2']
[x3 y3 z3 0 0 0 0 0 0] * [a20] = [x3']
[0 0 0 x3 y3 z3 0 0 0]   [a21]   [y3']
[0 0 0 0 0 0 x3 y3 z3]   [a22]   [z3']
[x4 y4 z4 0 0 0 0 0 0]           [x4']
[0 0 0 x4 y4 z4 0 0 0]           [y4']
[0 0 0 0 0 0 x4 y4 z4]           [z4']

          S            *   a   =   T

S would now be a matrix that contains your four source points in the format shown above, a is now a vector of the transformation coefficients in the matrix you want to solve (ordered in row-major format), and T would be a vector of target points in the format shown above.

To solve for the parameters, you simply have to use the mldivide operator or \ in MATLAB, which will compute the least squares estimate for you. Therefore:

a = S^{-1} * T

As such, simply build your matrix like above, then use the \ operator to solve for your transformation parameters in your matrix. When you're done, reshape T into a 3 x 3 matrix. Therefore:

S = ... ; %// Enter in your source points here like above
T = ... ; %// Enter in your target points in a right hand side vector like above

a = S \ T;
similarity_matrix = reshape(a, 3, 3).';

With regards to your error in small perturbations of each of the co-ordinates, the more points you have the better. Using 4 will certainly give you a solution, but it isn't enough to mitigate any errors in my opinion.

Minor Note: This (more or less) is what fitgeotrans does under the hood. It computes the best homography given a bunch of source and target points, and determines this using least squares.

Hope this answered your question!

Answer 2

The answer by @rayryeng is correct, given that you have a set of up to 3 points in a 3-dimensional space. If you need to transform m points in n-dimensional space (m>n), then you first need to add m-n coordinates to these m points such that they exist in m-dimensional space (i.e. the a matrix in @rayryeng becomes a square matrix)... Then the procedure described by @rayryeng will give you the exact transformation of points, you then just need to select only the coordinates of the transformed points in the original n-dimensional space.

As an example, say you want to transform the points:

(2 -2 2) -> (-3 5 -4)
(2 3 0) -> (3 4 4)
(-4 -2 5) -> (-4 -1 -2)
(-3 4 1) -> (4 0 5)
(5 -4 0) -> (-3 -2 -3)

Notice that you have m=5 points which are n=3-dimensional. So you need to add coordinates to these points such that they are n=m=5-dimensional, and then apply the procedure described by @rayryeng.

I have implemented a function that does that (find it below). You just need to organize the points such that each of the source-points is a column in a matrix u, and each of the target points is a column in a matrix v. The matrices u and v are going to be, thus, 3 by 5 each.

WARNING:

• the matrix A in the function may require A LOT of memory for moderately many points nP, because it has nP^4 elements.

• To overcome this, for square matrices u and v, you can simply use T=v*inv(u) or T=v/u in MATLAB notation.

• The code may run very slowly...

In MATLAB:

u = [2 2 -4 -3 5;-2 3 -2 4 -4;2 0 5 1 0]; % setting the set of source points
v = [-3 3 -4 4 -3;5 4 -1 0 -2;-4 4 -2 5 -3]; % setting the set of target points
T = findLinearTransformation(u,v); % calculating the transformation

You can verify that T is correct by:

I = eye(5);
uu = [u;I((3+1):5,1:5)]; % filling-up the matrix of source points so that you have 5-d points
w = T*uu; % calculating target points
w = w(1:3,1:5); % recovering the 3-d points
w - v % w should match v ... notice that the error between w and v is really small

The function that calculates the transformation matrix:

function [T,A] = findLinearTransformation(u,v)
% finds a matrix T (nP X nP) such that T * u(:,i) = v(:,i)
% u(:,i) and v(:,i) are n-dim col vectors; the amount of col vectors in u and v must match (and are equal to nP)
%
    if any(size(u) ~= size(v))
        error('findLinearTransform:u','u and v must be the same shape and size n-dim vectors');
    end
    [n,nP] = size(u); % n -> dimensionality; nP -> number of points to be transformed
    if nP > n % if the number of points to be transform exceeds the dimensionality of points
        I = eye(nP);
        u = [u;I((n+1):nP,1:nP)]; % then fill up the points to be transformed with the identity matrix
        v = [v;I((n+1):nP,1:nP)]; % as well as the transformed points
        [n,nP] = size(u);
    end
    A = zeros(nP*n,n*n);
    for k = 1:nP
        for i = ((k-1)*n+1):(k*n)
            A(i,mod((((i-1)*n+1):(i*n))-1,n*n) + 1) = u(:,k)';
        end
    end
    v = v(:);
    T = reshape(A\v, n, n).';
end