Matlab calculate 3D similarity transformation. fitgeotrans for 3D - matlab

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