python implementation of 3D rigid body translation and rotation

Question

I've been trying to work out how to solve the following problem using python:

1. We have points a, b, c, d which form a rigid body
2. Some unknown 3D translation and rotation is applied to the rigid body
3. We now know the coordinates for a, b, c
4. We want to calculate coordinates for d

What I know so far:

• Trying to do this with "straightforward" Euler angle calculations seems like a bad idea due to gimbal lock etc.

• Step 4 will therefore involve a transformation matrix, and once you know the rotation and translation matrix it looks like this step is easy using one of these:

  • http://www.lfd.uci.edu/~gohlke/code/transformations.py.html
  • https://pypi.python.org/pypi/euclid/0.01

What I can't work out is how I can calculate the rotation and translation matrices given the "new" coordinates of a, b, c.

I can see that in the general case (non-rigid body) the rotation part of this is Wahba's problem, but I think that for rigid bodies there should be some faster way of calculating it directly by working out a set of orthogonal unit vectors using the points.

Answer 1

For a set of corresponding points that you're trying to match (with possible perturbation) I've used SVD (singular value decomposition), which appears to exist in numpy.

An example of this technique (in Python even) can be found here, but I haven't evaluated it for correctness.

What you're going for is a "basis transform" or "change of basis" which will be represented as a transformation matrix. Assuming your 3 known points are not collinear, you can create your initial basis by:

1. Computing the vectors: x=(b-a) and y=(c-a)
2. Normalize x (x = x / magnitude(x))
3. Project y onto x (proj_y = x DOT y * x)
4. Subtract the projection from y (y = y - proj_y)
5. Normalize y
6. Compute z = x CROSS y

That gives you an initial x,y,z coordinate basis A. Do the same for your new points, and you get a second basis B. Now you want to find transform T which will take a point in A and convert it to B (change of basis). That part is easy. You can invert A to transform the points back to the Normal basis, then use B to transform into the second one. Since A is orthonormal, you can just transpose A to get the inverse. So the "new d" is equal to d * inverse(A) * B. (Though depending on your representation, you may need to use B * inverse(A) * d.)

You need to have some familiarity with matrices to get all that. Your representation of vectors and matrices will inform you as to which order to multiply the matrices to get T (T is either inverse(A)*B or B*inverse(A)).

To compute your basis matrix from your vectors x=(x1,x2,x3), y=(y1,y2,y3), z=(z1,z2,z3) you populate it as:

| x1 y1 z1 |
| x2 y2 z2 |
| x3 y3 z3 |