I'm trying to compute the extrinsic matrix from the pose (position and orientation) of the camera given in world coordinates. I used the following to compute the extrinsic matrix,
T = [R -Rt; 0 1] 3x4 Matrix
The rotation(theta2) of the camera is about the Y-axes of the camera i.e. yaw about the camera axis. The translation vector is [x, y, z] in meters.
My Setup
theta1 = deg2rad(theta1);
RW1 = [[cos(theta1), 0, sin(theta1)];
[0,1, 0];
[-sin(theta1), 0, cos(theta1)]];
tW1 = [0; 0; 0];
TW1 = [RW1 tW1; 0 0 0 1];
theta2 = deg2rad(theta2);
R12 = [[cos(theta2), 0, sin(theta2)];
[0,1, 0];
[-sin(theta2), 0, cos(theta2)]];
t12 = [x; y; z];
T12 = [R12 t12; 0 0 0 1];
P1 = K * TW1;
P2 = K * T12;
K - Camera Intrinsic Matrix
Is this the right way to calculate the extrinsic matrix? Am I missing any transformations between the world and the camera frame?
I'm trying to implement this https://www.cs.cmu.edu/~16385/s17/Slides/11.4_Triangulation.pdf and for the camera matrix, I followed this https://www.cs.cmu.edu/~16385/s17/Slides/11.1_Camera_matrix.pdf.
Related
"Particles are centered at [0 0]. There are (free) particles moving around the center. Color interpolation is adopted to color the colors of the free particles.
t begins at 0. The particle position is randomly generated within a region in a uniform manner. A point p is inside the region iff (if only if), 50>=||p|| >= 10. Here, ||p|| is the distance between p and the origin [0 0]. Initialize the velocity of each particle so that it is orthogonal to the vector (p-[0 0]).
Velocity normalization: Make the velocity of the particle p orthogonal to the vector p – [0 0].
Assume p = [x y]. Then the velocity is
A) s * [y -x] /||p|| or
B) s * [ -y x]/||p||
Set s to 20. If you adopt the same rule (A or B) to set the velocities of all the particles, all the particles rotate in the same direction.
Color interpolation: The color of a particle depends on the distance, d, of the particle to the origin [0 0].
"
This is an old midterm question from a class. Here is my answer:
t = 0;
p = zeros(1,2); v = [0,0];
dt = 0.05;
M = 10000; m= 1;
tmax = 100;
figure, hold on
d = 0.025;
cp0 = [ 0 0 1]; %blue
cp1 = [ 1 0 0]; % red
n=50; s=20;
clf;
for i = 1:n
for j = 1:n
p([i,j]) = [10 + (50-10) .* rand(1,1),(10 + (50-10) .* rand(1,1))];
end
end
while t < tmax
for k= 1:n
for l = 1:n
F = -(p([k,l])./norm(p)).*(m*M./(1+p([k,l]).*p([k,l])));
a = F/m;
v = v + a.*dt;
v = s.*[-k,l]./norm(p);
d = norm([0,0] - p([k,l]))
p([k,l]) = p([k,l]) + v.*dt;
vD = min(1, d/50);
particle_color = cp0 + vD*(cp1-cp0); %color interpolation
end
clf;
plot(p,'o','MarkerEdgeColor',particle_color);
axis([-80 80 -80 80]); hold on
pause(0.05);
t = t + dt;
end
end
In my code, why are the particles not moving in circular motion?
Let's say I have the following vector field of force:
x = [-1 -0.5 0 0.5 1];
y = x;
dx = repmat([1,0.5,0,-0.5,-1],5,1);
dy = repmat([1,0.5,0,-0.5,-1]',1,5);
[X,Y] = meshgrid(x,y);
I want to obtain an energy landscape from this field.
If I just sum vectors I have the asymmetric picture:
imagesc(x,y,cumsum(dx,2)+cumsum(dy,1))
What is wrong in my code?
I have a point in 3d space (x,y,z). I want to move radially outward from that point discretely (say for r=1 and r=2). In the x,y plane I can simply move outward by stepping ((x+r cos(theta)), (y+r sin(theta)), z) with r = 1 or 2 and theta varying every, say 10 degrees.
However, I am unsure how to describe this movement if I want to have lines moving outward on a tilted plane and step my lines within this plane.
I thought it would be just using spherical coordinates. But if I'm drawing lines from a center point using (x=rho sin phi cos theta, y=..., z=...) won't that form a cone rather than a circle tilted on a plane?
P.S. Will be implementing this in MATLAB
You could first make the coordinates going outwards from P0 and then rotate the coordinates using a rotation matrix.
So you take points P for all R's and thetas, as MBo pointed out:
P = [ P0x + R * cos(theta); P0y + R * sin(theta); 0 ]
Then you make a rotation matrix that rotates the XY plane with the angles you want
If you multiply that with your coordinates you get the rotated coordinates. For example a 90 degree rotation about the Z axis for the point [1,0,0]:
However you probably want to rotate about the point P0 and not about the origin, then you have to make an affine matrix with the following translation:
tx = x- r00 * x - r01 * y - r02 * z
ty = y- r10 * x - r11 * y - r12 * z
tz = z- r20 * x - r21 * y - r22 * z
And then make an affine transformation matrix with T and R (designated as M in the figure, sorry):
In this figure Q are the old coordinates and Q' the new coordinates.
I had a similar problem and used this answer and adjusted it to your problem:
%input point and rotated plane
p0 = [10;10;10;1]; % the last entry is your homogeneous dimension
r0 = [45,45,45]; r0 = r0*pi/180;
%rotation to plane
Rx=[1 0 0 0;
0 cos(r0(1)) sin(r0(1)) 0;
0 -sin(r0(1)) cos(r0(1)) 0;
0 0 0 1];
Ry=[cos(r0(2)) 0 -sin(r0(2)) 0;
0 1 0 0;
sin(r0(2)) 0 cos(r0(2)) 0;
0 0 0 1];
Rz=[cos(r0(3)) sin(r0(3)) 0 0;
-sin(r0(3)) cos(r0(3)) 0 0;
0 0 1 0;
0 0 0 1];
R = Rz*Ry*Rx; A = R;
T = ( eye(3)-R(1:3,1:3) ) * p0(1:3); %calculate translation to rotate about the point P0
A(1:3,4) = T; % to rotate about the origin just leave out this line
%make coordinates for the points going outward from p0
nangles = 36; anglestep = 2*pi/nangles;
nradii = 2; radiistep = 1;
thetas = anglestep:anglestep:2*pi;
rs = radiistep:radiistep:nradii*radiistep;
npoints = nradii*nangles;
coordinates = zeros(4,npoints); curpoint = 0;
for itheta = 1:nangles; for iradius = 1:nradii;
curpoint = curpoint+1;
coordinates(:, curpoint) = p0+rs(iradius)*[cos(thetas(itheta));sin(thetas(itheta));0;0];
end; end
coordinates_tilted = A*coordinates; %rotate the coordinates to the new plane
Which results in this figure:
figure;
scatter3(coordinates_tilted(1,:),coordinates_tilted(2,:),coordinates_tilted(3,:), 'MarkerEdgeColor', 'green')
hold on
scatter3(coordinates(1,:),coordinates(2,:),coordinates(3,:), 'MarkerEdgeColor', 'red')
legend('tilted', 'original')
Or plot them as lines:
%or as lines
coorarray = reshape(coordinates, [4 nradii nangles]);
Xline = squeeze(coorarray(1,:,:));
Yline = squeeze(coorarray(2,:,:));
Zline = squeeze(coorarray(3,:,:));
coorarray_tilted = reshape(coordinates_tilted, [4 nradii nangles]);
Xline_tilted = squeeze(coorarray_tilted(1,:,:));
Yline_tilted = squeeze(coorarray_tilted(2,:,:));
Zline_tilted = squeeze(coorarray_tilted(3,:,:));
figure;
plot3(Xline,Yline,Zline, 'r');
hold on
plot3(Xline_tilted,Yline_tilted,Zline_tilted, 'g');
legend( 'original', 'tilted')
Does this answer your question? These are now points at all multiples of 36 degree angles at a distance of one and two from point P0 in the plane that is tilted 45 degrees on all axes around the point P0. If you need individual 'pixels' to designate your line (so integer coordinates) you can round the coordinates and that would be sort of a nearest neighbour approach:
coordinates_tilted_nearest = round(coordinates_tilted);
How is your tilted plane defined?
Define it with base point P0 and two perpendicular unit vectors U and V. It is not hard to get this representation from any other. For example, if normal vector of your plane have angles ax, ay, az with axes OX, OY, OZ respectively, it's normalized form is N = (nx, ny, nz) = (Cos(ax), Cos(ay), Cos(az)). You can choose arbitrary vector U (lying in the plane) as described here, and find V vector as vector product V = U x N
Then needed points are:
P = P0 + U * R * Cos(Theta) + V * R * Sin(Theta)
I was asked to perform an image rotation about an arbitrary point. The framework they provided was in matlab so I had to fill a function called MakeTransformMat that receives the angle of rotation and the point where we want to rotate.
As I've seen in class to do this rotation first we translate the point to the origin, then we rotate and finally we translate back.
The framework asks me to return a Transformation Matrix. Am I right to build that matrix as the multiplication of the translate-rotate-translate matrices? otherwise, what am I forgetting?
function TransformMat = MakeTransformMat(theta,center_y,center_x)
%Translate image to origin
trans2orig = [1 0 -center_x;
0 1 -center_y;
0 0 1];
%Rotate image theta degrees
rotation = [cos(theta) -sin(theta) 0;
sin(theta) cos(theta) 0;
0 0 1];
%Translate back to point
trans2pos = [1 0 center_x;
0 1 center_y;
0 0 1];
TransformMat = trans2orig * rotation * trans2pos;
end
This worked for me. Here I is the input image and J is the rotated image
[height, width] = size(I);
rot_deg = 45; % Or whatever you like (in degrees)
rot_xc = width/2; % Or whatever you like (in pixels)
rot_yc = height/2; % Or whatever you like (in pixels)
T1 = maketform('affine',[1 0 0; 0 1 0; -rot_xc -rot_yc 1]);
R1 = maketform('affine',[cosd(rot_deg) sind(rot_deg) 0; -sind(rot_deg) cosd(rot_deg) 0; 0 0 1]);
T2 = maketform('affine',[1 0 0; 0 1 0; width/2 height/2 1]);
tform = maketform('composite', T2, R1, T1);
J = imtransform(I, tform, 'XData', [1 width], 'YData', [1 height]);
Cheers.
I've answered a very similar question elsewhere: Here is the link.
In the code linked to, the point about which you rotate is determined by how the meshgrid is defined.
Does that help? Have you read the Wikipedia page on rotation matrices?
I have a 3-D grayscale volume corresponding to ultrasound data. In Matlab this 3-D volume is simply a 3-D matrix of MxNxP. The structure I'm interested in is not oriented along the z axis, but along a local coordinate system already known (x'y'z'). What I have up to this point is something like the figure shown below, depicting the original (xyz) and the local coordinate systems (x'y'z'):
I want to obtain the 2-D projection of this volume (i.e. an image) through a specific plane on the local coordinate system, say at z' = z0. How can I do this?
If the volume was oriented along the z axis this projection could be readily achieved. i.e. if the volume, in Matlab, is V, then:
projection = sum(V,3);
thus, the projection can be computed just as the sum along the 3rd dimension of the array. However with a change of orientation the problem becomes more complicated.
I've been looking at radon transform (2D, that applies only to 2-D images and not volumes) and also been considering ortographic projections, but at this point I'm clueless as to what to do!
Thanks for any advice!
New attempt at solution:
Following the tutorial http://blogs.mathworks.com/steve/2006/08/17/spatial-transformations-three-dimensional-rotation/ and making some small changes, I might have something which could help you. Bear in mind, I have little or no experience with volumetric data in MATLAB, so the implementation is quite hacky.
In the below code I use tformarray() to rotate the structure in space. First, the data is centered, then rotated using rotationmat3D to produce the spacial transformation, before the data is moved back to its original position.
As I have never used tformarray before, I handeled datapoints falling outside the defined region after rotation by simply padding the data matrix (NxMxP) with zeros all around. If anyone know a better way, please let us know :)
The code:
%Synthetic dataset, 25x50x25
blob = flow();
%Pad to allow for rotations in space. Bad solution,
%something better might be possible to better understanding
%of tformarray()
blob = padarray(blob,size(blob));
f1 = figure(1);clf;
s1=subplot(1,2,1);
p = patch(isosurface(blob,1));
set(p, 'FaceColor', 'red', 'EdgeColor', 'none');
daspect([1 1 1]);
view([1 1 1])
camlight
lighting gouraud
%Calculate center
blob_center = (size(blob) + 1) / 2;
%Translate to origin transformation
T1 = [1 0 0 0
0 1 0 0
0 0 1 0
-blob_center 1];
%Rotation around [0 0 1]
rot = -pi/3;
Rot = rotationmat3D(rot,[0 1 1]);
T2 = [ 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1];
T2(1:3,1:3) = Rot;
%Translation back
T3 = [1 0 0 0
0 1 0 0
0 0 1 0
blob_center 1];
%Total transform
T = T1 * T2 * T3;
%See http://blogs.mathworks.com/steve/2006/08/17/spatial-transformations-three-dimensional-rotation/
tform = maketform('affine', T);
R = makeresampler('linear', 'fill');
TDIMS_A = [1 2 3];
TDIMS_B = [1 2 3];
TSIZE_B = size(blob);
TMAP_B = [];
F = 0;
blob2 = ...
tformarray(blob, tform, R, TDIMS_A, TDIMS_B, TSIZE_B, TMAP_B, F);
s2=subplot(1,2,2);
p2 = patch(isosurface(blob2,1));
set(p2, 'FaceColor', 'red', 'EdgeColor', 'none');
daspect([1 1 1]);
view([1 1 1])
camlight
lighting gouraud
The arbitrary visualization below is just to confirm that the data is rotated as expected, plotting a closed surface when the data passed the value '1'. With blob2, you should know be able to project by using simple sums.
figure(2)
subplot(1,2,1);imagesc(sum(blob,3));
subplot(1,2,2);imagesc(sum(blob2,3));
Assuming you have access to the coordinate basis R=[x' y' z'], and that those vectors are orthonormal, you can simply extract the representation in this basis by multiplying your data with the the 3x3 matrix R, where x',y',z' are column vectors.
With the data stored in D (Nx3), you can get the representation with R, by multiplying by it:
Dmarked = D*R;
and now D = Dmarked*inv(R), so going back and forth is stragihtforward.
The following code might provide help to see the transformation. Here I create a synthetic dataset, rotate it, and then rotate it back. Doing sum(DR(:,3)) would then be your sum along z'
%#Create synthetic dataset
N1 = 250;
r1 = 1;
dr1 = 0.1;
dz1 = 0;
mu1 = [0;0];
Sigma1 = eye(2);
theta1 = 0 + (2*pi).*rand(N1,1);
rRand1 = normrnd(r1,dr1,1,N1);
rZ1 = rand(N1,1)*dz1+1;
D = [([rZ1*0 rZ1*0] + repmat(rRand1',1,2)).*[sin(theta1) cos(theta1)] rZ1];
%Create roation matrix
rot = pi/8;
R = rotationmat3D(rot,[0 1 0]);
% R = 0.9239 0 0.3827
% 0 1.0000 0
% -0.3827 0 0.9239
Rinv = inv(R);
%Rotate data
DR = D*R;
%#Visaulize data
f1 = figure(1);clf
subplot(1,3,1);
plot3(DR(:,1),DR(:,2),DR(:,3),'.');title('Your data')
subplot(1,3,2);
plot3(DR*Rinv(:,1),DR*Rinv(:,2),DR*Rinv(:,3),'.r');
view([0.5 0.5 0.2]);title('Representation using your [xmarked ymarked zmarked]');
subplot(1,3,3);
plot3(D(:,1),D(:,2),D(:,3),'.');
view([0.5 0.5 0.2]);title('Original data before rotation');
If you have two normalized 3x1 vectors x2 and y2 corresponding to your local coordinate system (x' and y').
Then, for a position P, its local coordinate will be xP=P'x2 and yP=P'*y2.
So you can try to project your volume using accumarray:
[x y z]=ndgrid(1:M,1:N,1:P);
posP=[x(:) y(:) z(:)];
xP=round(posP*x2);
yP=round(posP*y2);
xP=xP+min(xP(:))+1;
yP=yP+min(yP(:))+1;
V2=accumarray([xP(:),yP(:)],V(:));
If you provide your data, I will test it.