I have 3 point clouds (cloud 0, cloud 1 and cloud 2), obtained in 3 different positions using a Terrestrial Laser Scanner. These clouds overlap between them, which means that there is a rigid 3D body transformation, T, which correctly registers one cloud on another. I have two of these transformations, T10, which moves cloud 1 to cloud 0; and T20, which moves cloud 2 to cloud 0 (cloud 0 was chosen as the global reference). The question is, how do I find the transformation that overlaps cloud 2 with cloud 1? I already found the rotation, but I can't find the translation vector. It's possible?
The rotation I found by multiplying the transformation T20 by the inverse of T10, because T10^(-1) = T01, therefore, T20*T01 = T21. When I apply this transformation to cloud 2 it rotates cloud 2 to cloud 1 correctly (both are in the same direction), but there is a shift between them, I don't understand why.
These transformations are just homogeneous matrices T (4x4) which are simply the junction of a rotation matrix R (3x3), and a translation vector t (3x1), right? Rotations can be composed. The fact that I found the rotation from cloud 2 to cloud 1 shows this. But why does this shift appear in translation?
In fact, I have several clouds, to register a cloud far from the origin, I need to accumulate several transformations through multiplications (for example: T50 = T54 * T43 * T32 * T21 * T10), the more I multiply the greater the difference in translation.
I would like to say that although the multiplication accumulates errors, they are very small, since the registration was done manually and refined by ICP. In fact, applying any of the transformations in pairs results in an almost perfect overlap, but accumulating them causes a huge deviation in translation. The rotation is so good that loop closure practically results in the identity matrix.
Are you neglecting the effect of rotation on translation?
If you have a transformation T with rotation R and translation v and another S with rotation Q and translation v, then the effect of applying T then S to a point x is to get y where
y = Q*(R*x+v) + u = Q*R + Q*v + u
That is the combined transformation has rotation
P = Q*R
and translation
w = Q*v + u
It follows from this that the transformation inverse to T has rotation
inv(R)
and translation
- inv(R)*v
We can, and it is common to, represent such transformations by 4x4 matrices, in such a way that composing and applying transformations reduces to matrix multiplication. Note, though, that this comes at some cost in efficiency.
S and T above would be represented by 4x4 matrices M and N
M = ( Q u)
= ( 0 1)
N = ( R v)
( 0 1)
then the representative L of the combined transformation T then S is
L = M*N
To apply S to a point x we compute
M*(x)
(1)
and the first three components of the result are the components of the transformed point.
Related
Say I have 2 3D vectors, each describing a direction in 3D space (or a rotation, but I'm not sure if that terminology is correct). How would I calculate the difference between the two vectors as an Euler angle? That is, if I applied the angle to the first vector, it would rotate to equal the other? I understand how Euler angles have issues and are implementation-dependant, but I don't know what the implications of this are on a question such as mine.
To clarify a little, when I say "3D vectors", I'm picturing the "translation" gizmo you get in most 3D modelling packages or in Unity (which is what I'm using).
EDIT: Actually I just reviewed the "vectors" that I'm using, and what I said is not quite correct. I actually have 6 vectors, 3 for each rotation. Each vector is a position in 3D space offset from the centre of rotation. This probably makes an already-difficult question near-impossible, right?
ADDITIONAL INFORMATION: Ok, so I've worked out what I actually want to ask (because this question is really badly done), and it applies more to Unity, so I've asked a more Unity-specific question over on Unity Answers.
I'm don't normally understand mathematical formulae that are posted online, so C++-styled pseudo-code would be by far the most helpful for me.
Any help would be much appreciated, and if my question lacks certain information, please just ask for more :)
If you are going to do 3d you need to understand linear algebra and matrix notation.[1] Affine 4x4 matrices are the basis of space transformations in all 3d applications I've ever seen, (Euler angles just give alternate means to describe that matrix). Even unity uses matrices, although to be able to efficiently do the Euler-Lagrange particle motion equation they prefer to have the decomposed form. A matrix encodes a entire space with 4 vectors. This is conceptually easy in this case (not the only use for matrices), the matrix encodes the directions x y z and offset vector w.
The reason matrix notation is useful is: It becomes possible to manipulate the things like normal math symbols. If you remember from school solving x from:
a * x = b
Divide both sides by a and you get
a/a * x = b /a ->
x = b / a
Now if you have 2 spaces with 3 vectors each you essentially have 2 fully formed spaces at origin. Assuming the vectors span a 3D space (in other words dont point all in one plane, its even better if they are orthogonal to each other in which case you can just use transformation functions directly). That means you have 3 spaces. So your problem is given you know 2 spaces. You need to know the space transform form space A -> space B (its customary to give matrices big letters to denote they are more complex). This is mathematically:
A * X = B
Where * is a matrix multiplications and A, X and B are transformation matrices. So then divide by A, but alas there's no matrix division, fortunately there is inverse and division is multiplication by inverse so that's what we do instead. Also one other note rotations are not commutative so we need to specify on which side we multiply so because A is before X we multiply on the left hand side with inverse of A. So we get:
A^-1 * A * X = A^-1 * B
Where ^-1 denotes matrix inverse. This simplifies to :
I * X = A^-1 * B ->
X = A^-1 * B
X is the rotation space. In unity code this looks like:
X = A.inverse * B
Where X, A and B are Matrix4x4 elements The language you use may have other conventions I'm using the java script reference here. You can covert this matrix to a quaternion and from there to Euler angles an example of this can be found here.
How to form A and B from the vectors? Just put the vector for starting space to A's columns 0-2 and destination spaces correspondingly to B columns[2].
[1] Yes its compulsory, its much simpler than it may seem at first. While you can live quite far without them they aren't any harder to use than saying rotate about x axis fro so and so. Also learn quats.
[2] I should check this, but unity seems to use column matrices so it should be right
PS: By the way if you have noisy data and more then 3 vectors per instance then you can use least squares to average the matrix t a 3 by 3 sub matrix.
I am developing a rotate-around-axis algorithm in 3 dimensions. My inputs are
the axis I am revolving around, as a vector from my center point
the center point (obviously)
the angle I wish to rotate around
my current position
I am wondering if there is a way to do this without trigonometry, just with vector operations. Does anyone have a potential solution?
EDIT: Is there a way that I could rotate by pi/4 radians (45 degrees) each time, rather than an inputted angle theta? This might simplify things a bit, I don't know.
Rotations are inherently well-described by and .
It's a handy trick that unit quaternions nicely represent 3-D rotations just as well as (and in some senses, better than) rotation matrices. Converting a rotation by angle about a normal axis where , does require a little bit of trigonometry: .
But from there on it's simple arithmetic.
A quaternion can be directly applied to rotate a vector with , or converted to a rotation matrix .
This is a rotation around the origin, of course. To rotate around an arbitrary point in space, simply translate by to the origin, rotate, then translate by to return.
use matrices: http://en.wikipedia.org/wiki/Rotation_matrix#Rotations_in_three_dimensions
If this is some sort of dumb homework problem, you can use Taylor Series approximation of the sine/consine functions. Whether or not this "counts" as trigonometry is I guess up for debate. You could then use these values in a rotation matrix or quarternion, if you want to use vector operations.
But again, there's no practical reason to do this.
Are there other techniques that don't use trig functions? Possibly, but there are no know efficient, general (i.e. for arbitrary angles) ways to perform rotations without use of trig functions.
However, based on your edit, you can precompute the sin and cos for a collection of angles you're interested in and store them in a lookup table. You need not be constrained in such a circumstance to π/4 increments, but you can do π/256 or π/1024 increments if you want. Also, you don't need two tables, since cos(θ) = sin(θ+π/2).
From there, you can use any of a number of interpolation methods to include simple rounding, linear interpolation or some sort of polynomial interpolation based on your needs.
You would then use either the matrix or quaternion based transformation to compute the rotated vector.
This will be faster than computing the sin and cos for general angles, though will require some additional space, and there will be an accuracy penalty as well. But if it satisfies your needs...
Theres a cheaper way than matrices, I think ive got it to sum count of adders.
The perimetre box of the vector is as good as an angle, if you step in partitions of the box size. (thats only a binary shift if its a power of 2.)
Then that would be a "box rotate" then just use the side report to give you how far along the diagonal you would be then you can split it up into so many gradients, the circle shape.
Id like to see someone proove that u can rotate without matrices or any trig like that too.
Is it possible to rotate without trigonometry? Yes.
Is it useful to rotate without using trigonometry? Probably not.
The first option is a problem-level solution: Change your coordinate system to spherical or cylindrical coordinates.
Since you rotate around an axis cylindrical coordinates of the form (alpha, radius, x3) will work.
Naming your center point O (for origin) and the point to rotate P, you can get the vector between them v=P-O. You also know the normal vector n of your plane of rotation (the vector you rotate around). With this, you can get the components of v that are parallel and orthogonal to n using a vector projection.
You have the freedom to choose how your new coordinate frame is rotated (relative to your original frame), so you can measure angles from the projection of v onto the plane of rotation. You also have the freedom to choose between degree and radians.
From there, you can now rotate to your heart's content using addition and subtraction.
Using dot(.,.) to denote the scalar product it would look something like this in code
v_parallel = dot(v, n) / dot(n, n) * n
radius = norm(v - v_parallel)
x3 = norm(v_parallel)
new_axis = (v - v_parallel) / norm(v - v_parallel)
P_polar = (0, radius, x3)
# P rotated by 90 degrees
P_polar = (pi/2, radius, x3)
# P rotated by -10 degrees
P_polar = (-pi/36, radius, x3)
However, if you want to change back to a standard basis you will have to use trigonometry again. Hence why I said this approach exists, but may not be too useful in practice.
Another approach comes from the cool observation that you can describe any planar rotation using two reflections along two given axis (represented by two vectors). The plane of rotation is the plane that is spun up by the two vectors and the angle of rotation is twice the angle between the two vectors.
You can reflect a vector using the vector projection from above; hence, you can do the entire process without trigonometry if you know the two vectors (let's call them x1 and x2).
tmp = v - 2 * dot(v, x1) / dot(x1, x1) * x1
v_rotated = tmp - 2 * dot(tmp, x2) / dot(x2, x2) * x2
The problem then turns into finding two vectors that are orthogonal to n and have an enclosing angle of alpha/2. How to do this is specific to your problem. For arbitrary alpha this is again the point where you can't dodge the trigonometry bullet; hence, it is again possible, but maybe not so viable in practice.
With help from Mathematica, it looks like we can rotate a point around a vector without Sin/Cos if you are willing to specify the amount of rotation as a number between -1 and 1, rather than an angle in radians.
The below starts with Mathematica's RotationTransform of a point {x,y,z} around a vector {u,v,w} by c radians (which contains many instances of Cos[c] and Sin[c]). It then substitutes all the Cos[c] with "c" and Sin[c] with Sqrt[1-c^2] (a trig identity for Sin in terms of Cos). Everything is simplified with the assumption that the rotation vector is normalized. The resulting equation produces the rotated point without any trig operations.
Note: as c ranges from -1 to 1 the point will only rotate through half a circle, the other half of the rotation can be achieved by flipping the signs on {u,v,w}.
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 am looking for an (almost everywhere) differentiable function f(p1, p2, p3, p4) that given four points will give me a scale-agnostic measure for co-planarity. It is zero if the four points lie on the same plane and positive otherwise. Scale-agnostic means that, when I uniformly scale all points the planarity measure will return the same.
I came up with something that is quite complex and not easy to optimize. Define u=p2-p1, v=p3-p1, w=p4-p1. Then the planarity measure is:
[(u x v) * w]² / (|u x v|² |w|²)
where x means cross product and '*' means dot product.
The numerator is simply (the square of) the volume of the tetrahedron defined by the four points, and the denominator is a normalizing factor that makes this measure become simply the cosine of an angle. Because angles do not changed under uniform scale, this function satisfies all my requirements.
Does anybody know of something simpler?
Alex.
Edit:
I eventually used an Augmented Lagrangian method to perform optimization, so I don't need it to be scale agnostic. Just using the constraint (u x v) * w = 0 is enough, as the optimization procedure finds the correct Lagrange multiplier to compensate for the scale.
Your methods seems ok, I'd do something like this for efficient implementation:
Take u, v, w as you did
Normalize them: various tricks exist to evaluate the inverse square root efficiently with whatever precision you want, like this jewel. Most modern processors have builtins for this operation.
Take f = |det(u, v, w)| ( = (u x v) . w ). There are fast direct implementations for 3x3 matrices; see #batty's answer to this question.
This amounts to what you do without the squares. It is still homogeneous and almost everywhere differentiable. Take the square of the determinant if you want something differentiable everywhere.
EDIT: #phkahler implicitly suggested using the ratio of the radius of the inscribed sphere to the radius of the circumscribed sphere as a measure of planarity. This is a bounded differentiable function of the points, invariant by scaling. However, this is at least as difficult to compute as what you (and I) suggest. Especially computing the radius of the circumscribed sphere is very sensitive to roundoff errors.
A measure that should be symmetric with respect to point reorderings is:
((u x v).w)^2/(|u||v||w||u-v||u-w||v-w|)
which is proportional to the volume of the tetrahedron squared divided by all 6 edge lengths. It is not simpler than your formula or Alexandre C.'s, but it is not much more complicated. However, it does become unnecessarily singular when any two points coincide.
A better-behaved, order-insensitive formula is:
let a = u x v
b = v x w
c = w x u
(a.w)^2/(|a| + |b| + |c| + |a+b+c|)^3
which is something like the volume of the tetrahedron divided by the surface area, but raised to appropriate powers to make the whole thing scale-insensitive. This is also a bit more complex than your formula, but it works unless all 4 points are collinear.
How about
|(u x v) * w| / |u|^3
(and you can change |x| to (x)^2 if you think it's simpler).
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.