I would like to know how to find the rotation matrix for a set of features in a frame.
I will be more specific. I have 2 frames with 20 features, let's say frame 1 and frame 2. I could estimate the location of the features in both frames. For example let say a certain frame 1 feature at location (x, y) and I know exactly where it is so let's say (x',y').
My question is that the features are moved and probably rotated so I wanna know how to compute the rotation matrix, I know the rotation matrix for 2D:
But I don't know how to compute the angle, and how to do that? I tried a function in OpenCV which is cv2DRotationMatrix(); but the problem which as I mentioned above I don't know how to compute the angle for the rotation matrix and another problem which it gives 2*3 matrix, so it won't work out cause if I will take this 20*2 matrix, (20 is the number of features and 2 are the location in (x,y)) and multiply it by the matrix by 2*3 which is the results from the function then I will get 20*3 matrix which it doesn't seem to be realistic cause I'm working with 2D.
So what should I do? To be more specific again, show me how to compute the angle to use it in the matrix?
I'm not sure I've understood your question, but if you want to find out the angle of rotation resulting from an arbitrary transform...
A simple hack is to transform the points [0 0] and [1 0] and getting the angle of the ray from the first transformed point to the second.
o = M • [0 0]
x = M • [1 0]
d = x - o
θ = atan2(d.y, d.x)
This doesn't consider skew and other non-orthogonal transforms, for which the notion of "angle" is vague.
Have a look at this function:
cvGetAffineTransform
You give it three points in the first frame and three in the second. And it computes the affine transformation matrix (translation + rotation)
If you want, you could also try
cvGetPerspectiveTransform
With that, you can get translation+rotation+skew+lot of others.
Related
I'm currently implementing an algorithm for 3D pointcloud filtering following a scientific paper.
I run in some problems when computing the rotation matrix for specific values. The goal is to rotate points into the coordinatesystem which is defined by the direction of the normal vector ( Z Axis). Since the following query is rotationally symmetric in X,Y axis, the orientation of these axis does not matter.
R is defined as follows: Rotationmatrix
[1 1 -(nx+ny)/nz]
R = [ (row1 x row3)' ]
[nx ny nz ]
n is normalized. The problem occures when n_z becomes really small or zero. Therefore i considered to normalize row 1 before computing the crossproduct for row 2.
Nevertheless the determinant becomes -1. Will the rotationmatrix sill lead to correct results? R is orthogonal but det|R| not +1
thanks for any suggestions
You always get that
det(a, a×b, b) = - det( a, b, a×b)
= - dot(a×b, a×b)
is always negative. Thus you need to change the second row by negating it or by re-arranging the overall order of the rows.
Are you interested in rotating points around arbitrary axis? If yes, maybe quaternions is good solution.
You can check this if you want to transform a quaternion to matrix before you actually use it.
I'm doing something where I have a plane in a coord sys A with a set of points already on it. I also have a normal vector in space N. How can I rotate the points on coord sys A so that the underlying plane will have the same normal direction as N?
Wondering if any one has a good idea on how to do this. Thanks
If you have, or can easily compute, the normal vector to the plane that your points are currently in, I think the easiest way to do this will be to rotate around the axis common to the two planes. Here's how I'd go about it:
Let M be the vector normal to your current plane, and N be the vector normal to the plane you want to rotate into. If M == N you can stop now and leave the original points unchanged.
Calculate the rotation angle as
costheta = dot(M,N)/(norm(M)*norm(N))
Calculate the rotation axis as
axis = unitcross(M, N)
where unitcross is a function that performs the cross product and normalizes it to a unit vector, i.e. unitcross(a, b) = cross(a, b) / norm(cross(a, b)). As user1318499 pointed out in a comment, this step can cause an error if M == N, unless your implementation of unitcross returns (0,0,0) when a == b.
Compute the rotation matrix from the axis and angle as
c = costheta
s = sqrt(1-c*c)
C = 1-c
rmat = matrix([ x*x*C+c x*y*C-z*s x*z*C+y*s ],
[ y*x*C+z*s y*y*C+c y*z*C-x*s ]
[ z*x*C-y*s z*y*C+x*s z*z*C+c ])
where x, y, and z are the components of axis. This formula is described on Wikipedia.
For each point, compute its corresponding point on the new plane as
newpoint = dot(rmat, point)
where the function dot performs matrix multiplication.
This is not unique, of course; as mentioned in peterk's answer, there are an infinite number of possible rotations you could make that would transform the plane normal to M into the plane normal to N. This corresponds to the fact that, after you take the steps described above, you can then rotate the plane around N, and your points will be in different places while staying in the same plane. (In other words, each rotation you can make that satisfies your conditions corresponds to doing the procedure described above followed by another rotation around N.) But if you don't care where in the plane your points wind up, I think this rotation around the common axis is the simplest way to just get the points into the plane you want them in.
If you don't have M, but you do have the coordinates of the points in your starting plane relative to an origin in that plane, you can compute the starting normal vector from two points' positions x1 and x2 as
M = cross(x1, x2)
(you can also use unitcross here but it doesn't make any difference). If you have the points' coordinates relative to an origin that is not in the plane, you can still do it, but you'll need three points' positions:
M = cross(x3-x1, x3-x2)
A single vector (your normal - N) will not be enough. You will need another two vectors for the other two dimensions. (Imagine that your 3D space could still rotate/spin around the normal vector, and you need another 2 vectors to nail it down). Once you have the normal and another one on the plane, the 3rd one should be easy to find (left- or right-handed depending on your system).
Make sure all three are normalized (length of 1) and put them in a matrix; use that matrix to transform any point in your 3D space (use matrix multiplication). This should give you the new coordinates.
I'm thinking make a unit vector [0,0,1] and use the dot-product along two planes to find the angle of difference, and shift all your points by those angles. This is assuming you want the z-axis to align with the normal vector, else just use [1,0,0] or [0,1,0] for x and y respectively.
As far as I know, Direct3D works with an LH coordinate system right?
So how would I get position and x/y/z axis (local orientation axis) out of a LH 4x4 (world) matrix?
Thanks.
In case you don't know: LH stands for left-handed
If the 4x4 matrix is what I think it is (a homogeneous rigid body transformation matrix, same as an element of SE(3)) then it should be fairly easy to get what you want. Any rigid body transformation can be represented by a 4x4 matrix of the form
g_ab = [ R, p;
0, 1]
in block matrix notation. The ab subscript denotes that the transformation will take the coordinates of a point represented in frame b and will tell you what the coordinates are as represented in frame a. R here is a 3x3 rotation matrix and p is a vector that, when the rotation matrix is unity (no rotation) tells you the coordinates of the origin of b in frame a. Usually, however, a rotation is present, so you have to do as below.
The position of the coordinate system described by the matrix will be given by applying the transformation to the point (0,0,0). This will well you what world coordinates the point is located at. The trick is that, when dealing with SE(3), you have to add a 1 at the end of points and a 0 at the end of vectors, which makes them vectors of length 4 instead of length 3, and hence operable on by the matrix! So, to transform point (0,0,0) in your local coordinate frame to the world frame, you'd right multiply your matrix (let's call it g_SA) by the vector (0,0,0,1). To get the world coordinates of a vector (x,y,z) you multiply the matrix by (x,y,z,0). You can think of that as being because vectors are differences of points, so the 1 in the last element goes the away. So, for example, to find the representation of your local x-axis in the world coordinates, you multiply g_SA*(1,0,0,0). To find the y-axis you do g_SA*(0,1,0,0), and so on.
The best place I've seen this discussed (and where I learned it from) is A Mathematical Introduction to Robotic Manipulation by Murray, Li and Sastry and the chapter you are interested in is 2.3.1.
sorry - I should know this but I don't.
I have computed the position of a reference frame (S1) with respect to a base reference frame (S0) through two different processes that give me two different 4x4 affine transformation matrices. I'd like to compute an error between the two but am not sure how to deal with the rotational component. Would love any advice.
thank you!
If R0 and R1 are the two rotation matrices which are supposed to be the same, then R0*R1' should be identity. The magnitude of the rotation vector corresponding to R0*R1' is the rotation (in radians, typically) from identity. Converting rotation matrices to rotation vectors is efficiently done via Rodrigues' formula.
To answer your question with a common use case, Python and OpenCV, the error is
r, _ = cv2.Rodrigues(R0.dot(R1.T))
rotation_error_from_identity = np.linalg.norm(r)
You are looking for the single axis rotation from frame S1 to frame S0 (or vice versa). The axis of the rotation isn't all that important here. You want the rotation angle.
Let R0 and R1 be the upper left 3x3 rotation matrices from your 4x4 matrices S0 and S1. Now compute E=R0*transpose(R1) (or transpose(R0)*R1; it doesn't really matter which.)
Now calculate
d(0) = E(1,2) - E(2,1)
d(1) = E(2,0) - E(0,2)
d(2) = E(0,1) - E(1,0)
dmag = sqrt(d(0)*d(0) + d(1)*d(1) + d(2)*d(2))
phi = asin (dmag/2)
I've left out some hairy details (and these details can bite you). In particular, the above is invalid for very large error angles (error > 90 degrees) and is imprecise for large error angles (angle > 45 degrees).
If you have a general-purpose function that extracts the single axis rotation from a matrix, use it. Or if you have a general-purpose function that extracts a quaternion from a matrix, use that. (Single axis rotation and quaternions are very closely related to one another).
I'm experimenting with using axis-angle vectors for rotations in my hobby game engine. This is a 3-component vector along the axis of rotation with a length of the rotation in radians. I like them because:
Unlike quats or rotation matrices, I can actually see the numbers and visualize the rotation in my mind
They're a little less memory than quaternions or matrices.
I can represent values outside the range of -Pi to Pi (This is important if I store an angular velocity)
However, I have a tight loop that updates the rotation of all of my objects (tens of thousands) based on their angular velocity. Currently, the only way I know to combine two rotation axis vectors is to convert them to quaternions, multiply them, and then convert the result back to an axis/angle. Through profiling, I've identified this as a bottleneck. Does anyone know a more straightforward approach?
You representation is equivalent to quaternion rotation, provided your rotation vectors are unit length. If you don't want to use some canned quaternion data structure you should simply ensure your rotation vectors are of unit length, and then work out the equivalent quaternion multiplications / reciprocal computation to determine the aggregate rotation. You might be able to reduce the number of multiplications or additions.
If your angle is the only thing that is changing (i.e. the axis of rotation is constant), then you can simply use a linear scaling of the angle, and, if you'd like, mod it to be in the range [0, 2π). So, if you have a rotation rate of α raidans per second, starting from an initial angle of θ0 at time t0, then the final rotation angle at time t is given by:
θ(t) = θ0+α(t-t0) mod 2π
You then just apply that rotation to your collection of vectors.
If none of this improves your performance, you should consider using a canned quaternion library as such things are already optimized for the kinds of application you're disucssing.
You can keep them as angle axis values.
Build a cross-product (anti-symmetric) matrix using the angle axis values (x,y,z) and weight the elements of this matrix by multiplying them by the angle value. Now sum up all of these cross-product matrices (one for each angle axis value) and find the final rotation matrix by using the matrix exponential.
If matrix A represents this cross-product matrix (built from Angle Axis value) then,
exp(A) is equivalent to the rotation matrix R (i.e., equivalent to your quaternion in matrix form).
Therefore,
exp (A1 + A2) = R1 * R2
probably a more expensive calucation in the end...
You should use unit quaternions rather than scaled vectors to represent your rotations. It can be shown (not by me) that any representation of rotations using three parameters will run into problems (i.e. is singular) at some point. In your case it occurs where your vector has a length of 0 (i.e. the identity) and at lengths of 2pi, 4pi, etc. In these cases the representation becomes singular. Unit quaternions and rotation matrices do not have this problem.
From your description, it sounds like you are updating your rotation state as a result of numerical integration. In this case you can update your rotation state by converting your rotational rate (\omega) to a quaternion rate (q_dot). If we represent your quaternion as q = [q0 q1 q2 q3] where q0 is the scalar part then:
q_dot = E*\omega
where
[ -q1 -q2 -q3 ]
E = [ q0 -q3 q2 ]
[ q3 q0 -q1 ]
[ -q2 q1 q0 ]
Then your update becomes
q(k+1) = q(k) + q_dot*dt
for simple integration. You could choose a different integrator if you choose.
Old question, but another example of stack overflow answering questions the OP wasn't asking. OP already listed out his reasoning for not using quaternions to represent velocity. I was in the same boat.
That said, the way you combine two angular velocities, with each represented by a vector, which represents the axis of rotation with its magnitude representing the amount of rotation.
Just add them together. Component-by-component. Hope that helps some other soul out there.