I have 6 points in space with known coordinates in mm and corresponding 2D pixel coordinates in the image (image size is 640x320 pixels and points coordinates have been measured from upper left of the image. also I have the focal length of the camera as 43.456mm. trying to find the camera position and orientation.
My matlab code here will give me the camera location as -572.8052
-676.7060 548.7718 and seems correct but I am having a hard time finding the orientation values (yaw pitch roll of the camera in degrees)
I know that the rotation values should be 60.3,5.6,-45.1
the last 4 lines in my code needs to be updated to output the orientation of the camera.
I would really really appreciate your help on this.
Thanks.
Here is my matlab code:
Points_2D= [135 183 ; 188 129 ; 298 256 ; 301 43 ; 497 245; 464 110];
Points_3D= [-22.987 417.601 -126.543 ; -132.474 37.67 140.702 ; ...
388.445 518.635 -574.784 ; 250.015 259.803 67.137 ; ...
405.915 -25.566 -311.834 ; 568.859 164.809 -162.604 ];
M = [0;0;0;0;0;0;0;0;0;0;0];
A = [];
for i = 1:size(Points_2D,1)
u_i = Points_2D(i,1);
v_i = Points_2D(i,2);
x_i = Points_3D(i,1);
y_i = Points_3D(i,2);
z_i = Points_3D(i,3);
A_vec_1 = [x_i y_i z_i 1 0 0 0 0 -u_i*x_i -u_i*y_i -u_i*z_i -u_i]; %
A_vec_2 = [ 0 0 0 0 x_i y_i z_i 1 -v_i*x_i -v_i*y_i -v_i*z_i -v_i]; %
A(end+1,:) = A_vec_1;
A(end+1,:) = A_vec_2;
end
[U,S,V] = svd(A);
M = V(:,end);
M = transpose(reshape(M,[],3));
Q = M(:,1:3);
m_4 = M(:,4);
Center = (-Q^-1)*m_4
k=[43.456/640 0 320 ;0 43.456/320 160;0 0 1 ];
Rotation= (Q^-1)*k;
CC=Rotation'
eul=rotm2eul(CC)
First thing first: 6 points are enough but it is likely that you have some error. To get a better performance, it is recommended to have more than 6 points, like 10-15 preferably.
Your code seems correct until:
Q = M(:,1:3);
m_4 = M(:,4);
So you are looking for extrinsic and intrinsic parameters of the camera, i.e. rotation, translation, alpha(skew in x direction), ro, beta(skew in x direction), u0, v0 (translation of camera center). So a total of 5 intrinsics, 6 extrinsic parameters.
Here is a link which explains the details how to calculate those parameters. I had a code which I didn't test thoroughly, may be some errors but it was working in my case.
Continuing from M which is the 3x4 matrix you found:
a1 = M(1, 1:3);
a2 = M(2, 1:3);
a3 = M(3, 1:3);
b = M(1:3,4);
% Decomposition of the parameters
eps = 1; %this can be -1 or +1, based on the value you choose, you will have two different results.
ro = eps/sqrt(sumsqr(a3));
r3 = ro*a3;
u0 = ro.^2*(dot(a1,a3))
v0 = ro.^2*(dot(a2,a3))
cos_theta = -dot(cross(a1,a3),cross(a2,a3))/ ...
dist_cross(a1,a3)/(dist_cross(a2,a3));
theta = acos(cos_theta);
alpha = ro^2*dist_cross(a1,a3)*sin(theta)
beta = ro^2*dist_cross(a2,a3)*sin(theta)
theta_deg = theta*180/pi
r1 = 1/dist_cross(a2,a3)*cross(a2,a3);
r2 = cross(r3,r1);
R = [r1;r2;r3] % Rotation matrix 3x3
% ro*A.inv(R) = K
K = [alpha -alpha*cot(theta) u0;
0 beta/sin(theta) v0;
0 0 1 ];
T = ro*(inv(K)*b) % Translation matrix, 1x3
where
function [dis] = dist_cross(mi,mj)
dis = sqrt(sumsqr(cross(mi,mj)));
end
I don't guarantee it is totally correct but it should help.
Related
My mathematics (and matlab) is just not up to this and so I humbly ask for your help:
I have a model of flow through a annulus. I want to get the radius of the peak flow point and the radius of the annulus to the centreline.
So, I have 3 points, ABC, in a 3D model whose co-ordinates we know but not aligned to xyz. I want AB to be two points on a plane and point A to be the origin (centre of the annulus). Point C is on a plane parallel to the previous plane (the peak flow point). I would like to know how to calculate the distance between C on its plane to the normal vector of the AB plane that goes through the point A.
Here are pics of the model:
Flow through an annulus
Points in the model
Now I've had a friend who's cleverer than I come up with a matlab code to translate and rotate the model and then calculate the magnitudes of AB and AnormC. However, it's not working as it puts C at a greater radius than B. I also realise there is more than one way to solve this problem.
The code is below. Any thoughts where we've gone wrong? Or maybe is there a better way to do this? I thought of vectors but my scribblings amount to nought.
Thank you.
Toby
%Origin
O = [0 0 0]'; Ox = [O' 1]';
%Vector co-ordinates of 1 (A - Origin centreline), ...
% 2 (B - Radius of artery), and 3 (C - Radius of Peak Velocity)
A = [13.3856 -60.0377 15.8443]'; Ax = [A' 1]';
B = [26.9486 -51.0653 20.9265]'; Bx = [B' 1]';
C = [16.2240 -92.5594 40.8687]'; Cx = [C' 1]';
%Find the new i unit vector in the old co-ords (the unit vector along the new x axis)
AB = B - A;
magAB = sqrt(sum(AB.^2));
new_i=AB./magAB;
%Calculate the angle to rotate through Z when transforming
thetaZ = atan(new_i(2)/new_i(1));
%Hence, define the translation matrix (to move the origin to A) and ...
% the rotation matrixes (to align the new x axis with AB and new_i)
T = [1 0 0 -A(1) ; 0 1 0 -A(2) ; 0 0 1 -A(3) ; 0 0 0 1];
Rz = [cos(thetaZ) sin(thetaZ) 0 0 ; -sin(thetaZ) cos(thetaZ) 0 0 ; 0 0 1 0 ; 0 0 0 1];
Transform = Rz * T;
%transform Cx to the new co-ordinates by translation and rotation in Z
A_dash = round(Transform * Ax,10);
B_dash = round(Transform * Bx,10);
C_dash = round(Transform * Cx,10);
new_i_t = round(Transform * [new_i' 1]',4); new_i_t = new_i_t(1:3);
new_O = round(Transform * Ox,4); new_O = new_O(1:3);
A_dash = A_dash(1:3); B_dash = B_dash(1:3); C_dash = C_dash(1:3);
%Perform a final rotation in Y
thetaY = atan(B_dash(3)/B_dash(1));
Ry = [cos(thetaY) 0 sin(thetaY) ; 0 1 0 ; -sin(thetaY) 0 cos(thetaY)];
B_dash = Ry * B_dash;
C_dash = Ry * C_dash;
%Taking point C onto the plane of AB
C_dashflat = C_dash.*[1 1 0]';
%Find Radius of Peak Flow
Radius_Peak_Flow = sqrt(sum(C_dashflat.^2));
%Find Radius of Artery
Radius_Artery = magAB;
%Ratio (between 0 -1)
Ratio = Radius_Peak_Flow/Radius_Artery
I am trying to rotate a 2D image in Matlab xyz 3-D space. I want to rotate the image I by an angle θ=i about the x-axis. I am doing this by means of multiplication with the rotation matrix (as defined here):
This is my code:
x = linspace(-1,1,size(I,1));
[x0,y0,z0] = meshgrid(x,x,1);
R = [1 0 0; 0 cosd(-i) -sind(-i); 0 sind(-i) cosd(-i)];
xy = [x0(:),y0(:), z0(:)]*R';
X = reshape(xy(:,1),size(x0));
Y = reshape(xy(:,2),size(y0));
Z = reshape(xy(:,3),size(z0));
rotatedI = interp3(x0,y0,z0,I,X,Y,Z, 'nearest');
rotatedI(isnan(rotatedI)) = 0;
The error that I am getting is in the interpolation line:
Error using griddedInterpolant
Interpolation requires at least two sample points in each
dimension.
So what exactly is the problem here and how can I fix it?
I am confused because this code works fine when specialized to two dimensions. Any explanation is much appreciated.
the problem is that your input z grid z0 contains a single value (1), while your desired z grid Z contains multiple values. therefore there is no way to interpolate this single value into multiple ones.
you can assign the input z grid with multiple values so that it will be possible to interpolate.
I = im2double(imread('cameraman.tif'));
I = cat(3,I,I,I);
i = 10;
x = linspace(-1,1,size(I,1));
[x0,y0,z0] = meshgrid(x,x,-1:1); % 3 z values
R = [1 0 0; 0 cosd(-i) -sind(-i); 0 sind(-i) cosd(-i)];
xy = [x0(:),y0(:), z0(:)]*R;
X = reshape(xy(:,1),size(x0));
Y = reshape(xy(:,2),size(y0));
Z = reshape(xy(:,3),size(z0));
rotatedI = interp3(x0,y0,z0,I,X,Y,Z, 'nearest');
rotatedI(isnan(rotatedI)) = 0;
imshow(rotatedI);
in addition, you can use rotx, roty, and rotz to generate rotation matrices easily.
UPDATE
if you want to rotate the image 2D plane in the 3D space you don't need to use interpolation and can do something like:
I = im2double(imread('cameraman.tif'));
i = 10;
x = linspace(-1,1,size(I,1));
[x0,y0,z0] = meshgrid(x,x,-1:1); % 3 z values
[x0,y0,z0] = meshgrid(x,x,1); % 3 z values
R = [1 0 0; 0 cosd(-i) -sind(-i); 0 sind(-i) cosd(-i)];
xy = [x0(:),y0(:), z0(:)]*R;
X = reshape(xy(:,1),size(x0));
Y = reshape(xy(:,2),size(y0));
Z = reshape(xy(:,3),size(z0));
warp(X,Y,Z,I);
grid on
I have a video and I have made a Sobel mask for it on MATLAB. Now I have to apply that Sobel mask on each frame of the video by reading each frame through for loop. The process is something like:
Step 1: Reading frame.
step 2: Converting it to grayscale using rgb2gray.
Step 3: Converting it to double.
Here, after applying the mask when I try to write the frame on the resultant video.avi file, I get the following error:
"Frames of type double must be in the range of 0 to 1"
What is wrong with my code? The code I wrote is shown below:
vid = VideoReader('me.mp4');
frames = read(vid);
total = get(vid, 'NumberOfFrames');
write = VideoWriter('me.avi');
open(write);
mask1 = [-1 -2 -1; 0 0 0; 1 2 1]; % Horizontal mask
mask2 = [-1 0 1; -2 0 2; -1 0 1]; %Vertical Mask
for k = 1 : 125
image = frames(:,:,:,k);
obj = image;
obj1 = rgb2gray(obj);
obj2=double(obj1);
for row = 2 : size(obj2, 1) - 1
for col = 2 : size(obj2, 2) - 1
c1 = obj2(row - 1, col - 1) * mask1(1 ,1);
c2 = obj2(row - 1, col) * mask1(1 ,2);
c3 = obj2(row - 1, col + 1) * mask1(1 ,3);
c4 = obj2(row, col - 1)*mask1(2, 1);
c5 = obj2(row, col)*mask1(2, 2);
c6 = obj2(row, col + 1)*mask1(2, 3);
c7 = obj2(row + 1, col - 1)*mask1(3,1);
c8 = obj2(row + 1, col)*mask1(3,2);
c9 = obj2(row + 1, col + 1)*mask1(3,3);
c11 = obj2(row - 1, col - 1)*mask2(1 , 1);
c22 = obj2(row, col - 1)*mask2(2, 1);
c33 = obj2(row + 1, col - 1)*mask2(3, 1);
c44 = obj2(row -1, col)*mask2(1, 2);
c55 = obj2(row, col)*mask2(2 , 2);
c66 = obj2(row +1, col)*mask2(2 , 3);
c77 = obj2(row - 1, col + 1)*mask2(1 , 3);
c88 = obj2(row, col +1)*mask2(2 , 3);
c99 = obj2(row + 1, col + 1)*mask2(3 , 3);
result = c1 + c2 + c3 +c4 +c5+ c6+ c7+ c8 +c9;
result2 = c11 + c22 + c33 + c44 + c55 + c66 + c77 + c88 + c99;
%result = double(result);
%result2 = double(result2);
rim1(row, col) = ((result^2+result2^2) *1/2);
rim2(row, col) = atan(result/result2);
end
end
writeVideo(write, rim2); %This line has the problem with rim2 as rim2 is the frame i'm trying to write on the video file.
end
close(write);
rim2 has range [-pi/2, pi/2] at the end, which is not compatible with the write function which expects [0,1] range.
Convert it to [0,1] range using the mat2gray function, i.e.
writeVideo(write, mat2gray(rim2));
Your code will then work as expected (confirmed on my machine).
By the way, this doesn't affect your code, but presumably you meant to do im2double(A) rather than double(A). The former produces a "proper" grayscale image in the range [0,1], whereas the latter simply converts your uint8 image in the range [0,255] to double format (i.e. [0.0, 255.0]).
The line of rim2 inside your double for loop is using atan, which will generate values that are both positive and negative - from -pi/2 to +pi/2 exactly. rim2 is expected to have values that are only between [0,1]. I can't figure out what exactly you're doing, but it looks like you're calculating the magnitude and gradient angle at each pixel location. If you want to calculate the magnitude, you have to take the square root of the result, not simply multiply by 1/2. The calculation of the gradient (... or even the whole Sobel filter calculation...) is very funny.
I'll just assume this is working for your purposes so I'm not sure how to change the output of rim2for suitable display but perhaps you could scale it to the range of [0,1] before you write the video so that it's within this range.
Something like this would work before you write the frame:
rim2 = (rim2 - min(rim2(:))) / (max(rim2(:)) - min(rim2(:)));
writeVideo(write, rim2);
The above is your typical min-max normalization that is seen in practice. Specifically, the above will ensure that the smallest value is 0 while the largest value is 1 per frame. If you want to be consistent over all frames, simply add pi/2 then divide by pi. This assumes that the minimum is -1 and the maximum is +1 over all frames however.
rim2 = (rim2 + pi/2) / pi;
writeVideo(write, rim2);
However, I suspect you want to write the magnitude to file, not the angle. Therefore, replace the video writing with rim1 as the frame to write instead of rim2, then normalize after. Make sure your gradient calculation is correct though:
rim1(row, col) = ((result^2+result2^2)^(1/2));
% or use sqrt:
% rim1(row, col) = sqrt(result^2 + result2^2);
Now write to file:
rim1 = (rim1 - min(rim1(:))) / (max(rim1(:)) - min(rim1(:)));
writeVideo(write, rim1);
However, if I can provide a method of efficiency, don't use for loops to compute the gradient and angle. Use conv2 and ensure you use the 'same' flag or imfilter from the image processing toolbox to perform the filtering for you, then calculate the gradient and angle vectorized. Also, convert to grayscale and cast your frame in one go in the main loop. I'll assume you have the image processing toolbox as having the computer vision toolbox (you have this as you're using a VideoWriter object) together with the image processing toolbox is what most people have:
vid = VideoReader('me.mp4');
frames = read(vid);
total = get(vid, 'NumberOfFrames');
write = VideoWriter('me.avi');
open(write);
mask1 = [-1 -2 -1; 0 0 0; 1 2 1]; % Horizontal mask
mask2 = [-1 0 1; -2 0 2; -1 0 1]; %Vertical Mask
for k = 1 : 125
obj2 = double(rgb2gray(frames(:,:,:,k))); % New
grad1 = imfilter(obj2, mask1); % New
grad2 = imfilter(obj2, mask2); % New
rim1 = sqrt(grad1.^2 + grad2.^2); % New
rim2 = atan2(grad1, grad2); % New
% Normalize
rim2 = (rim2 - min(rim2(:))) / (max(rim2(:)) - min(rim2(:)));
writeVideo(write, rim2);
end
close(write);
I am given a grayscale image and I would like to create gradient magnitude image from it.
I know there is a function called imgradient, but I have no idea of how to modify the magnitude so that it is computed the following way:
∇f ≈ abs(z7+ 2z8+ z9- z1- 2z2- z3) + abs(z3+ 2z6+ z9- z1- 2z4- z7)
I get that you want to do spatial filtering using sobel mask (as per your equation). So, I suggest you to use following command: BW = edge(I,'Sobel');
Or If you want exact output as per equation given by you then try this:
I = imread('cameraman.tif');
I = padarray(I,[1 1],'symmetric');
sobel_mask_x = [-1 -2 -1; 0 0 0; 1 2 1];
sobel_x = filter2(sobel_mask_x ,I);
sobel_mask_y = sobel_mask_x';
sobel_y = filter2(sobel_mask_y ,I);
sobel_responce = abs(sobel_x)+abs(sobel_y);
figure,imshow(sobel_responce,[]); title('Sobel gradient');
This is faster instead of using for loops.
You have to implement your own gradient function, and apply it to each pixel in the image I:
//example of a 4x5 image I
I = [1 3 2 6 5; 6 1 0 18 21; 13 12 13 14 11; 16 15 28 9 20]
G = I; //set your gradient matrix to be equal to I so that the sides have a value
//get the height and width of image I
[h, w] = size(I);
//loop through each pixel that is not on the side
for x=2:w-1
for y=2:h-1
//for each pixel:
z1 = I(y-1, x-1); //e.g z1 is the pixel on the upper left
z2 = I(y, x-1);
z3 = I(y+1, x-1);
z4 = I(y-1, x);
z6 = I(y+1, x);
z7 = I(y-1, x+1);
z8 = I(y, x+1);
z9 = I(y+1, x+1);
G(y,x) = abs(z7+ 2*z8+ z9- z1- 2*z2- z3) + abs(z3+ 2*z6+ z9- z1- 2*z4- z7);
end
end
Normally, a Gabor filter, as its name suggests, is used to filter an image and extract everything that it is oriented in the same direction of the filtering.
In this question, you can see more efficient code than written in this Link
Assume 16 scales of Filters at 4 orientations, so we get 64 gabor filters.
scales=[7:2:37], 7x7 to 37x37 in steps of two pixels, so we have 7x7, 9x9, 11x11, 13x13, 15x15, 17x17, 19x19, 21x21, 23x23, 25x25, 27x27, 29x29, 31x31, 33x33, 35x35 and 37x37.
directions=[0, pi/4, pi/2, 3pi/4].
The equation of Gabor filter in the spatial domain is:
The equation of Gabor filter in the frequency domain is:
In the spatial domain:
function [fSiz,filters,c1OL,numSimpleFilters] = init_gabor(rot, RF_siz)
image=imread('xxx.jpg');
image_gray=rgb2gray(image);
image_gray=imresize(image_gray, [100 100]);
image_double=double(image_gray);
rot = [0 45 90 135]; % we have four orientations
RF_siz = [7:2:37]; %we get 16 scales (7x7 to 37x37 in steps of two pixels)
minFS = 7; % the minimum receptive field
maxFS = 37; % the maximum receptive field
sigma = 0.0036*RF_siz.^2 + 0.35*RF_siz + 0.18; %define the equation of effective width
lambda = sigma/0.8; % it the equation of wavelength (lambda)
G = 0.3; % spatial aspect ratio: 0.23 < gamma < 0.92
numFilterSizes = length(RF_siz); % we get 16
numSimpleFilters = length(rot); % we get 4
numFilters = numFilterSizes*numSimpleFilters; % we get 16x4 = 64 filters
fSiz = zeros(numFilters,1); % It is a vector of size numFilters where each cell contains the size of the filter (7,7,7,7,9,9,9,9,11,11,11,11,......,37,37,37,37)
filters = zeros(max(RF_siz)^2,numFilters); % Matrix of Gabor filters of size %max_fSiz x num_filters, where max_fSiz is the length of the largest filter and num_filters the total number of filters. Column j of filters matrix contains a n_jxn_j filter (reshaped as a column vector and padded with zeros).
for k = 1:numFilterSizes
for r = 1:numSimpleFilters
theta = rot(r)*pi/180; % so we get 0, pi/4, pi/2, 3pi/4
filtSize = RF_siz(k);
center = ceil(filtSize/2);
filtSizeL = center-1;
filtSizeR = filtSize-filtSizeL-1;
sigmaq = sigma(k)^2;
for i = -filtSizeL:filtSizeR
for j = -filtSizeL:filtSizeR
if ( sqrt(i^2+j^2)>filtSize/2 )
E = 0;
else
x = i*cos(theta) - j*sin(theta);
y = i*sin(theta) + j*cos(theta);
E = exp(-(x^2+G^2*y^2)/(2*sigmaq))*cos(2*pi*x/lambda(k));
end
f(j+center,i+center) = E;
end
end
f = f - mean(mean(f));
f = f ./ sqrt(sum(sum(f.^2)));
p = numSimpleFilters*(k-1) + r;
filters(1:filtSize^2,p)=reshape(f,filtSize^2,1);
fSiz(p)=filtSize;
end
end
% Rebuild all filters (of all sizes)
nFilts = length(fSiz);
for i = 1:nFilts
sqfilter{i} = reshape(filters(1:(fSiz(i)^2),i),fSiz(i),fSiz(i));
%if you will use conv2 to convolve an image with this gabor, so you should also add the equation below. But if you will use imfilter instead of conv2, so do not add the equation below.
sqfilter{i} = sqfilter{i}(end:-1:1,end:-1:1); %flip in order to use conv2 instead of imfilter (%bug_fix 6/28/2007);
convv=imfilter(image_double, sqfilter{i}, 'same', 'conv');
figure;
imagesc(convv);
colormap(gray);
end
phi = ij*pi/4; % ij = 0, 1, 2, 3
theta = 3;
sigma = 0.65*theta;
filterSize = 7; % 7:2:37
G = zeros(filterSize);
for i=(0:filterSize-1)/filterSize
for j=(0:filterSize-1)/filterSize
xprime= j*cos(phi);
yprime= i*sin(phi);
K = exp(2*pi*theta*sqrt(-1)*(xprime+ yprime));
G(round((i+1)*filterSize),round((j+1)*filterSize)) =...
exp(-(i^2+j^2)/(sigma^2))*K;
end
end
As of R2015b release of the Image Processing Toolbox, you can create a Gabor filter bank using the gabor function in the image processing toolbox, and you can apply it to an image using imgaborfilt.
In the frequency domain:
sigma_u=1/2*pi*sigmaq;
sigma_v=1/2*pi*sigmaq;
u0=2*pi*cos(theta)*lambda(k);
v0=2*pi*sin(theta)*lambda(k);
for u = -filtSizeL:filtSizeR
for v = -filtSizeL:filtSizeR
if ( sqrt(u^2+v^2)>filtSize/2 )
E = 0;
else
v_theta = u*cos(theta) - v*sin(theta);
u_theta = u*sin(theta) + v*cos(theta);
E=(1/2*pi*sigma_u*sigma_v)*((exp((-1/2)*(((u_theta-u0)^2/sigma_u^2))+((v_theta-v0)^2/sigma_v^2))) + (exp((-1/2)*(((u_theta+u0)^2/sigma_u^2))+((v_theta+v0)^2/sigma_v^2))));
end
f(v+center,u+center) = E;
end
end