I am trying to write an algorithm in MatLab which takes as its input a lower triangular matrix. The output should be the inverse of this matrix (which also should be in lower triangular form). I have almost managed to solve this, but one part of my algorithm still leaves me scratching my head. So far I have:
function AI = inverse(A)
n = length(A);
I = eye(n);
AI = zeros(n);
for k = 1:n
AI(k,k) = (I(k,k) - A(k,1:(k-1))*AI(1:(k-1),k))/A(k,k);
for i = k+1:n
AI(i,k) = (I(i,k) - (??????????????))/A(i,i);
end
end
I have marked with question marks the part I am unsure of. I have tried to find a pattern for this part of the code by writing out the procedure on paper, but I just can't seem to find a proper way to solve this part.
If anyone can help me out, I would be very grateful!
Here is my code to get the inverse of a lower triangular matrix by using row transformation:
function AI = inverse(A)
len = length(A);
I = eye(len);
M = [A I];
for row = 1:len
M(row,:) = M(row,:)/M(row,row);
for idx = 1:row-1
M(row,:) = M(row,:) - M(idx,:)*M(row,idx);
end
end
AI = M(:,len+1:end);
end
You can see how it's done on Octave's source. This seems to be implemented in different places depending on the class of the matrix. For Float type Diagonal Matrix it's on liboctave/array/fDiagMatrix.cc, for Complex Diagonal matrix it's on liboctave/array/CDiagMatrix.cc, etc...
One of the advantages of free (as in freedom) software is that you are free to study how things are implemented ;)
Thanks for all the input! I was actually able to find a very nice and easy way to solve this problem today, given that the input is a lower triangular matrix:
function AI = inverse(A)
n = length(A);
I = eye(n);
AI = zeros(n);
for k = 1:n
for i = 1:n
AI(k,i) = (I(k,i) - A(k,1:(k-1))*AI(1:(k-1),i))/A(k,k);
end
end
Related
In model predictive control, an optimization problem is solved at every time instant and it is very common to write down the matrices in a compact form. Without going into the details of the optimization problem, suppose I have matrices and . I need to compute the matrices and defined as
Note that N_p is called "prediction horizon" and it is not the order of matrix A. How can I compute these matrices in a fast and efficient way? In Matlab, I have done the following, but maybe there is a more efficient way to compute these matrices:
A_cal = zeros(length(A)*Np, length(A)); %calligraphic A matrix
B_cal = zeros(size(B,1)*Np, size(Bd,2)*Np); %calligraphic B matrix
temp = eye(size(A));
for j = 1:Np
A_cal(1+(j-1)*length(A):j*length(A),:)= temp;
if j > 1
%The current row is obtained as shift of the previous row, and only the block in the first column is computed
B_cal(1+(j-1)*size(B,1):j*size(B,1),:) = circshift(B_cal(1+(j-2)*size(B,1):(j-1)*size(B,1),:),size(B,2),2);
B_cal(1+(j-1)*size(B,1):j*size(B,1),1:size(B,2)) = temp_prev*B_cent;
end
temp_prev = temp; %this variable contains A^(j-1)
temp = temp * A_cent; %use temp variable to speed up the matrix power computation
end
I assume you already solved your problem, but here is the code from my exercises sheet, creating your matrices.
S_x being the first matrix.
function S_x = compute_Sx(A,N)
S_x = eye(size(A));
for i=1:N
S_x = [S_x;A^i];
end
end
function S_u = compute_Su(A,B,N)
S_u = zeros(size(A,1)*N,size(B,2)*N);
for i=1:N
S_u = S_u + kron(diag(ones(N-i+1,1),-i+1),A^(i-1)*B);
end
S_u = [zeros(size(A,1),size(B,2)*N);S_u];
end
Best Regards
I am trying to solve two coupled reaction diffusion equations in 1d, using pdpe, namely
$\partial_t u_1 = \nabla^2 u_1 + 2k(-u_1^2+u_2)$
$\partial_t u_2 = \nabla^2 u_1 + k(u_1^2-u_2)$
The solution is in the domain $x\in[0,1]$, with initial conditions being two identical Gaussian profiles centered at $x=1/2$. The boundary conditions are absorbing for both components, i.e. $u_1(0)=u_2(0)=u_1(1)=u_2(1)=0$.
Pdepe gives me a solution without prompting any errors. However, I think the solutions must be wrong, because when I set the coupling to zero, i.e. $k=0$ (and also if I set it to be very small, say $k=0.001$), the solutions do not coincide with the solution of the simple diffusion equation
$\partial_t u = \nabla^2 u$
as obtained from pdepe itself.
Strangely enough, the solutions $u_1(t)=u_2(t)$ from the "coupled" case with coupling set to zero, and the solution for the case uncoupled by construction $u(t')$ coincide if we set $t'=2t$, that is, the solution of the "coupled" case evolves twice as fast as the solution of the uncoupled case.
Here's a minimal working example:
Coupled case
function [xmesh,tspan,sol] = coupled(k) %argument is the coupling k
std=0.001; %width of initial gaussian
center=1/2; %center of gaussian
xmesh=linspace(0,1,10000);
tspan=linspace(0,1,1000);
sol = pdepe(0,#pdefun,#icfun,#bcfun,xmesh,tspan);
function [c,f,s] = pdefun(x,t,u,dudx)
c=ones(2,1);
f=zeros(2,1);
f(1) = dudx(1);
f(2) = dudx(2);
s=zeros(2,1);
s(1) = 2*k*(u(2)-u(1)^2);
s(2) = k*(u(1)^2-u(2));
end
function u0 = icfun(x)
u0=ones(2,1);
u0(1) = exp(-(x-center)^2/(2*std^2))/(sqrt(2*pi)*std);
u0(2) = exp(-(x-center)^2/(2*std^2))/(sqrt(2*pi)*std);
end
function [pL,qL,pR,qR] = bcfun(xL,uL,xR,uR,t)
pL=zeros(2,1);
pL(1) = uL(1);
pL(2) = uL(2);
pR=zeros(2,1);
pR(1) = uR(1);
pR(2) = uR(2);
qL = [0 0;0 0];
qR = [0 0;0 0];
end
end
Uncoupled case
function [xmesh,tspan,sol] = uncoupled()
std=0.001; %width of initial gaussian
center=1/2; %center of gaussian
xmesh=linspace(0,1,10000);
tspan=linspace(0,1,1000);
sol = pdepe(0,#pdefun,#icfun,#bcfun,xmesh,tspan);
function [c,f,s] = pdefun(x,t,u,dudx)
c=1;
f = dudx;
s=0;
end
function u0 = icfun(x)
u0=exp(-(x-center)^2/(2*std^2))/(sqrt(2*pi)*std);
end
function [pL,qL,pR,qR] = bcfun(xL,uL,xR,uR,t)
pL=uL;
pR=uR;
qL = 0;
qR = 0;
end
end
Now, suppose we run
[xmesh,tspan,soluncoupled] = uncoupled();
[xmesh,tspan,solcoupled] = coupled(0); %coupling k=0, i.e. uncoupled solutions
One can directly check by plotting the solutions for any time index $it$ that, even if they should be identical, the solutions given by each function are not identical, e.g.
hold all
plot(xmesh,soluncoupled(it+1,:),'b')
plot(xmesh,solcoupled(it+1,:,1),'r')
plot(xmesh,solcoupled(it+1,:,2),'g')
On the other hand, if we double the time of the uncoupled solution, the solutions are identical
hold all
plot(xmesh,soluncoupled(2*it+1,:),'b')
plot(xmesh,solcoupled(it+1,:,1),'r')
plot(xmesh,solcoupled(it+1,:,2),'g')
The case $k=0$ is not singular, one can set $k$ to be small but finite, and the deviations from the case $k=0$ are minimal, i.e. the solution still goes twice as fast as the uncoupled solution.
I really don't understand what is going on. I need to work on the coupled case, but obviously I don't trust the results if it does not give the right limit when $k\to 0$. I don't see where I could be making a mistake. Could it be a bug?
I found the source of the error. The problem lies in the qL and qR variables of bcfun for the coupled() function. The MATLAB documentation, see here and here, is slightly ambiguous on whether the q's should be matrices or column vectors. I had used matrices
qL = [0 0;0 0];
qR = [0 0;0 0];
but in reality I should have used column vectors
qL = [0;0];
qR = [0;0];
Amazingly, pdpe didn't throw an error, and simply gave wrong results. This should perhaps be fixed by the developers.
I am trying to estimate the pose of a camera by scanning two images taken from it, detecting features in the images, matching them, creating the fundamental matrix, using the camera intrinsics to calculate the essential matrix and then decompose it to find the Rotation and Translation.
Here is the matlab code:
I1 = rgb2gray(imread('1.png'));
I2 = rgb2gray(imread('2.png'));
points1 = detectSURFFeatures(I1);
points2 = detectSURFFeatures(I2);
points1 = points1.selectStrongest(40);
points2 = points2.selectStrongest(40);
[features1, valid_points1] = extractFeatures(I1, points1);
[features2, valid_points2] = extractFeatures(I2, points2);
indexPairs = matchFeatures(features1, features2);
matchedPoints1 = valid_points1(indexPairs(:, 1), :);
matchedPoints2 = valid_points2(indexPairs(:, 2), :);
F = estimateFundamentalMatrix(matchedPoints1,matchedPoints2);
K = [2755.30930612600,0,0;0,2757.82356074384,0;1652.43432833339,1234.09417974414,1];
%figure; showMatchedFeatures(I1, I2, matchedPoints1, matchedPoints2);
E = transpose(K)*F*K;
W = [0,-1,0;1,0,0;0,0,1];
Z = [0,1,0;-1,0,0;0,0,0];
[U,S,V] = svd(E);
R = U*inv(W)*transpose(V);
T = U(:,3);
thetaX = radtodeg(atan2(R(3,2),R(3,3)));
thetaY = radtodeg(atan2(-R(3,1),sqrt(R(3,2)^2 +R(3,3)^2)));
thetaZ = radtodeg(atan2(R(2,1),R(1,1)));
The problem I am facing is that R and T are always incorrect.
ThetaZ is most of the times equal to ~90, If I repeat the calculation a lot of times I sometimes get the expected angles. (Only in some cases though)
I dont seem to understand why. It might be because the Fundamental Matrix I calculated is wrong. Or is there a different spot where I am going wrong?
Also what scale/units is T in? (Translation Vector) Or is it inferred differently.
P.S. New to computer vision...
Please note that from decomposing E, 4 solutions are possible (2 possible rotations X 2 possible translations).
Specifically regarding R, it can also be:
R = UWtranspose(V);
Similarly, T can also be:
T = -U(:,3);
To check if this is your bug, please post here all the 4 possible solutions for a given case where you get ThetaZ~90.
Another thing I would check (since you have K), is estimating the essential matrix directly (without going through fundamental matrix): http://www.mathworks.com/matlabcentral/fileexchange/47032-camera-geometry-algorithms/content//CV/CameraGeometry/EssentialMatrixFrom2DPoints.m
Try transposing K. The K that you get from estimateCameraParameters assumes row-vectors post-multiplied by a matrix, while the K in most textbooks assumes column-vectors pre-multipied by a matrix.
Edit: In the R2015b release of the Computer Vision System Toolbox there is a cameraPose function, which computes relative orientation and location from the fundamental matrix.
U and V need to be enforcedto be SO(3). http://mathworld.wolfram.com/SpecialOrthogonalMatrix.html
In other words, if U and/or V has a negative determinat, the last column in U and/or V need to be negated. (det(U) < 0) => U(:,3) = -U(:,3)
Best Regards.
I have a question if that's ok. I was recently looking for algorithm to calculate MFCCs. I found a good tutorial rather than code so I tried to code it by myself. I still feel like I am missing one thing. In the code below I took FFT of a signal, calculated normalized power, filter a signal using triangular shapes and eventually sum energies corresponding to each bank to obtain MFCCs.
function output = mfcc(x,M,fbegin,fs)
MF = #(f) 2595.*log10(1 + f./700);
invMF = #(m) 700.*(10.^(m/2595)-1);
M = M+2; % number of triangular filers
mm = linspace(MF(fbegin),MF(fs/2),M); % equal space in mel-frequency
ff = invMF(mm); % convert mel-frequencies into frequency
X = fft(x);
N = length(X); % length of a short time window
N2 = max([floor(N+1)/2 floor(N/2)+1]); %
P = abs(X(1:N2,:)).^2./N; % NoFr no. of periodograms
mfccShapes = triangularFilterShape(ff,N,fs); %
output = log(mfccShapes'*P);
end
function [out,k] = triangularFilterShape(f,N,fs)
N2 = max([floor(N+1)/2 floor(N/2)+1]);
M = length(f);
k = linspace(0,fs/2,N2);
out = zeros(N2,M-2);
for m=2:M-1
I = k >= f(m-1) & k <= f(m);
J = k >= f(m) & k <= f(m+1);
out(I,m-1) = (k(I) - f(m-1))./(f(m) - f(m-1));
out(J,m-1) = (f(m+1) - k(J))./(f(m+1) - f(m));
end
end
Could someone please confirm that this is all right or direct me if I made mistake> I tested it on a simple pure tone and it gives me, in my opinion, reasonable answers.
Any help greatly appreciated :)
PS. I am working on how to apply vectorized Cosinus Transform. It looks like I would need a matrix of MxM of transform coefficients but I did not find any source that would explain how to do it.
You can test it yourself by comparing your results against other implementations like this one here
you will find a fully configurable matlab toolbox incl. MFCCs and even a function to reverse MFCC back to a time signal, which is quite handy for testing purposes:
melfcc.m - main function for calculating PLP and MFCCs from sound waveforms, supports many options.
invmelfcc.m - main function for inverting back from cepstral coefficients to spectrograms and (noise-excited) waveforms, options exactly match melfcc (to invert that processing).
the page itself has a lot of information on the usage of the package.
I am trying to optimize my code and am not sure how and if I would be able to vectorize this particular section??
for base_num = 1:base_length
for sub_num = 1:base_length
dist{base_num}(sub_num) = sqrt((x(base_num) - x(sub_num))^2 + (y(base_num) - y(sub_num))^2);
end
end
The following example provides one method of vectorization:
%# Set example parameters
N = 10;
X = randn(N, 1);
Y = randn(N, 1);
%# Your loop based solution
Dist1 = cell(N, 1);
for n = 1:N
for m = 1:N
Dist1{n}(m) = sqrt((X(n) - X(m))^2 + (Y(n) - Y(m))^2);
end
end
%# My vectorized solution
Dist2 = sqrt(bsxfun(#minus, X, X').^2 + bsxfun(#minus, Y, Y').^2);
Dist2Cell = num2cell(Dist2, 2);
A quick speed test at N = 1000 has the vectorized solution running two orders of magnitude faster than the loop solution.
Note: I've used a second line in my vectorized solution to mimic your cell array output structure. Up to you whether you want to include it or two combine it into one line etc.
By the way, +1 for posting code in the question. However, two small suggestions for the future: 1) When posting to SO, use simple variable names - especially for loop subscripts - such as I have in my answer. 2) It is nice when we can copy and paste example code straight into a script and run it without having to do any changes or additions (again such as in my answer). This allows us to converge on a solution more rapidly.