I currently have a stereo camera setup. I have calibrated both cameras and have the intrinsic matrix for both cameras K1 and K2.
K1 = [2297.311, 0, 319.498;
0, 2297.313, 239.499;
0, 0, 1];
K2 = [2297.304, 0, 319.508;
0, 2297.301, 239.514;
0, 0, 1];
I have also determined the Fundamental matrix F between the two cameras using findFundamentalMat() from OpenCV. I have tested the Epipolar constraint using a pair of corresponding points x1 and x2 (in pixel coordinates) and it is very close to 0.
F = [5.672563368940768e-10, 6.265600996978877e-06, -0.00150188302445251;
6.766518121363063e-06, 4.758206104804563e-08, 0.05516598334827842;
-0.001627120880791009, -0.05934224611334332, 1];
x1 = 133,75
x2 = 124.661,67.6607
transpose(x2)*F*x1 = -0.0020
From F I am able to obtain the Essential Matrix E as E = K2'*F*K1. I decompose E using the MATLAB SVD function to get the 4 possibilites of rotation and translation of K2 with respect to K1.
E = transpose(K2)*F*K1;
svd(E);
[U,S,V] = svd(E);
diag_110 = [1 0 0; 0 1 0; 0 0 0];
newE = U*diag_110*transpose(V);
[U,S,V] = svd(newE); //Perform second decompose to get S=diag(1,1,0)
W = [0 -1 0; 1 0 0; 0 0 1];
R1 = U*W*transpose(V);
R2 = U*transpose(W)*transpose(V);
t1 = U(:,3); //norm = 1
t2 = -U(:,3); //norm = 1
Let's say that K1 is used as the coordinate frame for which we make all measurements. Therefore, the center of K1 is at C1 = (0,0,0). With this it should be possible to apply the correct rotation R and translation t such that C2 = R*(0,0,0)+t (i.e. the center of K2 is measured with respect to the center of K1)
Now let's say that using my corresponding pairs x1 and x2. If I know the length of each pixel in both my cameras and since I know the focal length from the intrinsic matrix, I should be able to determine two vectors v1 and v2 for both cameras that intersect at the same point as seen below.
pixel_length = 7.4e-6; //in meters
focal_length = 17e-3; //in meters
dx1 = (133-319.5)*pixel_length; //x-distance from principal point of 640*480 image
dy1 = (75-239.5) *pixel_length; //y-distance from principal point of 640*480 image
v1 = [dx1 dy1 focal_length] - (0,0,0); //vector found using camera center and corresponding image point on the image plane
dx2 = (124.661-319.5)*pixel_length; //same idea
dy2 = (67.6607-239.5)*pixel_length; //same idea
v2 = R * ( [dx2 dy2 focal_length] - (0,0,0) ) + t; //apply R and t to measure v2 with respect to K1 frame
With this vector and knowing the line equation in parametric form, we can then equate the two lines to triangulate and solve the two scalar quantities s and t through the left hand divide function in MATLAB to solve for the system of equations.
C1 + s*v1 = C2 + t*v2
C1-C2 = tranpose([v2 v1])*transpose([s t]) //solve Ax = B form system to find s and t
With s and t determined we can find the triangulated point by plugging back into the line equation. However, my process has not been successful as I cannot find a single R and t solution in which the point is in front of both cameras and where both cameras are pointed forwards.
Is there something wrong with my pipeline or thought process? Is it at all possible to obtain each individual pixel ray?
When you decompose the essential matrix into R and t you get 4 different solutions. Three of them project the points behind one or both cameras, and one of them is correct. You have to test which one is correct by triangulating some sample points.
There is a function in the Computer Vision System Toolbox in MATLAB called cameraPose, which will do that for you.
Should it be not C1-C2 = transpose([v2 -v1] * transpose([t s]). This works.
Checked your code and found that the determinants of both R1 and R2 are -1, which is incorrect because as a rotation matrix R should have a determinant equal to 1. Just take R=-R and try again.
Related
I have been struggeling with this quiz question. This was part of FSG 2022 registration quiz and I can't figure out how to solve it
At first I thought that I can use extrinsic and intrinsic parameters to calculate 3D coordinates using equations described by Mathworks or in this article. Later I realized that the distance to the object is provided in camera frame, which means that this could be treat as a depth camera and convert depth info into 3d space as described in medium.com article
this article is using formula show below to calculate x and y coordinates and is very similar to this question, yet I can't get the correct solution.
One of my Matlab scripts attempting to solve it:
rot = eul2rotm(deg2rad([102 0 90]));
trans = [500 160 1140]' / 1000; % mm to m
t = [rot trans];
u = 795; % here was typo as pointed out by solstad.
v = 467;
cx = 636;
cy = 548;
fx = 241;
fy = 238;
z = 2100 / 1000 % mm to m
tmp_x = (u - cx) * z / fx;
tmp_y = (v - cy) * z / fy;
% attempt 1
tmp_cords = [tmp_x; tmp_y; z; 1]
linsolve(t', tmp_cords)'
% result is: 1.8913 1.8319 -0.4292
% attempt 2
tmp_cords = [tmp_x; tmp_y; z]
rot * tmp_cords + trans
% result is: 2.2661 1.9518 0.4253
If possible I would like to see the calculation process not any kind of a python code.
Correct answer is under the image.
Correct solution provided by the organisers were 2.030, 1.272, 0.228 m
The task states that the object's euclidean (straight-line) distance is 2.1 m. That doesn't mean its distance along z is 2.1 m. Those two only coincide if there is no x or y component in the object's translation to the camera frame.
The z component of the object's translation will be less than 2.1 meters.
You need to take a ray/vector for the screen space coordinates (normalized) and multiply that by the euclidean distance.
v_x = (u - cx) / fx;
v_y = (v - cy) / fy;
v_z = 1;
v = [v_x; v_y; v_z];
dist = 2.1;
tmp = v / norm(v) * dist;
The rotation may be an issue. Roll happens around X, then pitch happens around Y, and then yaw happens around Z. These operations are applied in that order, i.e. inner to outer.
R_Z * R_Y * R_X * v
My rotation matrix is
[[ 0. 0.20791 0.97815]
[ 1. 0. 0. ]
[ 0. 0.97815 -0.20791]]
That camera, taking the usual (X right, Y down, Z far) frame, would be looking, upside down, out the windshield, and slightly down.
Make sure that eul2rotm() does the right thing (specify axis order as 'XYZ') or that you use something else.
You can use rotvec2mat3d() to build individual rotation matrices from an axis-angle encoding.
Perhaps also review different MATLAB conventions regarding matrix multiplication: https://www.mathworks.com/help/images/migrate-geometric-transformations-to-premultiply-convention.html
I used Python and scipy.spatial.transform.Rotation.from_euler('xyz', [R_roll, R_pitch, R_yaw], degrees=True).as_matrix() to arrive at the sample solution.
Properly, the task should have specified a frame conversion step between vehicle and camera because the differing views are quite confusing, with a car having +X being forward and a camera having +Z being forward...
In addition to Christoph Rackwitz answer, which is correct and should get all the credited, here is a working Matlab script:
rot = eul2rotm(deg2rad([90 0 102]));
trans = [500 160 1140]' / 1000; % mm to m
u = 795;
v = 467;
cx = 636;
cy = 548;
fx = 241;
fy = 238;
v_x = (u - cx) / fx;
v_y = (v - cy) / fy;
v_z = 1;
v = [v_x; v_y; v_z];
dist = 2.1;
tmp = v / norm(v) * dist;
rot * tmp + trans
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 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.
From a couple references (i.e., http://en.wikipedia.org/wiki/Rotation_matrix "Rotation matrix from axis and angle", and exercise 5.15 in "Computer Graphics - Principles and Practice" by Foley et al, 2nd edition in C), I've seen this definition of a rotation matrix (implemented below in Octave) that rotates points by a specified angle about a specified vector. Although I have used it before, I'm now seeing rotation problems that appear to be related to orientation. The problem is recreated in the following Octave code that
takes two unit vectors: src (green in figures) and dst (red in figures),
calculates the angle between them: theta,
calculates the vector normal to both: pivot (blue in figures),
and finally attempts to rotate src into dst by rotating it about vector pivot by angle theta.
% This test fails: rotated unit vector is not at expected location and is no longer normalized.
s = [-0.49647; -0.82397; -0.27311]
d = [ 0.43726; -0.85770; -0.27048]
test_rotation(s, d, 1);
% Determine rotation matrix that rotates the source and normal vectors to the x and z axes, respectively.
normal = cross(s, d);
normal /= norm(normal);
R = zeros(3,3);
R(1,:) = s;
R(2,:) = cross(normal, s);
R(3,:) = normal;
R
% After rotation of the source and destination vectors, this test passes.
s2 = R * s
d2 = R * d
test_rotation(s2, d2, 2);
function test_rotation(src, dst, iFig)
norm_src = norm(src)
norm_dst = norm(dst)
% Determine rotation axis (i.e., normal to two vectors) and rotation angle.
pivot = cross(src, dst);
theta = asin(norm(pivot))
theta_degrees = theta * 180 / pi
pivot /= norm(pivot)
% Initialize matrix to rotate by an angle theta about pivot vector.
ct = cos(theta);
st = sin(theta);
omct = 1 - ct;
M(1,1) = ct - pivot(1)*pivot(1)*omct;
M(1,2) = pivot(1)*pivot(2)*omct - pivot(3)*st;
M(1,3) = pivot(1)*pivot(3)*omct + pivot(2)*st;
M(2,1) = pivot(1)*pivot(2)*omct + pivot(3)*st;
M(2,2) = ct - pivot(2)*pivot(2)*omct;
M(2,3) = pivot(2)*pivot(3)*omct - pivot(1)*st;
M(3,1) = pivot(1)*pivot(3)*omct - pivot(2)*st;
M(3,2) = pivot(2)*pivot(3)*omct + pivot(1)*st;
M(3,3) = ct - pivot(3)*pivot(3)*omct;
% Rotate src about pivot by angle theta ... and check the result.
dst2 = M * src
dot_dst_dst2 = dot(dst, dst2)
if (dot_dst_dst2 >= 0.99999)
"success"
else
"FAIL"
end
% Draw the vectors: green is source, red is destination, blue is normal.
figure(iFig);
x(1) = y(1) = z(1) = 0;
ubounds = [-1.25 1.25 -1.25 1.25 -1.25 1.25];
x(2)=src(1); y(2)=src(2); z(2)=src(3);
plot3(x,y,z,'g-o');
hold on
x(2)=dst(1); y(2)=dst(2); z(2)=dst(3);
plot3(x,y,z,'r-o');
x(2)=pivot(1); y(2)=pivot(2); z(2)=pivot(3);
plot3(x,y,z,'b-o');
x(2)=dst2(1); y(2)=dst2(2); z(2)=dst2(3);
plot3(x,y,z,'k.o');
axis(ubounds, 'square');
view(45,45);
xlabel("xd");
ylabel("yd");
zlabel("zd");
hold off
end
Here are the resulting figures. Figure 1 shows an orientation that doesn't work. Figure 2 shows an orientation that works: the same src and dst vectors but rotated into the first quadrant.
I was expecting the src vector to always rotate onto the dst vector, as shown in Figure 2 by the black circle covering the red circle, for all vector orientations. However Figure 1 shows an orientation where the src vector does not rotate onto the dst vector (i.e., the black circle is not on top of the red circle, and is not even on the unit sphere).
For what it's worth, the references that defined the rotation matrix did not mention orientation limitations, and I derived (in a few hours and a few pages) the rotation matrix equation and didn't spot any orientation limitations there. I'm hoping the problem is an implementation error on my part, but I haven't been able to find it yet in either of my implementations: C and Octave. Have you experienced orientation limitations when implementing this rotation matrix? If so, how did you work around them? I would prefer to avoid the extra translation into the first quadrant if it isn't necessary.
Thanks,
Greg
Seems two minus signs have escaped:
M(1,1) = ct - P(1)*P(1)*omct;
M(1,2) = P(1)*P(2)*omct - P(3)*st;
M(1,3) = P(1)*P(3)*omct + P(2)*st;
M(2,1) = P(1)*P(2)*omct + P(3)*st;
M(2,2) = ct + P(2)*P(2)*omct; %% ERR HERE; THIS IS THE CORRECT SIGN
M(2,3) = P(2)*P(3)*omct - P(1)*st;
M(3,1) = P(1)*P(3)*omct - P(2)*st;
M(3,2) = P(2)*P(3)*omct + P(1)*st;
M(3,3) = ct + P(3)*P(3)*omct; %% ERR HERE; THIS IS THE CORRECT SIGN
Here is a version that is much more compact, faster, and also based on Rodrigues' rotation formula:
function test
% first test: pass
s = [-0.49647; -0.82397; -0.27311];
d = [ 0.43726; -0.85770; -0.27048]
d2 = axis_angle_rotation(s, d)
% Determine rotation matrix that rotates the source and normal vectors to the x and z axes, respectively.
normal = cross(s, d);
normal = normal/norm(normal);
R(1,:) = s;
R(2,:) = cross(normal, s);
R(3,:) = normal;
% Second test: pass
s2 = R * s;
d2 = R * d
d3 = axis_angle_rotation(s2, d2)
end
function vec = axis_angle_rotation(vec, dst)
% These following commands are just here for the function to act
% the same as your original function. Eventually, the function is
% probably best defined as
%
% vec = axis_angle_rotation(vec, axs, angle)
%
% or even
%
% vec = axis_angle_rotation(vec, axs)
%
% where the length of axs defines the angle.
%
axs = cross(vec, dst);
theta = asin(norm(axs));
% some preparations
aa = axs.'*axs;
ra = vec.'*axs;
% location of circle centers
c = ra.*axs./aa;
% first coordinate axis on the circle's plane
u = vec-c;
% second coordinate axis on the circle's plane
v = [axs(2)*vec(3)-axs(3)*vec(2)
axs(3)*vec(1)-axs(1)*vec(3)
axs(1)*vec(2)-axs(2)*vec(1)]./sqrt(aa);
% the output vector
vec = c + u*cos(theta) + v*sin(theta);
end
I have a question connected to this code:
t = -20:0.1:20;
plot3(zeros(size(t)),t,-t.^2);
grid on
hold on
i = 1;
h = plot3([0 0],[0 t(i)],[0 -t(i)^2],'r');
h1 = plot3([-1 0],[0 0],[-400 -200],'g');
for(i=2:length(t))
set(h,'xdata',[-1 0],'ydata',[0 t(i)],'zdata',[-400 -t(i)^2]);
pause(0.01);
end
In this code, I plot two intersecting lines. H1, and H2. H1 is fixed, H2 moves as a function of time. H2 happens to trace a parabola in this example, but its movement could be arbitrary.
How can I calculate and draw the bisector of the angle between these two intersecting lines for every position of the line H2? I would like to see in the plot the bisector and the line H2 moving at the same time.
Solving this problem for one position of H2 is sufficient, since it will be the same procedure for all orientations of H2 relative to H1.
I am not a geometry genius, there is likely an easier way to do this. As of now, no one has responded though, so this will be something.
You have three points in three space:
Let A be the common vertice of the two line segments.
Let B and C be two known points on the two line segments.
Choose an arbitrary distance r where
r <= distance from A to B
r <= distance from A to C
Measure from A along line segment AB a distance of r. This is point RB
Measure from A along line segment AC a distance or r. This is point RC
Find the mid point of line segment connecting RB and RC. This is point M
Line segment AM is the angular bisector of angle CAB.
Each of these steps should be relatively easy to accomplish.
Here is basically MatlabDoug's method with some improvement on the determination of the point he calls M.
t = -20:0.1:20;
plot3(zeros(size(t)),t,-t.^2);
grid on
hold on
v1 = [1 0 200];
v1 = v1/norm(v1);
i = 1;
h = plot3([-1 0],[0 t(i)],[-400 -t(i)^2],'r');
h1 = plot3([-1 0],[0 0],[-400 -200],'g');
l = norm([1 t(i) -t(i)^2+400]);
p = l*v1 + [-1 0 -400];
v2 = (p + [0 t(i) -t(i)^2])/2 - [-1 0 -400];
p2 = [-1 0 -400] + v2/v2(1);
h2 = plot3([-1 p2(1)],[0 p2(2)],[-400 p2(3)],'m');
pause(0.1)
for(i=2:length(t))
l = norm([1 t(i) -t(i)^2+400]);
p = l*v1 + [-1 0 -400];
v2 = (p + [0 t(i) -t(i)^2])/2 - [-1 0 -400];
p2 = [-1 0 -400] + v2/v2(1);
set(h,'xdata',[-1 0],'ydata',[0 t(i)],'zdata',[-400 -t(i)^2]);
set(h2,'xdata',[-1 p2(1)],'ydata',[0 p2(2)],'zdata',[-400 p2(3)]);
pause;
end
I just use the following:
Find the normalized vectors AB, and AC, where A is the common point of the segments.
V = (AB + AC) * 0.5 // produces the direction vector that bisects AB and AC.
Normalize V, then do A + V * length to get the line segment of the desired length that starts at the common point.
(Note that this method does not work on 3 points along a line to produce a perpendicular bisector, it will yield a vector with no length in that case)
I have added a C# implementation (in the XZ plane using Unity 3D Vector3 struct) that handles Perpendicular and Reflex bisectors in case someone that knows MATLAB would translate it.
public Vector3 GetBisector(Vector3 center, Vector3 first, Vector3 second)
{
Vector3 firstDir = (first - center).normalized;
Vector3 secondDir = (second - center).normalized;
Vector3 result = ((firstDir + secondDir) * 0.5f).normalized;
if (IsGreaterThan180(-firstDir, secondDir))
{
// make into a reflex vector
(result.x, result.z) = (-result.x, -result.z);
}
if (result.sqrMagnitude < 0.99f)
{
// we have a colinear set of lines.
// return the perpendicular bisector.
result = Vector3.Cross(Vector3.up, -firstDir).normalized;
}
return result;
}
bool IsGreaterThan180(Vector3 dir, Vector3 dir2)
{
// < 0.0 for clockwise ordering
return (dir2.x * dir.z - dir2.z * dir.x) < 0.0f;
}
Also note that the returned bisector is a vector of unit length. Using "center + bisector * length" could be used to place it into worldspace.