There are two reasons for me to ask this question:
I want to know if my understanding on this issue is correct.
To clarify a doubt I have.
I want to change the co-ordinate system of a set of points (Old cartesian coordinates system to New cartesian co-ordinate system). This transformation will involve Translation as well as Rotation. This is what I plan to do:
With respect to this image I have a set of points which are in the XYZ coordinate system (Red). I want to change it with respect to the axes UVW (Purple). In order to do so, I have understood that there are two steps involved: Translation and Rotation.
When I translate, I only change the origin. (say, I want the UVW origin at (5,6,7). Then, for all points in my data, the x co-ordinates will be subtracted by 5, y by 6 and z by 7. By doing so. I get a set of Translated data.)
Now I have to apply a rotation transform (on the Translated data). The Rotation matrix is shown in the image. The values Ux, Uy and Uz are the co-ordinates of a point on the U axis which has unit distance from origin. Similarly, the values Vx, Vy and Vz are the coordinates of a point on the V axis which has a unit distance from origin. (I want to know if I am right here.) Wx, Wy, Wz is calculated as ((normalized u) X (normalised v))
(Also, if it serves any purpose, I would like to let you know that I am using MATLAB.)
edit:
I have a set of 42 points in 3D (42 X 3 matrix A) I want the first point to be considered as origin of UVW plane. So the values of the first point will be my translation vector. Correct?
Next, to calculate the Rotation vector: According to my requirement, the 6th row of matrix A has to be the U axis while 37th row has to be V axis. Consequently, vector u will be (1st row minus 6th row) of matrix A. While vector v will be (1st row minus 37th row) of matrix A.
The first row of Rotation Matrix will be vector u/|u| (normalized). Second row will be vector v/|v| (v normalized). The third row will be (u X v) . Am I right here?
Given this information, how can I calculate the value of Wx, Wy and Wx. How can I calculate the 3rd row of rotation matrix R?
Since you already have U and V, the two basis vectors of the orthonormal UVW system, the W basis vector would be the cross product of U and V. The cross product gives out the vector that is perpendicular to its operands; hence W = U × V. The components of W would fill in the third row of the rotation matrix.
Is my approach correct?
The order of the transforms matter; changing the order would lead to different results. When doing transformations of systems, usually scaling and rotation are tackled first and translation is dealt with lastly. The reason for this is that rotation would always be with respect to the origin. If the new system isn't on the old one's origin then applying rotation would rotate the new system not around its own origin but around the old system's origin. See the rightside case of figure 3-4 on this page to understand the difference what would happen if it's not on the origin; imagine the pot as the UVW coordinate system.
Think of both the coordinate systems being super-imposed (laid one atop the other). Now when you rotate UVW system with respect to the origin of XYZ, you will end up with the effect of rotating UVW w.r.t its own origin. Once rightly oriented, you can apply translation to it. However, if you'd already translated, then rotating would lead to translated rotation.
If you're using column-vector convention then TR would be the order i.e. rotation followed by translation. If you're using the row-vector convention then RT would be the order, again the order is rotation followed by translation.
You can apply the cross product of the Vectors OU and OV.
I think it's easier to perform it in steps. 1) Translation. 2) Rotation about x-axis. 3) Rotation about y-axis. 4) Rotation about z-axis.
% Assuming this is your coordinates before any operation
x0 = 5; y0 = 5; z0 = 5;
% This is the new origin
u = 5; v = 6, w = 7;
% If you wish to rotate pi/4 about x-axis, pi/3 about y-axis, pi/2 about z- axis, the three representative rotation matrix will be:
rx = [1 0 0; 0 cos(pi/4) -sin(pi/4); 0 sin(pi/4) cos(pi/4)];
ry = [cos(pi/3) 0 sin(pi/3); 0 1 0; -sin(pi/3) 0 cos(pi/3)];
rz = [cos(pi/2) -sin(pi/2) 0; sin(pi/2) cos(pi/2) 0; 0 0 1];
% First perform translation
xT = x0-u; yT = y0-v; zT = z0-w;
% Then perform rotation about x
rotated_x = mtimes( rx,[xT;yT;zT]);
% Then perform rotation about y
rotated_xy = mtimes( ry, rotated_x);
% Then perform rotation about z
rotated_xyz = mtimes( rz, rotated_xy);
Related
I am carrying out 3D imaging alignment using (as an overview) the following coding scheme:
ref = imref3d(size(img(:,:,:,1)),[0 32] ,[0 32] ,[0 20] );
[optimizer, metric] = imregconfig( 'monomodal');
tform=imregtform(img(:,:,:,2), ref, img(:,:,:,1), ref, 'rigid', optimizer, metric);
transform_mat=tform.T
For clarification, the variable img is a time series of 3D images (where each 3D image is a stack of 2D cross sections). At different time points, the object I am imaging can move. Thus, the purpose of the above code is to generate a geometric transformation that optimally aligns the second time point of the 3D volume ( img(:,:,:,2) ) to a reference image, which is the first time point of the 3D volume ( img(:,:,:,1)). The geometric transformation that is finally chosen by the optimization algorithm is output to the object tform. tform contains a 4x4 matrix as one of its properties (T); it is this 4x4 matrix that encodes the translation and rotation information. As you can see from my above code, I stored this 4x4 matrix in the variable transform_mat.
After reading through a lot of scattered mathworks documentation, I determined that this transform_mat variable (representing an affine rigid body transformation matrix) is in a "post-multiplied" form, which, as I understand it, just means that it is the transposed version of what one would traditionally see in a linear algebra text book.
For my purposes, I am interested in extracting specific rotation information from transform_mat. Here is what I have done so far:
rot_postmultiply=transform_mat(1:3,1:3); %extracting the elements that encode rotation-related info
rot_premultiply=rot_postmultply'; %transposing
Just to quickly interject, I created the premultiplied version of the rotation matrix because I believe that many of the functions that act on rotation matrices assume that it is in its premultiplied form.
From this 3x3 rotation matrix, I want to extract the rotations (in radians) about the STATIC x-axis, y-axis, and z-axis of the reference image. My initial attempt at carrying this out is as follows:
eul = rotm2eul(rot_premultiply);
The rotm2eul function provides me the 3 Euler Angles associated with this 3x3 rotation matrix. eul is a 1x3 vector and, according to documentation, the "default order for Euler angle rotations is 'ZYX' ". However, I am not certain that Euler Angles are actually describing the information I want to extract (i.e. rotations about the static x, y, z axes of the reference image).
I do not have a strong linear algebra/geometric transformation background, but my understanding of Euler Angles is that each rotation that takes place changes the coordinate system. For example, after the rotation about the Z axis (the first value in the eul vector), we now have new X and Y axes (call them X' and Y'). Then the "Y-axis rotation" (the second value in eul) is actually the rotation about Y'...not Y. Repeat this argument for the final "X-rotation" (which would really be about an X'' axis).
If anyone could offer some insight about how to proceed (and if my concerns about Euler Angles are correct), I would greatly appreciate it!
Also, sorry if the word "static" is the incorrect terminology. Hopefully, I have provided sufficient context so that no confusion arises.
If I understand you correctly, all you need is rotm2axang that converts a rotation given as an orthonormal rotation matrix, rotm, to the corresponding axis-angle representation, axang. The input rotation matrix must be in the premultiply form for rotations.
The output of rot2axang is an n-by-4 matrix of n axis-angle rotations. The first three elements of every row specify the rotation axis, and the last element defines the rotation angle (in radians).
If you dont have access to the robotics system toolbox that has that function, consider this alternative instead.
EDIT:
Rotation of a point in 3D space by an angle alpha about an arbitrary axis defined by a line between two points (x1,y1,z1) and (x2,y2,z2) can be achieved by the following steps:
translation so that the rotation axis passes through the origin
rotation around x axis so that the rotation axis lies in the xz plane (or projection on the xz plane)
rotation around y axis so that the rotation axis lies along the z axis (or projection on the z axis)
rotation by alpha around the z axis
the inverse of step 3
the inverse of step 2
the inverse of step 1
Are you asking about the angles in steps 2,3?
Otherwise, are you already in the origin for the axis around which you rotate?
If indeed you need the angles in step 2,3 then:
step #1 is done via a the translation matrix T:
T = [ 1 0 0 -x1
0 1 0 -y1
0 0 1 -z1
0 0 0 1 ];
Step #2 is done via the following:
Given that v = (a,b,c) is the unit vector along the rotation axis, then d = sqrt(b^2 + c^2) as the length of the projection onto the yz plane. Then rotate by the angle between the projection of rotation axis in the yz plane and the z axis given by the dot product of the z component of the unit vector v and its yz projection. This angle (beta) is determine by: cos(beta) = dot([0,0,c],[0,b,c]) /(c*d) = c/d , sin(beta) = cross( [0,0,c],[0,b,c]) /(c*d) = b/d
The rotation matrix Rx is therefor:
Rx = [ 1 0 0 0
0 c/d -b/d 0
0 b/d c/d 0
0 0 0 1 ] ;
Step 3 we rotate around the y axis so that the rotation axis lies along the positive z axis. Using dot and cross product relationships the cosine of the angle is d, the sine of the angle is a. The rotation matrix about the y axis Ry is
Ry = [ d 0 -a 0
0 1 0 0
a 0 d 0
0 0 0 1];
Last, we rotate around z simply by the angle alpha
Rz = [ cos(alpha) sin(alpha) 0 0
-sin(alpha) cos(alpha) 0 0
0 0 1 0
0 0 0 1 ] ;
I think the answers to this question should help you out: to decompose a 3D rotation into Cartesian components.
I have a target coordinate (xyz) within a 3D rectangle. The target coordinate is set assuming the 3D rectangle is in a flat plane with x,y,z. The target coordinate is set to be on one terminal end of the rectangle, with the 'y' component set to be -2 from the top of the 3D square (Refer to image). However, in reality the 3D rectangle is not in a flat plane with x,y,z axes, it is skewed. We calculate the skew by measuring two reference points on the top surface of the rectangle and now want to recalculate the target coordinate. I do not know how to ensure I am computing the skew correctly or (more importantly) plot the old vs new 3D rectangle and target coordinate. Below is code to generate both the matrix and the target within said matrix.
For the two reference points used to calculate the skew of the 3D rectangle, we name one 'bregma' and one 'lamda'.
%% CONVENTIONS:
%1) numbers get lower when
% 1) going to left on x axis
% 2) going down y axis
% 3) going posterior (or towards you) on z axis,
% opposite of convention
%2) coordinate order is AP (anterior posterior), ML
%(medial lateral), DV
%(dorsal ventral)
%Bregma is the anterior reference point and
%Lambda is posterior reference point . The goal variables
%are the point in space I want to reach. I'm hoping the code creates new
%points for me as caused by a change in elevation between point "c" and "f"
%Note, for AP/ML/DV absolute values, the actual integers %are merely
%reference points in space. It is the delta between AP %and AP or ML and ML
%that is meaningful
%values a-f would be acquired from the stereotaxic frame
a= 43; %AP at bregma
b= 20; %ML at bregma
c= 22; %DV at bregma
d= 38.8; %AP at lambda
e= 20; %ML at Lambda
f= 21.5; %DV at lamda This variable causes the change %in which I want the rotation to correct
%Bregma =[b a c];
%Following three points are given relative to bregma, this is convention. So mathematically 'Bregma=[0 0 0];'
%These are also the coordinates I want altered from the rotation matrix
MLgoal=0; %ML to injection
APgoal=0; %AP to injection
DVgoal=-2.0; %DV to injection
DVgoal_zaxis_signconvention_correction = -DVgoal; %we want sign convention flipped for z axis[![enter image description here][1]][1]
A= a-d;
M= b-e;
D= c-f;
%Bregma =[0 0 0];
v = [A M D];
mv = norm(v);
u=[A 0 0]; %theoretical vector [ML,AP,DV] bregma to %lambda
mu = norm(u);
%angle in radians
X=acos(dot(u,v)/(mu*mv));
cu=cross(u,v);
w=cu/norm(cu); %unit vector
%rotate about this unit vector by angle X
wx=w(1);
wy=w(2);
wz=w(3);
%sustitute variables into general rotation for more %readable code
vx = 1-cos(X);
cx = cos(X);
sx = sin(X);
%rotation matrix about arbitrary unit vector [wx; wy; %wz] by angle X
r=[vx*wx*wx + cx, vx*wx*wy - wz*sx, vx*wx*wz + wy*sx;
vx*wx*wy + wz*sx, vx*wy*wy + cx, vx*wy*wz - wx*sx;
vx*wx*wz - wy*sx, vx*wy*wz + wx*sx, vx*wz*wz + cx];
%Newcoordinate output should tell you the change in %targeting coordinate
%after correcting for the output of rotational matrix. %The value for DV
%seems too large based on the subtle increase elevation %of reference point
%Bregma to reference point Lambda
Newcoordinates=r* [APgoal;MLgoal;DVgoal_zaxis_signconvention_correction]; %should give you the new coordinates for the injection %site
Decomposing rotation matrix (x,y',z'') - Cartesian angles
Im currently working with rotation matrices and I have the following problem:
Given three coordinate systems (O0,x0,y0,z0; O1,x1,y1,z1; O2,x2,y2,z2) which coincide. We rotate first the frame #1 with the respect to frame #0, then the frame #2 with respect to frame #1.
The order of the rotations: R = Rx_alpha * Ry_beta * Rz_gamma, so first about x, then y', then z'', which are also known as the Cartesian angles.
If R1 stands for the 1st and R2 for the 2nd rotation, we are looking for the angles of the 2nd frame with respect to initial frame (#0) after both of the rotations. This can be done by decomposing the rotation matrix R (where:R = R1*R2 ). There are many literature available, how it can be done by Euler- and RPY-angles, but I don't find any, how to solve this problem in case of Cartesian angles.
I have a matlab function which works only by simple rotations. If all the angles have values different than 0 (example below), then the result becomes really unstable.
Orientation of the 1st frame with respect to the frame #0:
alpha1 = 30*pi/180;
beta1 = 10*pi/180;
gamma1 = 0*pi/180;
Orientation of the 2nd frame with respect to the frame #1
alpha2 = 10*pi/180;
beta2 = 10*pi/180;
gamma2 = 0*pi/180;
The matlab function I was using for solving the problem:
function [q] = cartesian_angles(R)
beta = asin(R(1,3));
*% Catching the numerical singularty*
if abs(abs(beta)-pi/2) > eps;
*% singulartiy of acos*
gamma1 = acos(R(1,1) / cos(beta));
gamma2 = asin(-R(1,2) / cos(beta));
if gamma2<0
gamma=2*pi-gamma1;
else
gamma=gamma1;
end
alpha1 = acos(R(3,3) / cos(beta));
alpha2 = asin(-R(2,3) / cos(beta));
if alpha2<0
alpha = 2*pi-alpha1;
else
alpha = alpha1;
end
else
fprintf('beta=pi/2 \n')
gamma = 0;
alpha = 0;
beta = 0;
end;
alpha = alpha*180/pi;
beta = beta*180/pi;
gamma = gamma*180/pi;
q = [alpha; beta; gamma];
Thank you for any help! If you have some questions don't hesitate to ask!
Marci
First, I'm going to assume you are passing into your function a well conditioned, right-handed rotation matrix. I'm going to use the same rotation sequence as you listed above, X Y' Z''
If you know the symbolic construction of the rotation matrix you are trying to extract angles from, the math is pretty straight forward. Below is an example of matlab code to determine the construction of the rotation matrix of order X-Y'-Z''
a = sym('a');%x
b = sym('b');%y
g = sym('g');%z
Rx = [1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
Ry = [cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
Rz = [cos(g) -sin(g) 0;sin(g) cos(g) 0;0 0 1];
R = Rz*Ry*Rx
The output looks like this:
R =
[ cos(b)*cos(g), cos(g)*sin(a)*sin(b) - cos(a)*sin(g), sin(a)*sin(g) + cos(a)*cos(g)*sin(b)]
[ cos(b)*sin(g), cos(a)*cos(g) + sin(a)*sin(b)*sin(g), cos(a)*sin(b)*sin(g) - cos(g)*sin(a)]
[ -sin(b), cos(b)*sin(a), cos(a)*cos(b)]
Here's the same result in a nicer looking format:
Now let's go over the math to extract the angles from this matrix. Now would be a good time to become comfortable with the atan2() function.
First solve for the beta angle (by the way, alpha is the rotation about the X axis, beta is the rotation about Y' axis, and gamma is the angle about the Z'' axis):
beta = atan2(-1*R(3,1),sqrt(R(1,1)^2+R(2,1)^2))
Written more formally,
Now that we have solved for the beta angle we can solve more simply for the other two angles:
alpha = atan2(R(3,2)/cos(beta),R(3,3)/cos(beta))
gamma = atan2(R(2,1)/cos(beta),R(1,1)/cos(beta))
Simplified and in a nicer format,
The above method is a pretty robust way of getting the Euler angles out of your rotation matrix. The atan2 function really makes it much simpler.
Finally I will answer how to solve for the rotation angles after a series of rotations. First consider the following notation. A vector or rotation matrix will be notated in the following way:
Here "U" represents the universal frame, or global coordinate system. "Fn" represents the nth local coordinate system that is different from U. R means rotation matrix (this notation could also be used for homogeneous transformations). The left side superscript will always represent the parent frame of reference of the rotation matrix or vector. The left side subscript indicates the child frame of reference. For example, if I have a vector in F1 and I want to know what it is equivalently in the universal frame of reference I would perform the following operation:
To get the vector resolved in the universal frame I simply multiplied it by the rotation matrix that transforms things from F1 to U. Notice how the subscripts are "cancelled" out by the superscript of the next item in the equation. This is a clever notation to help someone from getting things mixed up. If you recall, a special property of well conditioned rotation matrices is that the inverse matrix is the transpose of the matrix, which is will also be the inverse transformation like this:
Now that the notation details are out of the way, we can start to consider solving for complicated series of rotations. Lets say I have "n" number of coordinate frames (another way of saying "n" distinct rotations). To figure out a vector in the "nth" frame in the universal frame I would do the following:
To determine the Cardan/Euler angles that result from "n" rotations, you already know how to decompose the matrix to get the correct angles (also known as inverse kinematics in some fields), you simply need the correct matrix. In this example I am interested in the rotation matrix that takes things in the "nth" coordinate frame and resolves them into the Universal frame U:
There is it, I combined all the rotations into the one of interest simply by multiplying in the correct order. This example was easy. More complicated cases come when someone wants to find the reference frame of one rigid body resolved in the frame of another and the only thing the two rigid bodies have in common is their measurement in a universal frame.
I want to also note that this notation and method can also be used with homogeneous transformations but with some key differences. The inverse of a rotation matrix is its transpose, this is not true for homogeneous transformations.
Thank you for you answer willpower2727, your answer was really helpful!
But I would like to mention, that the code you have shown is useful to decompose rotational matrices, which are built in the following way:
R = Rz*Ry*Rx
What I'm looking for:
R = Rx*Ry*Rz
Which results into the following rotational matrix:
However, it's not a problem, as following the method how you calculate the angles alpha, beta and gamma, it was easy to modify the code so it decomposes the matrix shown above.
The angles:
beta = atan2( R(1,3), sqrt(R(1,1)^2+(-R(1,2))^2) )
alpha = atan2( -(R(2,3)/cos(beta)),R(3,3)/cos(beta) )
gamma = atan2( -(R(1,2)/cos(beta)),R(1,1)/cos(beta) )
One thing is still not clear though. The method is perfectly useful, bunt only if I calculate the angles after one rotation. As there are more rotations linked after each other, the results are false. However, it's still solvable, I guess, considering the following way: Let's say, we have two rotations linked after each other (R1 and R2). q1 shows the angles of R1, q2 of R2. after decomposing the single matrices. The total angle of rotation of the matrix R=R1*R2can be easily calculated through summing up the rangles before: q=q1+q2
Is there no way, how to calculate the angles of the total rotation, not by summing the partial angles, but decomposing the matrix R=R1*R2?
UPDATE:
Considering the following basic example. The are to rotations linked after each other:
a1 = 10*pi/180
b1 = 20*pi/180
g1 = 40*pi/180
R1 = Rx_a1*Ry_b1_Rz_g1
a2 = 20*pi/180
b2 = 30*pi/180
g2 = 30*pi/180
R2 = Rx_a2*Ry_b2*Rz_g2
Decomposing the individual matrices R1 and R2 results in the rights angles. The problem occures, when I link the rotations after each other and I try to determinate the angles of the last frame in the inertial frame. Theoretically this could be done by decomposing the product of all rotational matrices of the chain of transformations.
R = R1*R2
Decomposing this matrix gives the following false result shown in degrees:
a = 0.5645
b = 54.8024
g = 61.4240
Marci
I'm trying to work with vector data to calculate the angle between two vectors. I'm using atan2 which operates in the x-plane but I want to remove points from the y-plane.
How do I rotate my data so that my y-plane is my x-plane, do my analysis, and then rotate it back? I have the calculations already for my analysis, just don't know how to rotate the vector data while retaining each position.
Assuming that your data points are in a N x 3 matrix where N is the total number of points that you have, simply apply a rotation matrix to each point.
You can rotate a point by performing a very simple matrix multiplication. Given a point as a 3 element column vector X, the output point X' is simply:
X' = R*X
R is a rotation matrix. There are three rotation matrices depending on which axis you want to rotate with respect with. In your case, you want to rotate 90 degrees clockwise about the x-axis. The general forms for rotating about each axis in a counter-clockwise direction are given here:
Source: Wikipedia
In your case, you want the first matrix. Because you want to rotate 90 degrees clockwise, you would specify the angle to be -90 degrees. As such, construct your rotation matrix for the x-axis as so:
R = [1 0 0; 0 cosd(-90) -sind(-90); 0 sind(-90) cosd(-90)];
Also, cos(90) = 0 and sin(-90) = -1 assuming degrees, so this really simplifies to:
R =
1 0 0
0 0 1
0 -1 0
To undo the rotation you made, simply apply a 90 degree counter-clockwise rotation, and so substitute 90 degrees into the above expression. Doing this gives us:
R =
1 0 0
0 0 -1
0 1 0
However, your points are in a N x 3 matrix. We want to represent each column as a point, and so what you need to do is transpose your matrix, then do the multiplication, and then transpose back (if desired). Assuming your points are stored in X, just do this:
R = [1 0 0; 0 cosd(-90) -sind(-90); 0 sind(-90) cosd(-90)];
Xrotate = (R*X.').';
Xrotate will contain a N x 3 matrix that contains your rotated points. Each column will be a rotated point given the source point in the original X matrix. It should be noted that each row of X gets rotated and placed into a column in Xrotate and we'll need to transpose this result to get it back into the shape of X. Do your processing, and when you're ready you can rotate your points back. Assuming that Xrotate is N x 3, simply do this:
R2 = [1 0 0; 0 cosd(90); -sind(90); 0 sind(90); cosd(90)];
Xout = (R2*Xrotate.').';
Xout contains the rotation and is re-transposed back so that each row is a point and each column is a dimension.
Anyway, the best way to compute the angle between 2 vectors is using the dot product: u · v = norm(u,2) * norm(v,2) * cos(angle)
You can compute it really fast this way u·v are 2 sums and 3 multiplications, and the norm function has 3 multiplications, 2 sums and a square root, and 2 divisions previous to acos(). Count them as 20 operations to get the angle, much better than multiplying matrices to vectors, changing the coordinates system, aplying the operations you were trying...
Quoting from the manual:
Syntax
rotate(h,direction,alpha)
rotate(...,origin)
Description
The rotate function rotates a graphics object in three-dimensional
space.
rotate(h,direction,alpha) rotates the graphics object h by alpha
degrees. direction is a two- or three-element vector that describes
the axis of rotation in conjunction with the origin of the axis of
rotation. The default origin of the axis of rotation is the center of
the plot box. This point is not necessarily the origin of the axes.
Positive alpha is defined as the righthand-rule angle about the
direction vector as it extends from the origin of rotation.
If h is an array of handles, all objects must be children of the
same axes.
rotate(...,origin) specifies the origin of the axis of rotation as a
three-element vector [x0,y0,z0].
I have a camera and its K matrix (calibration matrix) also I have image of plane, I know the real points of the 4 corners and thier correspondence pixel. I know how to compute the H matrix if z=0 (H is homography matrix between Image and the real plane).
And Now I try to get the real point of the plane (3D point) with the rotation matrix and the transltion vector
I follow this paper :Calibrating an Overhead Video Camera by Raul Rojas in section 3 - 3.3.
My code is:
ImagePointsScreen=[16,8,1;505,55,1;505,248,1;44,301,1;];
screenImage=imread( 'screen.jpg');
RealPointsMirror=[0,0,1;9,0,1;9,6,1;0,6,1]; %Mirror
RealPointsScreen=[0,0,1;47.5,0,1;47.5,20,1;0,20,1];%Screen
imagesc(screenImage);
hold on
for i=1:4
drawBubble(ImagePointsScreen(i,1),ImagePointsScreen(i,2),1,'g',int2str(i),'r')
end
Points3DScreen=Get3DpointSurface(RealPointsScreen,ImagePointsScreen,'Screen');
figure
hold on
plot3(Points3DScreen(:,1),Points3DScreen(:,2),Points3DScreen(:,3));
for i=1:4
drawBubble(Points3DScreen(i,1),Points3DScreen(i,2),1,'g',int2str(i),'r')
end
function [ Points3D ] = Get3DpointSurface( RealPoints,ImagePoints,name)
M=zeros(8,9);
for i=1:4
M((i*2)-1,1:3)=-RealPoints(i,:);
M((i*2)-1,7:9)=RealPoints(i,:)*ImagePoints(i,1);
M(i*2,4:6)=-RealPoints(i,:);
M(i*2,7:9)=RealPoints(i,:)*ImagePoints(i,2);
end
[U S V] = svd(M);
X = V(:,end);
H(1,:)=X(1:3,1)';
H(2,:)=X(4:6,1)';
H(3,:)=X(7:9,1)';
K=[680.561906875074,0,360.536967117290;0,682.250270165388,249.568615725655;0,0,1;];
newRO=pinv(K)*H;
h1=newRO(1:3,1);
h2=newRO(1:3,2);
scaleFactor=(norm(h1)+norm(h2))/2;
newRO=newRO./scaleFactor;
r1=newRO(1:3,1);
r2=newRO(1:3,2);
r3=cross(r1,r2);
r3=r3/norm(r3);
R=[r1,r2,r3];
RInv=pinv(R);
O=-RInv*newRO(1:3,3);
M=K*[R,-R*O];
for i=1:4
res=pinv(M)* [ImagePoints(i,1),ImagePoints(i,2),1]';
res=res';
res=res*(1/res(1,4));
Points3D(i,:)=res';
end
Points3D(i+1,:)=Points3D(1,:); %just add the first point to the end of the array for draw square
end
My result is :
Now I have two problem :
1.The point 1 is at (0,0,0) and this is not the real location
2.the points are upside down
What I am doing worng?
A homography is normally the transform of a plane in two positions/rotations.
The position in camera coordinates of a plane is normally called pose or extrinsic parameters
opencv has a solvePnP() function which uses Ransac to estimate the position of a known plane.
ps. Sorry don't know the equivalent matlab but Bouguet has a matlab version of the openCV 3D functions on his site
I found the answer in the paper: Calibrating an Overhead Video Camera by Raul Rojas in section 3 - 3.3.
for the start: H=K^-1*H
Given four points in the image and their known coordinates in the world, the
matrix H can be recovered, up to a scaling factor . We know that the first
two columns of the rotation matrix R must be the first two columns of the
transformation matrix. Let us denote by h1, h2, and h3 the three columns of
the matrix H.Due to the scaling factor we then have that
xr1 = h1
and
xr2 = h2
Since |r1| = 1, then x= |h1|/|r1| = |h1| and x = |h2|/|r2| = |h2|. We can thus
compute the factor and eliminate it from the recovered matrix H. We just set
H'= H/x
In this way we recover the first two columns of the rotation matrix R.
The third column of R can be found remembering that any column in a rotation
matrix is the cross product of the other two columns (times the appropriate
plus or minus sign). In particular
r3 = r1 × r2
Therefore, we can recover from H the rotation matrix R. We can also recover
the translation vector (the position of the camera in field coordinates). Just
remember that
h'3 = −R^t
Therefore the position vector of the camera pin-hole t is given by
t = −R^-1 h3