Leveberg-Marquard Optimization for Intensity-based Image Alignment Matlab Failed - image

I am trying to implement the intensity-based image alignment algorithm in the paper Efficient, Robust, and Fast Global Motion Estimation for Video Coding on MATLAB. The problem is that the Levenberg-Marquardt (LM) optimization doesn't work properly, so the energy function cannot approach even a local minima due to very tiny update term computed from the LM approximation equation. I've searching for a week but still could not figure out the problem. Here is my implementation.
I have two gray images I and T equal in size (W x H) and would like to estimate a rigid transformation F (scale + rotation + translation) that aligns I onto T. This could be obtained by minimizing the following energy function
Where
According to LM, I can derive the gradient term of E with respect to each parameter a_i (i=1,4) as follow (from equation (5) in the paper).
And the terms of the Hessian matrix of E are (from equation (4) in the paper)
From the above equations, I derive the bellow MATLAB procedure to compute beta and alpha terms.
%========== 1. Compute gradient of T using Sobel filter
gx = [-1 -2 -1; 0 0 0; 1 2 1] / 4;
gy = [-1 0 1; -2 0 2; -1 0 1] / 4;
Tx = conv2( T, gx, 'same' );
Ty = conv2( T, gy, 'same' );
%========== 2. Warp I using F to compute diff_image = I(x, y) - T(F(x,y,a)). F was previously initialized by an identity matrix of size 3x3
tform = affine2d( F );
I_warp = imwarp( I, tform, 'linear', 'OutputView', imref2d( size( T ) ) );
diff_img = I_wapr - T;
% create a mask for overlaping region between two aligned images
mask = ones( size( T ) );
mask_warp = imwarp( mask, tform, 'nearest', 'OutputView', imref2d( size( T ) ) );
overlap_area = numel( mask_warp );
diff_img = diff_img .* mask;
error = sum( sum( diff_img ) ) / 2 / overlap_area;
% create x, y grids
[y, x] = ndgrid( 0 : h-1, 0 : w-1 );
x_warp = imwarp( x, t_form, 'nearest', 'OutputView', imref2d( size(T) ) );
y_warp = imwarp( y, t_form, 'nearest', 'OutputView', imref2d( size(T) ) );
%======== compute beta_i = - dE/da_i (i=1,4)
sx = Tx .* diff_img;
sy = Ty .* diff_img;
beta_1 = sum( sum( x_warp .* sx + y_warp .* sy ) );
beta_2 = sum( sum( y_warp .* sx - x_warp .* sy ) );
beta_3 = sum( sum( sx ) );
beta_4 = sum( sum( sy ) );
beta = -[beta_1; beta_2; beta_3; beta_4] / overlap_area;
%======= compute alpha_ij = (dE/da_i) * (dE/da_j) i,j = 1,4
Sxx = (Tx .^ 2) .* mask_warp;
Syy = (Ty .^ 2) .* mask_warp;
Sxy = (Tx.* Ty) .* mask_warp;
xx = x_warp .^2;
yy = y_warp .^2;
xy = x_warp .* y_warp;
alpha_11 = sum( sum( xx .* Sxx + 2 * xy .* Sxy + yy .* Syy ) );
alpha_12 = sum( sum( (yy - xx) .* Sxy + xy .* (Sxx - Syy) ) );
alpha_13 = sum( sum( x .* Sxx + y .* Sxy ) );
alpha_14 = sum( sum( x .* Sxy + y .* Syy ) );
alpha_22 = sum( sum( yy .* Sxx - 2 * xy .* Sxy + xx .* Syy ) );
alpha_23 = sum( sum( y .* Sxx - x .* Sxy ) );
alpha_24 = sum( sum( y .* Sxy - x .* Syy ) );
alpha_33 = sum( sum( Sxx ) );
alpha_34 = sum( sum( Sxy ) );
alpha_44 = sum( sum( Syy ) );
alpha = [alpha_11 alpha_12 alpha_13 alpha_14;
alpha_12 alpha_22 alpha_23 alpha_24;
alpha_13 alpha_23 alpha_33 alpha_34;
alpha_14 alpha_24 alpha_34 alpha_44] / overlap_area;
% With lamda was previously initialized by 0.0001
for i = 1 : 4
alpha(i, i) = alpha(i, i) * (lamda + 1);
end
%======== Find the update term: delta_a = alpha^(-1) * beta
delta_a = pinv( alpha ) * beta
% Or we can solve for delta_a using SVD
%[U, S, V] = svd( alpha );
%inv_S = S;
%for ii = 1 : size(S, 1)
% if S(ii, ii)
% inv_S(ii, ii) = 1 / S(ii, ii);
% end
%end
%delta_a = V * inv_S * U' * beta;
%======== Update a_i and new error value
a = [ F(1, 1)-1;
F(2, 1);
F(3, 1);
F(3, 2)];
new_a = a + delta_a;
new_F = [new_a(1)+1 -new_a(2) 0;
new_a(2) new_a(1)+1 0;
new_a(3) new_a(4) 1];
tform = affine2d( new_F );
mask_warp = imwarp( mask, tform, 'nearest', 'OutputView', imref2d(size(T)));
new_I_warp = imwarp( I, tform, 'linear', 'OutputView', imref2d(size(T)));
new_diff_img = (new_I_warp - T) .* mask_warp;
new_error = sum( sum( new_diff_img .^2 ) ) / 2/ sum( sum( mask_warp ) );
if( new_error > error )
lamda = lamda * 10;
elseif new_error < error
lamda = lamda /10;
The above process is repeated until the iteration reach the limitation or the new_error value less than a predetermined threshold. However, after each iteration, the update terms are too small that parameters make trivial movement. For example,
delta a = 1.0e-04 * [0.0011 -0.0002 0.2186 0.2079]
Is there anybody know how to fixed it? Any help would be highly appreciated.

