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.
Related
So after beating my head against the wall for a few hours, I looked online for a solution to my problem, and it worked great. I just want to know what caused the issue with the way I was originally going about it.
here are some more details. The input is a 20x20px image from the MNIST datset, and there are 5000 samples, so X, or A1 is 5000x400. There are 25 nodes in the single hidden layer. The output is a one hot vector of 0-9 digits. y (not Y, which is the one hot encoding of y) is a 5000x1 vector with the value of 1-10.
Here was my original code for the cost function:
Y = zeros(m, num_labels);
for i = 1:m
Y(i, y(i)) = 1;
endfor
H = sigmoid(Theta2*[ones(1,m);sigmoid(Theta1*[ones(m, 1) X]'))
J = (1/m) * sum(sum((-Y*log(H]))' - (1-Y)*log(1-H]))')))
But then I found this:
A1 = [ones(m, 1) X];
Z2 = A1 * Theta1';
A2 = [ones(size(Z2, 1), 1) sigmoid(Z2)];
Z3 = A2*Theta2';
H = A3 = sigmoid(Z3);
J = (1/m)*sum(sum((-Y).*log(H) - (1-Y).*log(1-H), 2));
I see that this may be slightly cleaner, but what functionally causes my original code to get 304.88 and the other to get ~ 0.25? Is it the element wise multiplication?
FYI, this is the same problem as this question if you need the formal equation written out.
Thanks for any help I can get! I really want to understand where I'm going wrong
Transfer from the comments:
With a quick look, in J = (1/m) * sum(sum((-Y*log(H]))' - (1-Y)*log(1-H]))'))) there is definetely something going on with the parenthesis, but probably on how you pasted it here, not with the original code as this would throw an error when you run it. If I understand correctly and Y, H are matrices, then in your 1st version Y*log(H) is matrix multiplication while in the 2nd version Y.*log(H) is an entrywise multiplication (not matrix-multiplication, just c(i,j)=a(i,j)*b(i,j) ).
Update 1:
In regards to your question in the comment.
From the first screenshot, you represent each value yk(i) in the entry Y(i,k) of the Y matrix and each value h(x^(i))k as H(i,k). So basically, for each i,k you want to compute Y(i,k) log(H(i,k)) + (1-Y(i,k)) log(1-H(i,k)). You can do it for all the values together and store the result in matrix C. Then C = Y.*log(H) + (1-Y).*log(1-H) and each C(i,k) has the above mentioned value. This is an operation .* because you want to do the operation for each element (i,k) of each matrix (in contrast to multiplying the matrices which is totally different). Afterwards, to get the sum of all the values inside the 2D dimensional matrix C, you use the octave function sum twice: sum(sum(C)) to sum both columnwise and row-wise (or as # Irreducible suggested, just sum(C(:))).
Note there may be other errors as well.
I have a cubic expression here
I am trying to determine and plot δ𝛿 in the expression for P values of 0.0 to 5000. I'm really struggling to get the expression for δ in terms of the pressure P.
clear all;
close all;
t = 0.335*1e-9;
r = 62*1e-6;
delta = 1.2*1e+9;
E = 1e+12;
v = 0.17;
P = 0:100:5000
P = (4*delta*t)*w/r^2 + (2*E*t)*w^3/((1-v)*r^4);
I would appreciate if anyone could provide pointers.
I suggest two simple methods.
You evaluate P as a function of delta then you plot(P,delta). This is quick and dirty but if all you need is a plot it will do. The inconvenience is that you may to do some guess-and-trial to find the correct interval of P values, but you can also take a large enough value of delta_max and then restrict the x-axis limit of the plot.
Your function is a simple cubic, which you can solve analytically (see here if you are lost) to invert P(delta) into delta(P).
What you want is the functional inverse of your expression, i.e., δ𝛿 as a function of P. Since it's a cubic polynomial, you can expect up to three solutions (roots) for a give value of P. However, I'm guessing that you're only interested in real-valued solutions and nonnegative values of P. In that case there's just one real root for each value of P.
Given the values of your parameters, it makes most sense to solve this numerically using fzero. Using the parameter names in your code (different from equations):
t = 0.335*1e-9;
r = 62*1e-6;
delta = 1.2*1e9;
E = 1e12;
v = 0.17;
f = #(w,p)2*E*t*w.^3/((1-v)*r^4)+4*delta*t*w/r^2-p;
P = 0:100:5000;
w0 = [0 1]; % Bounded initial guess, valid up to very large values of P
w_sol = zeros(length(P),1);
for i = 1:length(P)
w_sol(i) = fzero(#(w)f(w,P(i)),w0); % Find solution for each P
end
figure;
plot(P,w_sol);
You could also solve this using symbolic math:
syms w p
t = 0.335*sym(1e-9);
r = 62*sym(1e-6);
delta = 1.2*sym(1e9);
E = sym(1e12);
v = sym(0.17);
w_sol = solve(p==2*E*t*w^3/((1-v)*r^4)+4*delta*t*w/r^2,w);
P = 0:100:5000;
w_sol = double(subs(w_sol(1),p,P)); % Plug in P values and convert to floating point
figure;
plot(P,w_sol);
Because of your numeric parameter values, solve returns an answer in terms of three RootOf objects, the first of which is the real one you want.
I am trying to find the first derivative of a Gaussian for an image (using Matlab) and I tried two ways.One using the gradient and one calculating the derivative but the results look different from each other.
Method 1
k=7,s=3% kernel,st.dev
f = fspecial('gaussian', [k k], s)
[Gx,Gy] = gradient(f)
Method 2
k=7,s=3% kernel,st.dev
[x,y] = meshgrid(-floor(k/2):floor(k/2), -floor(k/2):floor(k/2))
G = exp(-(x.^2+y.^2)/(2*s^2))/(2*pi*(s^2))
Gn=G/sum(G(:))
Gx = -x.*Gn/(s^2)
Gy = -y.*Gn/(s^2)
Gx and Gy should be the same from the two methods but there is a difference in the values. Does anyone know why that is? I was expecting that they will be the same. Is there a preferred way to calculate the derivative?
Thank you.
Edit: changed the G definition per Conrad's suggestion but problem still persists.
This looks wrong:
G = exp(-(x.^2+y.^2)/(2*s^2))/(2*pi*s);
Assuming this is supposed to the Normal density for (X,Y) where X and Y are independent zero mean RV with equal SDs = s, this should be:
G = exp(-(x.^2+y.^2)/(2*s^2))/(2*pi*(s^2));
(The term in front of the exponential in each component is 1/(sqrt(2*pi)*s) and you have this twice giving 1/(2*pi*s^2) )
According to the docs of fspecial: http://www.mathworks.com/help/images/ref/fspecial.html
so it seems that you should alter G to be:
G = exp(-(x.^2+y.^2)/(2*s^2));
a = 10:100
b = 10:100
c = power(a,b)
surf(a,b,c)
=> Error using surf (line 78)
Z must be a matrix, not a scalar or vector
any clue ?
Here's a vectorized way using bsxfun:
a = 10:100;
b = 1:.1:10; %// changed b to avoid very large numbers
c = bsxfun(#power, a, b.');
surf(a,b,c)
c=power(a,b) does not give you the combinations of all a power b, unfortunately.
Here there is a way of doing it (though most likely there is a vectorizedd way of doing it)
a = 10:100;
b = linspace(1,10,length(a));
% I changed the values of b, because 100^100 is that a big number that Matlab will not plot it, it is too big for storing in a double
%loop and save
for ii=1:length(b)
c(ii,:)=a.^(b(ii));
end
surf(a,b,c)
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