Related
I would like to change a 4x4 matrix from a right handed system where:
x is left and right, y is front and back and z is up and down
to a left-handed system where:
x is left and right, z is front and back and y is up and down.
For a vector it's easy, just swap the y and z values, but how do you do it for a matrix?
Let me try to explain it a little better.
I need to export a model from Blender, in which the z axis faces up, into OpenGL, where the y axis faces up.
For every coordinate (x, y, z) it's simple; just swap the y and z values: (x, z, y).
Because I have swapped the all the y and z values, any matrix that I use also needs to be flipped so that it has the same effect.
After a lot of searching I've eventually found a solution at gamedev:
If your matrix looks like this:
{ rx, ry, rz, 0 }
{ ux, uy, uz, 0 }
{ lx, ly, lz, 0 }
{ px, py, pz, 1 }
To change it from left to right or right to left, flip it like this:
{ rx, rz, ry, 0 }
{ lx, lz, ly, 0 }
{ ux, uz, uy, 0 }
{ px, pz, py, 1 }
I think I understand your problem because I am currently facing a similar one.
You start with a world matrix which transforms a vector in a space where Z is up (e.g. a world matrix).
Now you have a space where Y is up and you want to know what to do with your old matrix.
Try this:
There is a given world matrix
Matrix world = ... //space where Z is up
This Matrix changes the Y and Z components of a Vector
Matrix mToggle_YZ = new Matrix(
{1, 0, 0, 0}
{0, 0, 1, 0}
{0, 1, 0, 0}
{0, 0, 0, 1})
You are searching for this:
//same world transformation in a space where Y is up
Matrix world2 = mToggle_YZ * world * mToggle_YZ;
The result is the same matrix cmann posted below. But I think this is more understandable as it combines the following calculation:
1) Switch Y and Z
2) Do the old transformation
3) Switch back Z and Y
It is often the case that you want to change a matrix from one set of forward/right/up conventions to another set of forward/right/up conventions. For example, ROS uses z-up, and Unreal uses y-up. The process works whether or not you need to do a handedness-flip.
Note that the phrase "switch from right-handed to left-handed" is ambiguous. There are many left-handed forward/right/up conventions. For example: forward=z, right=x, up=y; and forward=x, right=y, up=z. You should really think of it as "how do I convert ROS' notion of forward/right/up to Unreal's notion of forward/right/up".
So, it's a straightforward job to create a matrix that converts between conventions. Let's assume we've done that and we now have
mat4x4 unrealFromRos = /* construct this by hand */;
mat4x4 rosFromUnreal = unrealFromRos.inverse();
Let's say the OP has a matrix that comes from ROS, and she wants to use it in Unreal. Her original matrix takes a ROS-style vector, does some stuff to it, and emits a ROS-style vector. She needs a matrix that takes an Unreal-style vector, does the same stuff, and emits an Unreal-style vector. That looks like this:
mat4x4 turnLeft10Degrees_ROS = ...;
mat4x4 turnLeft10Degrees_Unreal = unrealFromRos * turnLeft10Degrees_ROS * rosFromUnreal;
It should be pretty clear why this works. You take a Unreal vector, convert it to ROS-style, and now you can use the ROS-style matrix on it. That gives you a ROS vector, which you convert back to Unreal style.
Gerrit's answer is not quite fully general, because in the general case, rosFromUnreal != unrealFromRos. It's true if you're just inverting a single axis, but not true if you're doing something like converting X→Y, Y→Z, Z→X. I've found that it's less error-prone to always use a matrix and its inverse to do these convention switches, rather than to try to write special functions that flip just the right members.
This kind of matrix operation M * X * inverse(M) comes up a lot. You can think of it as a "change of basis" operation; to learn more about it, see https://en.wikipedia.org/wiki/Matrix_similarity.
I have been working on converting the Unity SteamVR_Utils.RigidTransform to ROS geometry_msgs/Pose and needed to convert Unity left handed coordinate system to the ROS right handed coordinate system.
This was the code I ended up writing to convert coordinate systems.
var device = SteamVR_Controller.Input(index);
// Modify the unity controller to be in the same coordinate system as ROS.
Vector3 ros_position = new Vector3(
device.transform.pos.z,
-1 * device.transform.pos.x,
device.transform.pos.y);
Quaternion ros_orientation = new Quaternion(
-1 * device.transform.rot.z,
device.transform.rot.x,
-1 * device.transform.rot.y,
device.transform.rot.w);
Originally I tried using the matrix example from #bleater, but I couldn't seem to get it to work. Would love to know if I made a mistake somewhere.
HmdMatrix44_t m = device.transform.ToHmdMatrix44();
HmdMatrix44_t m2 = new HmdMatrix44_t();
m2.m = new float[16];
// left -> right
m2.m[0] = m.m[0]; m2.m[1] = m.m[2]; m2.m[2] = m.m[1]; m2.m[3] = m.m[3];
m2.m[4] = m.m[8]; m2.m[5] = m.m[10]; m2.m[6] = m.m[9]; m2.m[7] = m.m[7];
m2.m[8] = m.m[4]; m2.m[9] = m.m[6]; m2.m[10] = m.m[5]; m2.m[11] = m.m[11];
m2.m[12] = m.m[12]; m2.m[13] = m.m[14]; m2.m[14] = m.m[13]; m2.m[15] = m.m[15];
SteamVR_Utils.RigidTransform rt = new SteamVR_Utils.RigidTransform(m2);
Vector3 ros_position = new Vector3(
rt.pos.x,
rt.pos.y,
rt.pos.z);
Quaternion ros_orientation = new Quaternion(
rt.rot.x,
rt.rot.y,
rt.rot.z,
rt.rot.w);
After 12 years, the question is still misleading because of the lack of description of axis direction.
What question asked for should probably be how to convert to .
The answer by #cmann is correct for the above question and #Gerrit explains the reason. And I will explain how to graphically get that conversion on the transform matrix.
We should be clear that orthogonal matrix contains both rotation matrix and point reflection(only point reflection will change the coordinate system between left-handed and right-handed). Thus they can be expressed as a 4x4 matrix and obey to transform matrix multiplying order. "The matrix of a composite transformation is obtained by multiplying the matrices of individual transformations."
to contains both rotation matrix and point reflection. But we can get the composite transformation graphically.
According to above image, after transformation, in RhC(Right-handedCorrdinate) will be in LfC as below
where is a transform bring points expressed in above RhC to points expressed in LhC.
Now We are able to convert () to () accroding to transform matrix multiplying order as below image.
The result is the same as #cmann's.
Result:
It depends if you transform your points by multiplying the matrix from the left or from the right.
If you multiply from the left (e.g: Ax = x', where A is a matrix and x' the transformed point), you just need to swap the second and third column.
If you multiply from the right (e.g: xA = x'), you need to swap the second and third row.
If your points are column vectors then you're in the first scenario.
Change sin factor to -sin for swaping coordinate spaces between right and left handed
Since this seems like a homework answer; i'll give you a start at a hint: What can you do to make the determinant of the matrix negative?
Further (better hint): Since you already know how to do that transformation with individual vectors, don't you think you'd be able to do it with the basis vectors that span the transformation the matrix represents? (Remember that a matrix can be viewed as a linear transormation performed on a tuple of unit vectors)
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 reading the Billboarding section in the book Real-Time Rendering and the author explains that in order to rotate a quadrilateral (i.e the billboard) to a certain orientation, the rotation matrix will be
M = (r, u, n)
Where r, u, n are the calculated (normalized) direction vectors.
From the book:
I've learned that in order to rotate stuff one must use a matrix that includes lots of dirty sin() and cos() calculations. How come this M matrix uses plain direction vectors?
Sine and cosine are used only when you want to convert from an angle representation to a vector representation. But let's first analyze what makes a matrix a rotation matrix.
Rotation matrices are ortho-normal and have determinant +1. That means that their column vectors are of unit length and that they are perpendicular to each other. One nice property of ortho-normal matrices is that you can invert them by transposing them. But that's just a nice feature.
If we have the 2D rotation matrix
M = / cos a sin a \
\ -sin a cos a /
, we see that this is the case. The first column vector is (cos a, -sin a). From the Pythagorean theorem, we get that this vector has unit length. Furthermore, it is perpendicular to the second column vector (their dot product is zero).
So far, so good. This matrix is a rotation matrix. But can we interpret the column vectors? Indeed, we can. The first column vector is the image of the vector (1, 0) (i.e. the right vector). The second column vector is the image of the vector (0, 1) (i.e. the up vector).
So you see that using sine and cosine are just another way to calculate the direction vectors. Doing so automatically ensures that they have unit length and that they are orthogonal to each other. But this is just one way. You can also calculate the direction vectors using the cross product or any other scheme. The critical point is that the rotation matrix properties are fulfilled.
You need those dirty trigonometric functions if your transformation is given with angles. However, if instead you know the image of the Cartesian unit vectors, you can construct the matrix easily.
If the image of [1; 0; 0] is r, [0; 1; 0] is u and [0; 0; 1] is n, then the effect of the matrix will be
M * [1; 0; 0] == 1*r + 0*u +0*n == r
M * [0; 1; 0] == 0*r + 1*u +0*n == u
M * [0; 0; 1] == 0*r + 0*u +1*n == n
which is exactly the transformation you need, if your matrix is M=[r u n].
Note that this will in general give you an affine transformation, it will only be a rigid rotation if your vectors are orthonormal.
I have an optimization problem to solve in order to filter an image.
I created a Linear Equation of the problem which deals with Sparse Matrices.
At first I will show the problem.
First, the Laplacian (Adjacency) matrix of the problem:
The matrix Dx / Dy is the forward difference operator -> Hence its transpose is the backward difference operator.
The matrix Ax / Ay is diagonal matrix with weights which are function of the gradient of the image (Point wise, namely the value depends only on the gradient on that pixel by itself).
The weights are:
Where Ix(i) is the horizontal gradient of the input image at the i-th pixel (When you vectorize the input image).
Assuming input Image G -> g = vec(G) = G(:).
I want to find and image U -> u = vec(U) = U(:) s.t.:
My questions are:
How can I build the matrices Dx / Dy / Ax / Ay effectively (They are all sparse)?
By setting M = (I + \lambda * {L}_{g}), Is there an optimized way to create M directly?
What would be the best way to solve this linear problem in MATLAB? Is there a way to by pass memory limitations (Namely, dealing with large images and still be able to solve it)?
Is there an Open Source library to solve it under limited memory resources? Any library with MATLAB API?
Thank You.
Given your comments, let's answer each question in a synopsis and go from there:
I will answer that question below using sparse and other related functions
Using (1), we can definitely build M in an optimized way.
Simply put, the \ operator is the best thing to use when solving an inverse. MathWorks have spent so much time trying to optimize it, and it pretty much uses LAPACK and BLAS under the hood, that you would be insane not to use it. The only time you wouldn't be able to use it is answered in (4).
There are some MATLAB scripts that can handle solving the matrix iteratively, like the Successive Overrelaxation technique, but you should only use those if your run out of memory (i.e. if \ doesn't give you an answer). With the sparse representation of the matrices, this shouldn't (hopefully) happen, so let's avoid using those functions for now.
Going back to your question, we can produce a sparse representation of L_g very nicely. Given the definition of Dx and Dy, we can use the sparse version of the eye command called speye. Therefore, Dx and Dy can be calculated by Dx = diff(speye(size(inputImage))); As an example, this is what would be produced if you tried doing this on a 7 x 5 image.
>> diff(speye(7,5))
ans =
(1,1) -1
(1,2) 1
(2,2) -1
(2,3) 1
(3,3) -1
(3,4) 1
(4,4) -1
(4,5) 1
(5,5) -1
As you can see, we are referencing only non-zero entries. Row 1, column 1 has a coefficient of -1, row 1, column 2 has a coefficient of 1 and so on. As for your Ax and Ay, that's also very easy to do. We have a diagonal matrix and we can set each of the entries manually. All we would do is specify a set of row indices, column indices, and what the values are at each point. Therefore, we can do that by:
inputImage = im2double(inputImage); %//Important
rows = 1 : numel(inputImage); %// Assuming a 2D matrix
cols = rows; % // Row and column indices are the same
valuesDx = exp(-(gradX(rows).^2 / 2*sigma*sigma ));
valuesDy = exp(-(gradY(rows).^2 / 2*sigma*sigma ));
The reason for the first call is because we want to make sure that the pixels are in double precision, as finding the inverse in MATLAB requires that you do this. It also ensures we don't overflow the type as we are normalizing the intensities between 0 and 1. You may have to adjust your standard deviation to reflect this. Now we just need to construct our Ax and Ay matrices, and let's put it together with Dx and Dy:
numberElements = numel(inputImage);
Ax = sparse(rows, cols, valuesDx, numberElements, numberElements);
Ay = sparse(rows, cols, valuesDy, numberElements, numberElements);
identity = speye(numberElements, numberElements);
Dx = diff(identity);
Dy = Dx.'; %// Transpose
The reason why I'm transposing Dx to get Dy is because the difference operator in the vertical direction should simply be the transpose (makes sense to me). These should all be sparse representations of each of the matrices you want. Matrix operations can also be performed on sparse matrices, including multiplication and the inverse. As such:
Lg = Dx.' * Ax * Dx + Dy.' * Ay * Dy;
You can now solve for u via:
u = (identity + lambda*Lg) \ g;
This assumes that g is structured with your pixels in your image in column-major format. The way I sampled the pixels to build Ax and Ay naturally follows this. As such, do g = inputImage(:);, assuming that we have converted to double and normalized between 0 and 1.
When you finally solve for u, you can reshape it back to an image by doing:
u = reshape(u, size(inputImage, 1), size(inputImage, 2));
u may also be sparse, so if you want the original image back, cast it using full():
u = full(u);
Hope this helps!
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