Related

How to convert rotation matrix to quaternion

Can I convert rotation matrix to quaternion?
I know how to convert quaternion to rotation matrix but I can't find way to do opposite that.
I can show you the code how to convert quaternion to rotation matrix as bellow.
Example(C++): Quaterniond quat; MatrixXd t; t = quat.matrix();
I want to know way to convert rotation matrix to quaternion like this.
A numerically stable algorithm for converting a direction cosine matrix D into a quaternion q is as follows:
T = D(1,1) + D(2,2) + D(3,3)
M = max( D(1,1), D(2,2), D(3,3), T )
qmax = (1/2) * sqrt( 1 – T + 2*M )
if( M == D(1,1) )
qx = qmax
qy = ( D(1,2) + D(2,1) ) / ( 4*qmax )
qz = ( D(1,3) + D(3,1) ) / ( 4*qmax )
qw = ±( D(3,2) - D(2,3) ) / ( 4*qmax )
elseif( M == D(2,2) )
qx = ( D(1,2) + D(2,1) ) / ( 4*qmax )
qy = qmax
qz = ( D(2,3) + D(3,2) ) / ( 4*qmax )
qw = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
elseif( M == D(3,3) )
qx = ( D(1,3) + D(3,1) ) / ( 4*qmax )
qy = ( D(2,3) + D(3,2) ) / ( 4*qmax )
qz = qmax
qw = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
else
qx = ±( D(3,2) - D(2,3) ) / ( 4*qmax )
qy = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
qz = ±( D(2,1) - D(1,2) ) / ( 4*qmax )
qw = qmax
endif
Note that there is a sign ambiguity inherent in quaternions. The algorithm above arbitrarily picks the sign of the largest element qmax to be positive, but it is equally valid to pick this sign as negative (i.e., essentially flipping all of the signs of the result). It is up to the user to determine which is the more appropriate selection based on the application.
The ± selection is made based on the quaternion convention you are using:
Choose + for Hamilton Left Chain Convention or JPL Right Chain Convention
Choose - for Hamilton Right Chain Convention or JPL Left Chain Convention
Hamilton Convention means the quaternion elements i, j, k behave in a right-handed manner for multiplication (like cross products):
i * j = k , j * k = i , k * i = j
JPL Convention means the quaternion elements i, j, k behave in a left-handed manner for multiplication (negative of cross products):
i * j = -k , j * k = -i , k * i = -j
Right Chain means the quaternion rotation operation on a vector has the unmodified quaternion on the right side:
D * v1 = v2 = q^-1 * v1 * q
Left Chain means the quaternion rotation operation on a vector has the unmodified quaternion on the left side:
D * v1 = v2 = q * v1 * q^-1
For completeness, here is the algorithm for the other direction, converting a quaternion to a direction cosine matrix:
D = (qw^2 - dot(qv,qv))*I3 + 2*qv*qv^T ± 2*qw*Skew(qv)
where ^T means transpose (for outer product in that term) and
qv = [qx]
[qy]
[qz]
I3 = [1 0 0]
[0 1 0]
[0 0 1]
Skew(qv) = [ 0 -qz qy]
[ qz 0 -qx]
[-qy qx 0]

