In my application, I use 2 cameras for 3D object reconstruction.
To calibrate the cameras, I compute the fundamental matrix using 2 sets of images in order to find the camera pose (rotation and translation).
I use SVD to find the R and T.
But when I try to check the accuracy of my matrices, it doesn't work at all: the position of the reconstructed points doesn't feat with the real positions.
How can I check if I am in the right way?
Here is my Matlab code that i use :
D2=[-0.168164529475, 0.110811875773, -0.000204013531649, -9.05039442317e-05, 0.0737585102411];
D1=[-0.187817541965, 0.351429195367, -0.000521080240718, -0.00052088823018, -1.00569541826];
K2=[2178.5537139, 0.0, 657.445233702;0.0, 2178.40086319, 494.319735021;0.0, 0.0, 1.0];
K1=[2203.30000377, 0.0, 679.24264123;0.0, 2202.99249047, 506.265831986;0.0, 0.0, 1.0];
load pts1.dat; % load image points from CAM42
load pts2.dat; % load image points from CAM49
% calcul de la matrice fondamentale
disp('Finding stereo camera matrices ...');
disp('(By default RANSAC optimasation method is used.)');
disp('- 4 : LTS');
disp('- 3 : MSAC');
disp('- 2 : RANSAC');
disp('- 1 : Norm8Point');
disp('- 0 : LMedS');
c = input('Chose method to find F :', 's');
if nargin > 0
switch c
case 4
method = 'LTS';
case 3
method = 'MSAC';
case 2
method = 'RANSAC';
case 1
method = 'Norm8Point';
otherwise
method = 'LMedS';
end
else
method = 'RANSAC';
end
%F = estimateFundamentalMatrix(points2', points1', 'Method', method, 'NumTrials', 4000, 'DistanceThreshold', 1e-4)
% calcul de la matrice essentielle
E = K2' * F * K1;
% calcul de R et T à partir de la décomposition SVD
[U S V] = svd(E);
Z = [0 -1 0;
1 0 0;
0 0 0]; % matrice anti-symetrique
W = [0 -1 0;
1 0 0;
0 0 1]; % matrice orthogonale
fprintf(sprintf('\ndev(Vt) = %f', det(V')));
fprintf(sprintf('\ndet(U) = %f', det(U )));
Ra = U * W * V'
Rb = U * W'* V'
T = U * Z * U';
T0 = U(: , 3)
T = [T(2,1); -T(3, 1); T(3, 2)];
disp('=======================');
% R1 = [Ra T0]
% R2 = [Ra -T0]
% R3 = [Rb T0]
% R4 = [Rb -T0]
% test des matrices trouvées. ---------------------------------------------
pti = 10; % point index
x1 = points1(pti,:)';
disp('x1 (real):'); x1 = [x1;1]
x2 = points2(pti,:)';
disp('x2 (real):'); x2 = [x2;1]
disp('===========');
x2 = Ra*x1 + T0 % [Ra, T0]
x2 = Ra*x1 - T0 % [Ra, -T0]
x2 = Rb*x1 + T % [Rb, T0]
x2 = Rb*x1 - T % [Rb, -T0]
fprintf('\nx1t*F*x2 = %f\n',x2'*F*x1);
disp('Epipolar line');
l1 = F*x1
l2 = F*x2
Thank you.
your fundamental matrix has to satisfy the correspondence condition
x' * F * x = 0
for point correspondences x' and x. (see http://www.robots.ox.ac.uk/~vgg/hzbook/hzbook2/HZepipolar.pdf, pp 257-260)
you may have a look at the question camera-motion-from-corresponding-images, which probably help you to check if you are on the right way.
Related
I tried to implement the cost function of the Dual Absolute Quadric in Matlab according to the following equation mentioned in this paper, with this data.
My problem is that the results didn't converge.
The code is down.
main code
%---------------------
% clear and close all
%---------------------
clearvars
close all
clc
%---------------------
% Data type long
%---------------------
format long g
%---------------------
% Read data
%---------------------
load('data.mat')
%---------------------------
% Display The Initial Guess
%---------------------------
disp('=======================================================')
disp('Initial Intrinsic parameters: ');
disp(A);
disp('xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')
%=========================================================================
DualAbsoluteQuadric = Optimize(A,#DAQ);
%---------------------
% Display The Results
%---------------------
disp('=======================================================')
disp('Dual Absoute Quadric cost function: ');
disp(DualAbsoluteQuadric);
disp('xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')
The optimization function used is:
function output = Optimize(A,func)
%------------------------------
options = optimoptions('lsqnonlin','Algorithm','levenberg-marquardt',...
'Display','iter','FunctionTolerance',1e-16,'Tolx',1e-16,...
'MaxFunctionEvaluations', 1000, 'MaxIterations',39,...
'OptimalityTolerance',1e-16);
%------------------------------
% NonLinear Optimization
%------------------------------
output_line = lsqnonlin(func,[A(1,1), A(1,3), A(2,2), A(2,3), A(1,2)],...
[],[],options);
%------------------------------------------------------------------------
output = Reshape(output_line);
The Dual Absolute Quadric Function:
function cost = DAQ(params)
Aj = [params(1) params(5) params(2) ;
0 params(3) params(4) ;
0 0 1 ];
Ai = [params(1) params(5) params(2) ;
0 params(3) params(4) ;
0 0 1 ];
% W^-1 (IAC Image of the Absolute Conic)
W_inv = Ai * Aj';
%----------------
%Find plane at infinity from MQM' ~ w (Dual Absolute Quadric)
Plane_at_infinity = PlaneAtInfinity(W_inv);
%Find H_Infty = [e21]F+e21*n'
Homography_at_infty = H_Infty(Plane_at_infinity);
%----------------
% Initialization
%----------------
global Fs;
% Initialize the cost as a vector
% (N-1 * N-2)/2: 9*8/2 = 36
vector_size = (size(Fs,3)-1)*(size(Fs,4)-2)/2;
cost = zeros(1, vector_size);
% Cost Function
k = 0;
loop_size = 3 * vector_size;
Second_Term = W_inv / norm(W_inv,'fro');
for i=1:3:loop_size
k = k+1;
First_Term = Homography_at_infty(:,i:i+2) * W_inv * ((Homography_at_infty(:,i:i+2))');
First_Term = First_Term / norm(First_Term, 'fro');
cost(k) = norm(First_Term - Second_Term,'fro');
end
end
Plane at infinity function:
function P_infty = PlaneAtInfinity(W_inv)
global PPM;
% Symbolic variables
X = sym('X', 'real');
Y = sym('Y', 'real');
Z = sym('Z', 'real');
L2 = sym('L2','real');
n = [X; Y; Z];
% DAQ
Q = [W_inv , (W_inv * n) ;
(n' * W_inv) , (n' * W_inv * n)];
% Get one only camera matrix (any)
M = PPM(:, :, 3);
% Autocalibration equation
m = M * Q * M';
% solve linear equations
solution = solve(m(1, 1) == (L2 * W_inv(1, 1)), ...
m(2, 2) == (L2 * W_inv(2, 2)), ...
m(3, 3) == (L2 * W_inv(3, 3)), ...
m(1, 3) == (L2 * W_inv(1, 3)));
P_infty = [double(solution.X(1)) double(solution.Y(1))...
double(solution.Z(1))]';
Homography at infinity function:
function H_Inf = H_Infty(planeInf)
global Fs;
k = 1;
% (3 x 3) x ((N-1)*(N-2) /2)
H_Inf = zeros(3,3*(size(Fs,3)-1)*(size(Fs,4)-2)/2);%(3*3)*36
for i = 2:size(Fs,3)
for j = i+1:size(Fs,4)
[~, ~, V] = svd(Fs(:,:,i,j)');
epip = V(:,end);
H_Inf(:,k:k+2) = epipole(Fs(:,:,i,j)) * Fs(:,:,i,j)+ epip * planeInf';
k = k+3;
end
end
end
Reshape function:
function output = Reshape(input)
%---------------------
% Reshape Intrinsics
%---------------------
% K = [a skew u0 ;
% 0 B v0 ;
% 0 0 1 ];
output = [input(1) input(5) input(2) ;
0 input(3) input(4) ;
0 0 1 ];
end
Epipole Function:
function epip = epipole(Fs)
% SVD Decompostition of (Fs)^T
[~,~,V] = svd(Fs');
% Get the epipole from the last vector of the SVD
epi = V(:,end);
% Reshape the Vector into Matrix
epip = [ 0 -epi(3) epi(2);
epi(3) 0 -epi(1);
-epi(2) epi(1) 0 ];
end
The plane at infinity has to be calculated as following:
function plane = computePlaneAtInfinity(P, K)
%Input
% P - Projection matrices
% K - Approximate Values of Intrinsics
%
%Output
% plane - coordinate of plane at infinity
% Compute the DIAC W^-1
W_invert = K * K';
% Construct Symbolic Variables to Solve for Plane at Infinity
% X,Y,Z is the coordinate of plane at infinity
% XX is the scale
X = sym('X', 'real');
Y = sym('Y', 'real');
Z = sym('Z', 'real');
XX = sym('XX', 'real');
% Define Normal to Plane at Infinity
N = [X; Y; Z];
% Equation of Dual Absolute Quadric (DAQ)
Q = [W_invert, (W_invert * N); (N' * W_invert), (N' * W_invert * N)];
% Select Any One Projection Matrix
M = P(:, :, 2);
% Left hand side of the equation
LHS = M * Q * M';
% Solve for [X, Y, Z] considering the System of Linear Equations
% We need 4 equations for 4 variables X,Y,Z,XX
S = solve(LHS(1, 1) == (XX * W_invert(1, 1)), ...
LHS(1, 2) == (XX * W_invert(1, 2)), ...
LHS(1, 3) == (XX * W_invert(1, 3)), ...
LHS(2, 2) == (XX * W_invert(2, 2)));
plane = [double(S.X(1)); double(S.Y(1)); double(S.Z(1))];
end
I'm trying to make a prototype audio recognition system by following this link: http://www.ifp.illinois.edu/~minhdo/teaching/speaker_recognition/. It is quite straightforward so there is almost nothing to worry about. But my problem is with the mel-frequency function. Here is the code as provided on the website:
function m = melfb(p, n, fs)
% MELFB Determine matrix for a mel-spaced filterbank
%
% Inputs: p number of filters in filterbank
% n length of fft
% fs sample rate in Hz
%
% Outputs: x a (sparse) matrix containing the filterbank amplitudes
% size(x) = [p, 1+floor(n/2)]
%
% Usage: For example, to compute the mel-scale spectrum of a
% colum-vector signal s, with length n and sample rate fs:
%
% f = fft(s);
% m = melfb(p, n, fs);
% n2 = 1 + floor(n/2);
% z = m * abs(f(1:n2)).^2;
%
% z would contain p samples of the desired mel-scale spectrum
%
% To plot filterbanks e.g.:
%
% plot(linspace(0, (12500/2), 129), melfb(20, 256, 12500)'),
% title('Mel-spaced filterbank'), xlabel('Frequency (Hz)');
f0 = 700 / fs;
fn2 = floor(n/2);
lr = log(1 + 0.5/f0) / (p+1);
% convert to fft bin numbers with 0 for DC term
bl = n * (f0 * (exp([0 1 p p+1] * lr) - 1));
b1 = floor(bl(1)) + 1;
b2 = ceil(bl(2));
b3 = floor(bl(3));
b4 = min(fn2, ceil(bl(4))) - 1;
pf = log(1 + (b1:b4)/n/f0) / lr;
fp = floor(pf);
pm = pf - fp;
r = [fp(b2:b4) 1+fp(1:b3)];
c = [b2:b4 1:b3] + 1;
v = 2 * [1-pm(b2:b4) pm(1:b3)];
m = sparse(r, c, v, p, 1+fn2);
But it gave me an error:
Error using * Inner matrix dimensions must agree.
Error in MFFC (line 17) z = m * abs(f(1:n2)).^2;
When I include these 2 lines just before line 17:
size(m)
size(abs(f(1:n2)).^2)
It gave me :
ans =
20 65
ans =
1 65
So should I transpose the second matrix? Or should I interpret this as an row-wise multiplication and modify the code?
Edit: Here is the main function (I simply run MFCC()):
function result = MFFC()
[y Fs] = audioread('s1.wav');
% sound(y,Fs)
Frames = Frame_Blocking(y,128);
Windowed = Windowing(Frames);
spectrum = FFT_After_Windowing(Windowed);
%imagesc(mag2db(abs(spectrum)))
p = 20;
S = size(spectrum);
n = S(2);
f = spectrum;
m = melfb(p, n, Fs);
n2 = 1 + floor(n/2);
size(m)
size(abs(f(1:n2)).^2)
z = m * abs(f(1:n2)).^2;
result = z;
And here are the auxiliary functions:
function f = Frame_Blocking(y,N)
% Parameters: M = 100, N = 256
% Default : M = 100; N = 256;
M = fix(N/3);
Frames = [];
first = 1; last = N;
len = length(y);
while last <= len
Frames = [Frames; y(first:last)'];
first = first + M;
last = last + M;
end;
if last < len
first = first + M;
Frames = [Frames; y(first : len)];
end
f = Frames;
function f = Windowing(Frames)
S = size(Frames);
N = S(2);
M = S(1);
Windowed = zeros(M,N);
nn = 1:N;
wn = 0.54 - 0.46*cos(2*pi/(N-1)*(nn-1));
for ii = 1:M
Windowed(ii,:) = Frames(ii,:).*wn;
end;
f = Windowed;
function f = FFT_After_Windowing(Windowed)
spectrum = fft(Windowed);
f = spectrum;
Transpose s or transpose the resulting f (it's just a matter of convention).
There is nothing wrong with the melfb function you are using, merely with the dimensions of the signal in the example you are trying to run (in the commented lines 14-17).
% f = fft(s);
% m = melfb(p, n, fs);
% n2 = 1 + floor(n/2);
% z = m * abs(f(1:n2)).^2;
The example assumes that you are using a "colum-vector signal s". From the size of your Fourier transformed f (done via fft which respects the input signal dimensions) your input signal s is a row-vector signal.
The part that gives you the error is the actual filtering operation that requires multiplying a p x n2 matrix with a n2 x 1 column-vector (i.e., each filter's response is multiplied pointwise with the Fourier of the input signal). Since your input s is 1 x n, your f will be 1 x n and the final matrix to vector multiplication for z will give an error.
Thanks to gevang's anwer, I was able to find out my mistake. Here is how I modified the code:
function result = MFFC()
[y Fs] = audioread('s2.wav');
% sound(y,Fs)
Frames = Frame_Blocking(y,128);
Windowed = Windowing(Frames);
%spectrum = FFT_After_Windowing(Windowed');
%imagesc(mag2db(abs(spectrum)))
p = 20;
%S = size(spectrum);
%n = S(2);
%f = spectrum;
S1 = size(Windowed);
n = S1(2);
n2 = 1 + floor(n/2);
%z = zeros(S1(1),n2);
z = zeros(20,S1(1));
for ii=1: S1(1)
s = (FFT_After_Windowing(Windowed(ii,:)'));
f = fft(s);
m = melfb(p,n,Fs);
% n2 = 1 + floor(n/2);
z(:,ii) = m * abs(f(1:n2)).^2;
end;
%f = FFT_After_Windowing(Windowed');
%S = size(f);
%n = S(2);
%size(f)
%m = melfb(p, n, Fs);
%n2 = 1 + floor(n/2);
%size(m)
%size(abs(f(1:n2)).^2)
%z = m * abs(f(1:n2)).^2;
result = z;
As you can see, I naively assumed that the function deals with row-wise matrices, but in fact it deals with column vectors (and maybe column-wise matrices). So I iterate through each column of the input matrix and then combine the results.
But I don't think this is efficient and vectorized code. Also I still can't figure out how to do column-wise operations on the input matrix (Windowed - after the windowing step), instead of using a loop.
I want to speed up my code. I always use vectorization. But in this code I have no idea how to avoid the for-loop. I would really appreciate a hint how to proceed.
thank u so much for your time.
close all
clear
clc
% generating sample data
x = linspace(10,130,33);
y = linspace(20,100,22);
[xx, yy] = ndgrid(x,y);
k = 2*pi/50;
s = [sin(k*xx+k*yy)];
% generating query points
xi = 10:5:130;
yi = 20:5:100;
[xxi, yyi] = ndgrid(xi,yi);
P = [xxi(:), yyi(:)];
% interpolation algorithm
dx = x(2) - x(1);
dy = y(2) - y(1);
x_ = [x(1)-dx x x(end)+dx x(end)+2*dx];
y_ = [y(1)-dy y y(end)+dy y(end)+2*dy];
s_ = [s(1) s(1,:) s(1,end) s(1,end)
s(:,1) s s(:,end) s(:,end)
s(end,1) s(end,:) s(end,end) s(end,end)
s(end,1) s(end,:) s(end,end) s(end,end)];
si = P(:,1)*0;
M = 1/6*[-1 3 -3 1
3 -6 3 0
-3 0 3 0
1 4 1 0];
tic
for nn = 1:numel(P(:,1))
u = mod(P(nn,1)- x_(1), dx)/dx;
jj = floor((P(nn,1) - x_(1))/dx) + 1;
v = mod(P(nn,2)- y_(1), dy)/dy;
ii = floor((P(nn,2) - y_(1))/dy) + 1;
D = [s_(jj-1,ii-1) s_(jj-1,ii) s_(jj-1,ii+1) s_(jj-1,ii+2)
s_(jj,ii-1) s_(jj,ii) s_(jj,ii+1) s_(jj,ii+2)
s_(jj+1,ii-1) s_(jj+1,ii) s_(jj+1,ii+1) s_(jj+1,ii+2)
s_(jj+2,ii-1) s_(jj+2,ii) s_(jj+2,ii+1) s_(jj+2,ii+2)];
U = [u.^3 u.^2 u 1];
V = [v.^3 v.^2 v 1];
si(nn) = U*M*D*M'*V';
end
toc
scatter3(P(:,1), P(:,2), si)
hold on
mesh(xx,yy,s)
This is the full example and is a cubic B-spline surface interpolation algorithm in 2D space.
I have a 3D volume and a 2D image and an approximate mapping (affine transformation with no skwewing, known scaling, rotation and translation approximately known and need fitting) between the two. Because there is an error in this mapping and I would like to further register the 2D image to the 3D volume. I have not written code for registration purposes before, but because I can't find any programs or code to solve this I would like to try and do this. I believe the standard for registration is to optimize mutual information. I think this would also be suitable here, because the intensities are not equal between the two images. So I think I should make a function for the transformation, a function for the mutual information and a function for optimization.
I did find some Matlab code on a mathworks thread from two years ago, based on an article. The OP reports that she managed to get the code to work, but I'm not getting how she did that exactly. Also in the IP package for matlab there is an implementation, but I dont have that package and there does not seem to be an equivalent for octave. SPM is a program that uses matlab and has registration implemented, but does not cope with 2d to 3d registration. On the file exchange there is a brute force method that registers two 2D images using mutual information.
What she does is pass a multi planar reconstruction function and an similarity/error function into a minimization algorithm. But the details I don't quite understand. Maybe it would be better to start fresh:
load mri; volume = squeeze(D);
phi = 3; theta = 2; psi = 5; %some small angles
tx = 1; ty = 1; tz = 1; % some small translation
dx = 0.25, dy = 0.25, dz = 2; %different scales
t = [tx; ty; tz];
r = [phi, theta, psi]; r = r*(pi/180);
dims = size(volume);
p0 = [round(dims(1)/2);round(dims(2)/2);round(dims(3)/2)]; %image center
S = eye(4); S(1,1) = dx; S(2,2) = dy; S(3,3) = dz;
Rx=[1 0 0 0;
0 cos(r(1)) sin(r(1)) 0;
0 -sin(r(1)) cos(r(1)) 0;
0 0 0 1];
Ry=[cos(r(2)) 0 -sin(r(2)) 0;
0 1 0 0;
sin(r(2)) 0 cos(r(2)) 0;
0 0 0 1];
Rz=[cos(r(3)) sin(r(3)) 0 0;
-sin(r(3)) cos(r(3)) 0 0;
0 0 1 0;
0 0 0 1];
R = S*Rz*Ry*Rx;
%make affine matrix to rotate about center of image
T1 = ( eye(3)-R(1:3,1:3) ) * p0(1:3);
T = T1 + t; %add translation
A = R;
A(1:3,4) = T;
Rold2new = A;
Rnew2old = inv(Rold2new);
%the transformation
[xx yy zz] = meshgrid(1:dims(1),1:dims(2),1:1);
coordinates_axes_new = [xx(:)';yy(:)';zz(:)'; ones(size(zz(:)))'];
coordinates_axes_old = Rnew2old*coordinates_axes_new;
Xcoordinates = reshape(coordinates_axes_old(1,:), dims(1), dims(2), dims(3));
Ycoordinates = reshape(coordinates_axes_old(2,:), dims(1), dims(2), dims(3));
Zcoordinates = reshape(coordinates_axes_old(3,:), dims(1), dims(2), dims(3));
%interpolation/reslicing
method = 'cubic';
slice= interp3(volume, Xcoordinates, Ycoordinates, Zcoordinates, method);
%so now I have my slice for which I would like to find the correct position
% first guess for A
A0 = eye(4); A0(1:3,4) = T1; A0(1,1) = dx; A0(2,2) = dy; A0(3,3) = dz;
% this is pretty close to A
% now how would I fit the slice to the volume by changing A0 and examining some similarity measure?
% probably maximize mutual information?
% http://www.mathworks.com/matlabcentral/fileexchange/14888-mutual-information-computation/content//mi/mutualinfo.m
Ok I was hoping for someone else's approach, that would probably have been better than mine as I have never done any optimization or registration before. So I waited for Knedlsepps bounty to almost finish. But I do have some code thats working now. It will find a local optimum so the initial guess must be good. I wrote some functions myself, took some functions from the file exchange as is and I extensively edited some other functions from the file exchange. Now that I put all the code together to work as an example here, the rotations are off, will try and correct that. Im not sure where the difference in code is between the example and my original code, must have made a typo in replacing some variables and data loading scheme.
What I do is I take the starting affine transformation matrix, decompose it to an orthogonal matrix and an upper triangular matrix. I then assume the orthogonal matrix is my rotation matrix so I calculate the euler angles from that. I directly take the translation from the affine matrix and as stated in the problem I assume I know the scaling matrix and there is no shearing. So then I have all degrees of freedom for the affine transformation, which my optimisation function changes and constructs a new affine matrix from, applies it to the volume and calculates the mutual information. The matlab optimisation function patternsearch then minimises 1-MI/MI_max.
What I noticed when using it on my real data which are multimodal brain images is that it works much better on brain extracted images, so with the skull and tissue outside of the skull removed.
%data
load mri; volume = double(squeeze(D));
%transformation parameters
phi = 3; theta = 1; psi = 5; %some small angles
tx = 1; ty = 1; tz = 3; % some small translation
dx = 0.25; dy = 0.25; dz = 2; %different scales
t = [tx; ty; tz];
r = [phi, theta, psi]; r = r*(pi/180);
%image center and size
dims = size(volume);
p0 = [round(dims(1)/2);round(dims(2)/2);round(dims(3)/2)];
%slice coordinate ranges
range_x = 1:dims(1)/dx;
range_y = 1:dims(2)/dy;
range_z = 1;
%rotation
R = dofaffine([0;0;0], r, [1,1,1]);
T1 = ( eye(3)-R(1:3,1:3) ) * p0(1:3); %rotate about p0
%scaling
S = eye(4); S(1,1) = dx; S(2,2) = dy; S(3,3) = dz;
%translation
T = [[eye(3), T1 + t]; [0 0 0 1]];
%affine
A = T*R*S;
% first guess for A
r00 = [1,1,1]*pi/180;
R00 = dofaffine([0;0;0], r00, [1 1 1]);
t00 = T1 + t + ( eye(3) - R00(1:3,1:3) ) * p0;
A0 = dofaffine( t00, r00, [dx, dy, dz] );
[ t0, r0, s0 ] = dofaffine( A0 );
x0 = [ t0.', r0, s0 ];
%the transformation
slice = affine3d(volume, A, range_x, range_y, range_z, 'cubic');
guess = affine3d(volume, A0, range_x, range_y, range_z, 'cubic');
%initialisation
Dt = [1; 1; 1];
Dr = [2 2 2].*pi/180;
Ds = [0 0 0];
Dx = [Dt', Dr, Ds];
%limits
LB = x0-Dx;
UB = x0+Dx;
%other inputs
ref_levels = length(unique(slice));
Qref = imquantize(slice, ref_levels);
MI_max = MI_GG(Qref, Qref);
%patternsearch options
options = psoptimset('InitialMeshSize',0.03,'MaxIter',20,'TolCon',1e-5,'TolMesh',5e-5,'TolX',1e-6,'PlotFcns',{#psplotbestf,#psplotbestx});
%optimise
[x2, MI_norm_neg, exitflag_len] = patternsearch(#(x) AffRegOptFunc(x, slice, volume, MI_max, x0), x0,[],[],[],[],LB(:),UB(:),options);
%check
p0 = [round(size(volume)/2).'];
R0 = dofaffine([0;0;0], x2(4:6)-x0(4:6), [1 1 1]);
t1 = ( eye(3) - R0(1:3,1:3) ) * p0;
A2 = dofaffine( x2(1:3).'+t1, x2(4:6), x2(7:9) ) ;
fitted = affine3d(volume, A2, range_x, range_y, range_z, 'cubic');
overlay1 = imfuse(slice, guess);
overlay2 = imfuse(slice, fitted);
figure(101);
ax(1) = subplot(1,2,1); imshow(overlay1, []); title('pre-reg')
ax(2) = subplot(1,2,2); imshow(overlay2, []); title('post-reg');
linkaxes(ax);
function normed_score = AffRegOptFunc( x, ref_im, reg_im, MI_max, x0 )
t = x(1:3).';
r = x(4:6);
s = x(7:9);
rangx = 1:size(ref_im,1);
rangy = 1:size(ref_im,2);
rangz = 1:size(ref_im,3);
ref_levels = length(unique(ref_im));
reg_levels = length(unique(reg_im));
t0 = x0(1:3).';
r0 = x0(4:6);
s0 = x0(7:9);
p0 = [round(size(reg_im)/2).'];
R = dofaffine([0;0;0], r-r0, [1 1 1]);
t1 = ( eye(3) - R(1:3,1:3) ) * p0;
t = t + t1;
Ap = dofaffine( t, r, s );
reg_im_t = affine3d(reg_im, A, rangx, rangy, rangz, 'cubic');
Qref = imquantize(ref_im, ref_levels);
Qreg = imquantize(reg_im_t, reg_levels);
MI = MI_GG(Qref, Qreg);
normed_score = 1-MI/MI_max;
end
function [ varargout ] = dofaffine( varargin )
% [ t, r, s ] = dofaffine( A )
% [ A ] = dofaffine( t, r, s )
if nargin == 1
%affine to degrees of freedom (no shear)
A = varargin{1};
[T, R, S] = decompaffine(A);
r = GetEulerAngles(R(1:3,1:3));
s = [S(1,1), S(2,2), S(3,3)];
t = T(1:3,4);
varargout{1} = t;
varargout{2} = r;
varargout{3} = s;
elseif nargin == 3
%degrees of freedom to affine (no shear)
t = varargin{1};
r = varargin{2};
s = varargin{3};
R = GetEulerAngles(r); R(4,4) = 1;
S(1,1) = s(1); S(2,2) = s(2); S(3,3) = s(3); S(4,4) = 1;
T = eye(4); T(1,4) = t(1); T(2,4) = t(2); T(3,4) = t(3);
A = T*R*S;
varargout{1} = A;
else
error('incorrect number of input arguments');
end
end
function [ T, R, S ] = decompaffine( A )
%I assume A = T * R * S
T = eye(4);
R = eye(4);
S = eye(4);
%decompose in orthogonal matrix q and upper triangular matrix r
%I assume q is a rotation matrix and r is a scale and shear matrix
%matlab 2014 can force real solution
[q r] = qr(A(1:3,1:3));
R(1:3,1:3) = q;
S(1:3,1:3) = r;
% A*S^-1*R^-1 = T*R*S*S^-1*R^-1 = T*R*I*R^-1 = T*R*R^-1 = T*I = T
T = A*inv(S)*inv(R);
t = T(1:3,4);
T = [eye(4) + [[0 0 0;0 0 0;0 0 0;0 0 0],[t;0]]];
end
function [varargout]= GetEulerAngles(R)
assert(length(R)==3)
dims = size(R);
if min(dims)==1
rx = R(1); ry = R(2); rz = R(3);
R = [[ cos(ry)*cos(rz), -cos(ry)*sin(rz), sin(ry)];...
[ cos(rx)*sin(rz) + cos(rz)*sin(rx)*sin(ry), cos(rx)*cos(rz) - sin(rx)*sin(ry)*sin(rz), -cos(ry)*sin(rx)];...
[ sin(rx)*sin(rz) - cos(rx)*cos(rz)*sin(ry), cos(rz)*sin(rx) + cos(rx)*sin(ry)*sin(rz), cos(rx)*cos(ry)]];
varargout{1} = R;
else
ry=asin(R(1,3));
rz=acos(R(1,1)/cos(ry));
rx=acos(R(3,3)/cos(ry));
if nargout > 1 && nargout < 4
varargout{1} = rx;
varargout{2} = ry;
varargout{3} = rz;
elseif nargout == 1
varargout{1} = [rx ry rz];
else
error('wrong number of output arguments');
end
end
end
I am trying to implement a Kalman Filter for estimating the state 'x' (displacement and velocity) of an oscillator. The code is below and should be simple to follow.
clear; clc; close all;
% io = csvread('sim.csv');
% u = io(:, 1);
% y = io(:, 2);
% clear io;
% Estimation of state of a single degree-of-freedom oscillator using
% the Kalman filter
% x[n + 1] = A x[n] + B u[n] + w[n]
% y[n] = C x[n] + D u[n] + v[n]
% Here, x[n] is 2 x 1, u[n] is 1 x 1
% A is 2 x 2, B is 2 x 1, C is 1 x 2, D is 1 x 1
%% Code begins here
N = 1000;
u = randn(N, 1); % Synthetic input
y = randn(N, 1); % Synthetic output
%% Definitions
dt = 0.005; % Time step in seconds
T = 1.50; % Oscillator period
zeta = 0.05; % Damping ratio
sv0 = max(abs(u)) * dt;
sd0 = sv0 * dt;
Q = [sd0 ^ 2 0.0; 0.0 sv0 ^ 2]; % Prediction error covariance matrix
smeas = 0.001 * max(abs(u));
R = smeas ^ 2; % Measurement error (co)variance scalar
wn = 2. * pi / Ts;
c = 2.0 * zeta * wn;
k = wn ^ 2;
A = [0. 1.; -k -c];
Ad = expm(A * dt);
Bd = A \ (Ad - eye(2));
Bd = Bd(:, 2);
C = [-k -c];
D = -1.0;
%% State-space model and Kalman filter
sys = ss(Ad, Bd, C, D, dt, 'inputname', 'u', 'outputname', 'y');
[kest,L,P] = kalman(sys, Q, R, []);
Here is my problem. I get the error: 'In the "kalman(SYS,QN,RN,NN,...)" command, QN must be a real square matrix with at most 1 rows.'.
I thought that QN = Q = const and should be 2 x 2, but it is asking for a scalar. Perhaps I don't understand the difference between Q and QN in MATLAB's 'kalman' help description. Any insights?
Thanks.
MATLAB is assuming the process noise is only one stochastic variable and not two like Q is representing.
So you have to add the G and H matrices to your sys like so:
G = eye(2);
H = [0,0];
sys = ss(Ad, [Bd, G], C, [D, H], dt, 'inputname', 'u', 'outputname', 'y');
Just as a reminder, using MATLAB's syntax:
x*=Ax+Bu+Gw
y=Cx+Du+Hw+v