Python implementation of three-dimensional rigid body transform and rotation

Question

I tried to solve how to solve the following problem using python:

• We have points a, b, c, d that form a solid
• Unknown 3D translation and rotation apply to a rigid body
• Now we know the coordinates for a, b, c
• We want to calculate the coordinates for d

What I know so far:

• Trying to do this with “simple” calculations of the Euler angle seems like a bad idea due to the locking of the driveshaft, etc.

• As a result, step 4 will include the transformation matrix, and once you know the rotation and translation matrix, it seems that this step is easy to use in one of the following ways:

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

What I can’t solve is how I can calculate the rotation and translation matrices, given the “new” coordinates a, b, c.

I see that in the general case (non-rigid body) part of the rotation of this Wahba problem, but I think that for solids there should be some faster way to calculate it directly by developing a set of orthogonal unit vectors using points.

Answer 1

To set the corresponding points that you are trying to match (with a possible perturbation), I used SVD (decomposition of singular values), which seems to exist in numpy.

An example of this technique (in Python even) can be found here , but I did not rate it for correctness.

What you are going to use is a “base transformation” or “base change”, which will be represented as a transformation matrix. Assuming your 3 known points are not collinear, you can create your initial base:

• Calculation of vectors: x = (ba) and y = (ca)
• Normalize x (x = x / value (x))
• Project y on x (proj_y = x DOT y * x)
• Subtract the projection from y (y = y - proj_y)
• Normalize y
• Calculate z = x CROSS y

This gives you the starting coordinate x, y, z A. Do the same for your new points, and you get the second base B. Now you want to find the transformation T, which will take the point in and convert it to B (change of base). This part is simple. You can invert A to convert points back to a normal basis, and then use B to convert to the second. Since A is orthonormalized, you can simply transpose A to get the opposite. Thus, the “new d” is equal to d * inverse (A) * B. (Although depending on your view, you may need to use B * inverse (A) * d.)

You need to have some familiarity with matrices in order to get all this. Your presentation of vectors and matrices will inform you of the order in which the matrices are multiplied to get T (T is either the inverse (A) * B or B * the inverse (A)).

To calculate the base matrix from your vectors x = (x1, x2, x3), y = (y1, y2, y3), z = (z1, z2, z3), you fill it as:

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