How to calculate the Spatial frequency in Gabor filter?

This question describe the gabor filter family and its application pretty well. Though, there is nothing described about the wavelength (spatial frequency) of the filter. The creation of gabor wavelets are done in the following for loop:
for v = 0 : 4
for u = 1 : 8
GW = GaborWavelet ( R, C, Kmax, f, u, v, Delt2 ); % Create the Gabor wavelets
figure( 2 );
subplot( 5, 8, v * 8 + u ),imshow ( real( GW ) ,[]); % Show the real part of Gabor wavelets
GW_ALL( v*8+u, :) = GW(:);
end
figure ( 3 );
subplot( 1, 5, v + 1 ),imshow ( abs( GW ),[]); % Show the magnitude of Gabor wavelets
end
I know that the second loop variable is the orientation with pi/8 intervals. Though, I don't know how the first loop variable is linked with the spatial frequency (wavelength) in this code and its function [pixels/cycle]. Can anyone help?
I found the answer finally. The GaborWavelet function is defined as follows:
function GW = GaborWavelet (R, C, Kmax, f, u, v, Delt2)
k = ( Kmax / ( f ^ v ) ) * exp( 1i * u * pi / 8 );% Wave Vector
kn2 = ( abs( k ) ) ^ 2;
GW = zeros ( R , C );
for m = -R/2 + 1 : R/2
for n = -C/2 + 1 : C/2
GW(m+R/2,n+C/2) = ( kn2 / Delt2 ) * exp( -0.5 * kn2 * ( m ^ 2 + n ^ 2 ) / Delt2) * ( exp( 1i * ( real( k ) * m + imag ( k ) * n ) ) - exp ( -0.5 * Delt2 ) );
end
end
The Kmax is the maximum frequency, f is the spacing factor and v is the resolution. The spacing factor f is usually considered as sqrt(2).
Based on this paper, k= 2*pi*f*exp(i*ϑ) and in the code Kmax=fmax*2*pi which is not described and is the key to find the wavelength of the filter. I also read this implementation and noticed that wavelength can easily be found using f = 1/lambda where lambda is wavelength of sinusoid.
So for example, if Kmax=pi/2 and v=0, so the k=Kmax*exp(1i*u*pi/8) and considering the above mentioned formula, lambda = 2*pi/Kmax = 4 [pixel/cycle].

Which image processing technique removes swirling(typical low-light imaging artifacts) and creates a clear image?

I took this picture
Which image processing technique removes the swirling(typical low-light imaging artifacts) and creates a clear image?
There is different techniques for filtering out the artifacts from image. Some are based on machine learning and some use the pattern recognition.
I found this method> Optimization based pattern artifact removal from an image .
This is how it works in summary> An input image 'I' is decomposed into a latent natural image 'L' and a pattern artifact image 'E', where I = L + E. Pattern artifacts have unnatural spectrum peaks, and the such kind of peaks are separated into 'E'.
This code cannot deal with complex patterns.
Here is the matlab code for the function>
function [L, E] = imdecpattern( Y, lambda, tol, nite )
%IMDECPATTERN recovers a degraded image with a pattern artifact
% by decomposing the image into the latent image and the pattern image.
%
% [L, E] = IMDECPATTERN( Y )
% [L, E] = IMDECPATTERN( Y, lambda, tol, nite )
%
% arguments
% Y : input image
% L : latent image
% E : artifact image
% lambda : decomposition degree (default lambda=3)
% tol : the tolerance of a constraint Y = L + E (default tolerance=0)
% nite : the number of iterations to solve the problem.
%
%
%
% This program solves the following equation
%
% min_{l,e} |Dl|2,1 + lambda |Fe|c,1
% s.t. | y - (l+e) |2 <= tol, 1'l = 0
%
% where y is an observed image, l is the latent image, e is the artifact image,
% D is differential filtering and F is Fourier transformation.
%
%
% REFERENCE
%
% K. Shirai, S. Ono, and M. Okuda, ``Minimization of mixed norm for frequency
% spectrum of images and its application of pattern noise decomposition,''
% EUSIPCO, 5pages, 2017.
%
%
% parameters
%
if ~exist( 'lambda', 'var' ) || isempty( lambda )
lambda = 3; % mainly controls decomposition degree
end
if ~exist( 'tol', 'var' ) || isempty( tol )
tol = 0; % tolerance of the l2 ball related to noise reduction
end
% for ADMM
if ~exist( 'nite', 'var' ) || isempty( nite )
nite = 30; % number of iterations
end
rho = 1; % step size
%
% proximity operators
%
% mixed l2,1 norm
% is directly written in the ADMM algorithm
% mixed lc,1 norm
prox_lc1 = #(V,gamma) V.*(1 - gamma./max(gamma,abs(V)));
% l2 ball projection centered at V with the radius V-Y
prox_l2ball = #(V,V_Y,tol) V + V_Y * (tol/max(tol,norm(V_Y(:)))); % V_Y = V - Y
% which corresponds to ( norm(V_Y(:)) <= tol ) ? ( V ) : ( Y + V_Y*(tol/norm(V_Y(:)) );
[sy,sx,sc] = size( Y );
N = sy*sx; % number of pixels
% % unitary Fourier transform, but the following non-unitary version is faster.
% F = #(X) fft2(X)/sqrt(N); Ft = #(X) sqrt(N)*ifft2(X);
%
% non-unitary Fourier transform
lambda = sqrt(N) * lambda;
F = #(X) fft2(X); Ft = #(X) real(ifft2(X));
Zeros = zeros(sy,sx,sc);
Ones = ones(sy,sx,sc);
% initialization of variables
L = Y; E = Zeros;
% aux variables Z and Lagrangian-multiplier variables U
Zlh = Zeros; Ulh = Zeros;
Zlv = Zeros; Ulv = Zeros;
Ze = Zeros; Ue = Zeros;
Z = Zeros; U = Zeros;
% filters
% circular differential filters
Dh = [0,-1,1];
Dv = [0;-1;1];
% laplacian filter
DhDh_DvDv = [0,-1,0;-1,4,-1;0,-1,0];
% pre-computation of inverse filters
FA11 = 1 + repmat( psf2otf( DhDh_DvDv, [sy,sx] ), [1,1,sc] );
FA22 = 1 + Ones;
FA12 = Ones;
FA21 = Ones;
idetFA = 1./(FA11.*FA22 - FA12.*FA21); % determinant
iFA11 = FA22 .* idetFA; iFA12 = -FA12 .* idetFA;
iFA21 = -FA21 .* idetFA; iFA22 = FA11 .* idetFA;
% % weighted version
% sigma = 0.5;
% FW = Ones;
% FW = repmat( psf2otf( fspecial( 'gaussian', 2*ceil(3*sigma)+1, sigma ), [sy,sx] ), [1,1,sc] );
% FW = fftshift(FW);
for t = 1:nite
%
% solve l and e
%
IZU = (Z - U);
Fbl = fft2( IZU...
+ imfilter( Zlh - Ulh, Dh, 'corr','circular' )...
+ imfilter( Zlv - Ulv, Dv, 'corr','circular' ) );
Fbe = fft2( IZU + Ft( Ze - Ue ) );
L = real( ifft2( iFA11.*Fbl + iFA12.*Fbe ) );
E = real( ifft2( iFA21.*Fbl + iFA22.*Fbe ) );
%
% solve z_l and u_l
%
Vlh = imfilter( L, Dh, 'conv','circular' ) + Ulh;
Vlv = imfilter( L, Dv, 'conv','circular' ) + Ulv;
% prox of l2,1 norm
Vl_norm = sqrt(Vlh.*Vlh + Vlv.*Vlv);
Scale = 1 - (1/rho) ./ max(1/rho, Vl_norm);
Zlh = Vlh .* Scale;
Zlv = Vlv .* Scale;
Ulh = Vlh - Zlh; % update u_l
Ulv = Vlv - Zlv;
%
% solve z_e and u_e
%
Ve = F( E ) + Ue;
gamma = (lambda/rho);
% gamma = (lambda/rho) * FW; % weighted version
Ze = prox_lc1( Ve, gamma );
% the constraint coresponding to mean(e)=0
Ze(1,1,:) = 0;
Ue = Ve - Ze; % update u_e
%
% solve z and u
%
V = (L + E) + U;
dVY = V - Y;
for c = 1:sc
Z(:,:,c) = prox_l2ball( Y(:,:,c), dVY(:,:,c), tol );
% % Actually, this version is faster than the above
% dVYc = dVY(:,:,c);
% norm_dVY = norm( dVYc(:) );
% if ( norm_dVY <= tol )
% Z(:,:,c) = V(:,:,c);
% else
% Z(:,:,c) = Y(:,:,c) + dVY(:,:,c) * (tol/norm_dVY);
% end
end
U = V - Z; % update U
end
end
This is the test result>

vectorize/optimize this code in MATLAB?

I am building my first large-scale MATLAB program, and I've managed to write original vectorized code for everything so for until I came to trying to create an image representing vector density in stereographic projection. After a couple failed attempts I went to the Mathworks file exchange site and found an open source program which fits my needs courtesy of Malcolm Mclean. With a test matrix his function produces something like this:
And while this is almost exactly what I wanted, his code relies on a triply nested for-loop. On my workstation a test data matrix of size 25000x2 took 65 seconds in this section of code. This is unacceptable since I will be scaling up to a data matrices of size 500000x2 in my project.
So far I've been able to vectorize the innermost loop (which was the longest/worst loop), but I would like to continue and be rid of the loops entirely if possible. Here is Malcolm's original code that I need to vectorize:
dmap = zeros(height, width); % height, width: scalar with default value = 32
for ii = 0: height - 1 % 32 iterations of this loop
yi = limits(3) + ii * deltay + deltay/2; % limits(3) & deltay: scalars
for jj = 0 : width - 1 % 32 iterations of this loop
xi = limits(1) + jj * deltax + deltax/2; % limits(1) & deltax: scalars
dd = 0;
for kk = 1: length(x) % up to 500,000 iterations in this loop
dist2 = (x(kk) - xi)^2 + (y(kk) - yi)^2;
dd = dd + 1 / ( dist2 + fudge); % fudge is a scalar
end
dmap(ii+1,jj+1) = dd;
end
end
And here it is with the changes I've already made to the innermost loop (which was the biggest drain on efficiency). This cuts the time from 65 seconds down to 12 seconds on my machine for the same test matrix, which is better but still far slower than I would like.
dmap = zeros(height, width);
for ii = 0: height - 1
yi = limits(3) + ii * deltay + deltay/2;
for jj = 0 : width - 1
xi = limits(1) + jj * deltax + deltax/2;
dist2 = (x - xi) .^ 2 + (y - yi) .^ 2;
dmap(ii + 1, jj + 1) = sum(1 ./ (dist2 + fudge));
end
end
So my main question, are there any further changes I can make to optimize this code? Or even an alternative method to approach the problem? I've considered using C++ or F# instead of MATLAB for this section of the program, and I may do so if I cannot get to a reasonable efficiency level with the MATLAB code.
Please also note that at this point I don't have ANY additional toolboxes, if I did then I know this would be trivial (using hist3 from the statistics toolbox for example).
Mem consuming solution
yi = limits(3) + deltay * ( 1:height ) - .5 * deltay;
xi = limits(1) + deltax * ( 1:width ) - .5 * deltax;
dx = bsxfun( #minus, x(:), xi ) .^ 2;
dy = bsxfun( #minus, y(:), yi ) .^ 2;
dist2 = bsxfun( #plus, permute( dy, [2 3 1] ), permute( dx, [3 2 1] ) );
dmap = sum( 1./(dist2 + fudge ) , 3 );
EDIT
handling extremely large x and y by breaking the operation into blocks:
blockSize = 50000; % process up to XX elements at once
dmap = 0;
yi = limits(3) + deltay * ( 1:height ) - .5 * deltay;
xi = limits(1) + deltax * ( 1:width ) - .5 * deltax;
bi = 1;
while bi <= numel(x)
% take a block of x and y
bx = x( bi:min(end, bi + blockSize - 1) );
by = y( bi:min(end, bi + blockSize - 1) );
dx = bsxfun( #minus, bx(:), xi ) .^ 2;
dy = bsxfun( #minus, by(:), yi ) .^ 2;
dist2 = bsxfun( #plus, permute( dy, [2 3 1] ), permute( dx, [3 2 1] ) );
dmap = dmap + sum( 1./(dist2 + fudge ) , 3 );
bi = bi + blockSize;
end
This is a good example of why starting a loop from 1 matters. The only reason that ii and jj are initiated at 0 is to kill the ii * deltay and jj * deltax terms which however introduces sequentiality in the dmap indexing, preventing parallelization.
Now, by rewriting the loops you could use parfor() after opening a matlabpool:
dmap = zeros(height, width);
yi = limits(3) + deltay*(1:height) - .5*deltay;
matlabpool 8
parfor ii = 1: height
for jj = 1: width
xi = limits(1) + (jj-1) * deltax + deltax/2;
dist2 = (x - xi) .^ 2 + (y - yi(ii)) .^ 2;
dmap(ii, jj) = sum(1 ./ (dist2 + fudge));
end
end
matlabpool close
Keep in mind that opening and closing the pool has significant overhead (10 seconds on my Intel Core Duo T9300, vista 32 Matlab 2013a).
PS. I am not sure whether the inner loop instead of the outer one can be meaningfully parallelized. You can try to switch the parfor to the inner one and compare speeds (I would recommend going for the big matrix immediately since you are already running in 12 seconds and the overhead is almost as big).
Alternatively, this problem can be solved in using kernel density estimation techniques. This is part of the Statistics Toolbox, or there's this KDE implementation by Zdravko Botev (no toolboxes required).
For the example code below, I get 0.3 seconds for N = 500000, or 0.7 seconds for N = 1000000.
N = 500000;
data = [randn(N,2); rand(N,1)+3.5, randn(N,1);]; % 2 overlaid distrib
tic; [bandwidth,density,X,Y] = kde2d(data); toc;
imagesc(density);

Trilateration and locating the point (x,y,z)

I want to find the coordinate of an unknown node which lie somewhere in the space which has its reference distance away from 3 or more nodes which all of them have known coordinate.
This problem is exactly like Trilateration as described here Trilateration.
However, I don't understand the part about "Preliminary and final computations" (refer to the wikipedia site). I don't get where I could find P1, P2 and P3 just so I can put to those equation?
Thanks
Trilateration is the process of finding the center of the area of intersection of three spheres. The center point and radius of each of the three spheres must be known.
Let's consider your three example centerpoints P1 [-1,1], P2 [1,1], and P3 [-1,-1]. The first requirement is that P1' be at the origin, so let us adjust the points accordingly by adding an offset vector V [1,-1] to all three:
P1' = P1 + V = [0, 0]
P2' = P2 + V = [2, 0]
P3' = P3 + V = [0,-2]
Note: Adjusted points are denoted by the ' (prime) annotation.
P2' must also lie on the x-axis. In this case it already does, so no adjustment is necessary.
We will assume the radius of each sphere to be 2.
Now we have 3 equations (given) and 3 unknowns (X, Y, Z of center-of-intersection point).
Solve for P4'x:
x = (r1^2 - r2^2 + d^2) / 2d //(d,0) are coords of P2'
x = (2^2 - 2^2 + 2^2) / 2*2
x = 1
Solve for P4'y:
y = (r1^2 - r3^2 + i^2 + j^2) / 2j - (i/j)x //(i,j) are coords of P3'
y = (2^2 - 2^2 + 0 + -2^2) / 2*-2 - 0
y = -1
Ignore z for 2D problems.
P4' = [1,-1]
Now we translate back to original coordinate space by subtracting the offset vector V:
P4 = P4' - V = [0,0]
The solution point, P4, lies at the origin as expected.
The second half of the article is describing a method of representing a set of points where P1 is not at the origin or P2 is not on the x-axis such that they fit those constraints. I prefer to think of it instead as a translation, but both methods will result in the same solution.
Edit: Rotating P2' to the x-axis
If P2' does not lie on the x-axis after translating P1 to the origin, we must perform a rotation on the view.
First, let's create some new vectors to use as an example:
P1 = [2,3]
P2 = [3,4]
P3 = [5,2]
Remember, we must first translate P1 to the origin. As always, the offset vector, V, is -P1. In this case, V = [-2,-3]
P1' = P1 + V = [2,3] + [-2,-3] = [0, 0]
P2' = P2 + V = [3,4] + [-2,-3] = [1, 1]
P3' = P3 + V = [5,2] + [-2,-3] = [3,-1]
To determine the angle of rotation, we must find the angle between P2' and [1,0] (the x-axis).
We can use the dot product equality:
A dot B = ||A|| ||B|| cos(theta)
When B is [1,0], this can be simplified: A dot B is always just the X component of A, and ||B|| (the magnitude of B) is always a multiplication by 1, and can therefore be ignored.
We now have Ax = ||A|| cos(theta), which we can rearrange to our final equation:
theta = acos(Ax / ||A||)
or in our case:
theta = acos(P2'x / ||P2'||)
We calculate the magnitude of P2' using ||A|| = sqrt(Ax + Ay + Az)
||P2'|| = sqrt(1 + 1 + 0) = sqrt(2)
Plugging that in we can solve for theta
theta = acos(1 / sqrt(2)) = 45 degrees
Now let's use the rotation matrix to rotate the scene by -45 degrees.
Since P2'y is positive, and the rotation matrix rotates counter-clockwise, we'll use a negative rotation to align P2 to the x-axis (if P2'y is negative, don't negate theta).
R(theta) = [cos(theta) -sin(theta)]
[sin(theta) cos(theta)]
R(-45) = [cos(-45) -sin(-45)]
[sin(-45) cos(-45)]
We'll use double prime notation, '', to denote vectors which have been both translated and rotated.
P1'' = [0,0] (no need to calculate this one)
P2'' = [1 cos(-45) - 1 sin(-45)] = [sqrt(2)] = [1.414]
[1 sin(-45) + 1 cos(-45)] = [0] = [0]
P3'' = [3 cos(-45) - (-1) sin(-45)] = [sqrt(2)] = [ 1.414]
[3 sin(-45) + (-1) cos(-45)] = [-2*sqrt(2)] = [-2.828]
Now you can use P1'', P2'', and P3'' to solve for P4''. Apply the reverse rotation to P4'' to get P4', then the reverse translation to get P4, your center point.
To undo the rotation, multiply P4'' by R(-theta), in this case R(45). To undo the translation, subtract the offset vector V, which is the same as adding P1 (assuming you used -P1 as your V originally).
This is the algorithm I use in a 3D printer firmware. It avoids rotating the coordinate system, but it may not be the best.
There are 2 solutions to the trilateration problem. To get the second one, replace "- sqrtf" by "+ sqrtf" in the quadratic equation solution.
Obviously you can use doubles instead of floats if you have enough processor power and memory.
// Primary parameters
float anchorA[3], anchorB[3], anchorC[3]; // XYZ coordinates of the anchors
// Derived parameters
float Da2, Db2, Dc2;
float Xab, Xbc, Xca;
float Yab, Ybc, Yca;
float Zab, Zbc, Zca;
float P, Q, R, P2, U, A;
...
inline float fsquare(float f) { return f * f; }
...
// Precompute the derived parameters - they don't change unless the anchor positions change.
Da2 = fsquare(anchorA[0]) + fsquare(anchorA[1]) + fsquare(anchorA[2]);
Db2 = fsquare(anchorB[0]) + fsquare(anchorB[1]) + fsquare(anchorB[2]);
Dc2 = fsquare(anchorC[0]) + fsquare(anchorC[1]) + fsquare(anchorC[2]);
Xab = anchorA[0] - anchorB[0];
Xbc = anchorB[0] - anchorC[0];
Xca = anchorC[0] - anchorA[0];
Yab = anchorA[1] - anchorB[1];
Ybc = anchorB[1] - anchorC[1];
Yca = anchorC[1] - anchorA[1];
Zab = anchorB[2] - anchorC[2];
Zbc = anchorB[2] - anchorC[2];
Zca = anchorC[2] - anchorA[2];
P = ( anchorB[0] * Yca
- anchorA[0] * anchorC[1]
+ anchorA[1] * anchorC[0]
- anchorB[1] * Xca
) * 2;
P2 = fsquare(P);
Q = ( anchorB[1] * Zca
- anchorA[1] * anchorC[2]
+ anchorA[2] * anchorC[1]
- anchorB[2] * Yca
) * 2;
R = - ( anchorB[0] * Zca
+ anchorA[0] * anchorC[2]
+ anchorA[2] * anchorC[0]
- anchorB[2] * Xca
) * 2;
U = (anchorA[2] * P2) + (anchorA[0] * Q * P) + (anchorA[1] * R * P);
A = (P2 + fsquare(Q) + fsquare(R)) * 2;
...
// Calculate Cartesian coordinates given the distances to the anchors (La, Lb and Lc)
// First calculate PQRST such that x = (Qz + S)/P, y = (Rz + T)/P.
// P, Q and R depend only on the anchor positions, so they are pre-computed
const float S = - Yab * (fsquare(Lc) - Dc2)
- Yca * (fsquare(Lb) - Db2)
- Ybc * (fsquare(La) - Da2);
const float T = - Xab * (fsquare(Lc) - Dc2)
+ Xca * (fsquare(Lb) - Db2)
+ Xbc * (fsquare(La) - Da2);
// Calculate quadratic equation coefficients
const float halfB = (S * Q) - (R * T) - U;
const float C = fsquare(S) + fsquare(T) + (anchorA[1] * T - anchorA[0] * S) * P * 2 + (Da2 - fsquare(La)) * P2;
// Solve the quadratic equation for z
float z = (- halfB - sqrtf(fsquare(halfB) - A * C))/A;
// Substitute back for X and Y
float x = (Q * z + S)/P;
float y = (R * z + T)/P;
Here are the Wikipedia calculations, presented in an OpenSCAD script, which I think helps to understand the problem in a visual wayand provides an easy way to check that the results are correct. Example output from the script
// Trilateration example
// from Wikipedia
//
// pA, pB and pC are the centres of the spheres
// If necessary the spheres must be translated
// and rotated so that:
// -- all z values are 0
// -- pA is at the origin
pA = [0,0,0];
// -- pB is on the x axis
pB = [10,0,0];
pC = [9,7,0];
// rA , rB and rC are the radii of the spheres
rA = 9;
rB = 5;
rC = 7;
if ( pA != [0,0,0]){
echo ("ERROR: pA must be at the origin");
assert(false);
}
if ( (pB[2] !=0 ) || pC[2] !=0){
echo("ERROR: all sphere centers must be in z = 0 plane");
assert(false);
}
if (pB[1] != 0){
echo("pB centre must be on the x axis");
assert(false);
}
// show the spheres
module spheres(){
translate (pA){
sphere(r= rA, $fn = rA * 10);
}
translate(pB){
sphere(r = rB, $fn = rB * 10);
}
translate(pC){
sphere (r = rC, $fn = rC * 10);
}
}
function unit_vector( v) = v / norm(v);
ex = unit_vector(pB - pA) ;
echo(ex = ex);
i = ex * ( pC - pA);
echo (i = i);
ey = unit_vector(pC - pA - i * ex);
echo (ey = ey);
d = norm(pB - pA);
echo (d = d);
j = ey * ( pC - pA);
echo (j = j);
x = (pow(rA,2) - pow(rB,2) + pow(d,2)) / (2 * d);
echo( x = x);
// size of the cube to subtract to show
// the intersection of the spheres
cube_size = [10,10,10];
if ( ((d - rA) >= rB) || ( rB >= ( d + rA)) ){
echo ("Error Y not solvable");
}else{
y = (( pow(rA,2) - pow(rC,2) + pow(i,2) + pow(j,2)) / (2 * j))
- ( i / j) * x;
echo(y = y);
zpow2 = pow(rA,2) - pow(x,2) - pow(y,2);
if ( zpow2 < 0){
echo ("z not solvable");
}else{
z = sqrt(zpow2);
echo (z = z);
// subtract a cube with one of its corners
// at the point where the sphers intersect
difference(){
spheres();
translate ([x,y - cube_size[1],z]){
cube(cube_size);
}
}
translate ([x,y - cube_size[1],z]){
%cube(cube_size);
}
}
}

Resources