# Applying offsets to six degrees of freedom

#### defunktlemon

Hi All.
I'm not 100% sure linear algebra will crunch this problem, but hopefully so. This may just be a case of matrices, which would be good cause I like those.

Imaging we have a robot with a camera attached. The camera can move in x,y,z, and also a,b,c. Where x is vertical, y horizontal and z lineal moving in and out. So, if it were a 3d / stereo camera it would give a distance reading on z.

a is rotation around x, b is rotation around y and c is rotation around z.

So, if I take an initial 4 (x,y,z) positions, one for each corner of an object and use these as my template locations, then if I change that object and place another identical object in it's place, but not exactly in the same location, I am able to get the offsets from the next 4 (x,y,z) positions and use these offsets to adjust the robot so that it can do any work on the object correctly as expected.

On top of this, I take a picture of a calibration dot grid, which is at a fixed location, and get coordinates for (x,y,z) and (a,b,c). And store these as my template positions in the event that the camera is moved or knocked, so that I can use these offsets when I change the next object.

The problem:
Problem is, in my object measurements I only have (x,y,z) and in the calibration measurements I have (x,y,z) and (a,b,c).
I need a way to translate the calibration offsets for (x,y,z) and (a,b,c) to the (x,y,z) for the object measurements and I'm not sure what method I should be looking at.

Thank you.

#### Country Boy

Math Team
In ordinary three dimensional coordinates, rotations can be done with 3 by 3 anti-symmetric matrices.

Rotation about the x-axis, through angle $$\displaystyle \theta$$ can be done by
$$\displaystyle \begin{bmatrix}1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta)\end{bmatrix}$$

Similarly rotation about the y-axis, through angle $$\displaystyle \theta$$ can be done by
$$\displaystyle \begin{bmatrix}cos(\theta) & 0 & -sin(\theta)\\ 0 & 1 & 0 \\ sin(\theta)& 0 & cos(\theta)\end{bmatrix}$$

and rotation about the z-axis by
$$\displaystyle \begin{bmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

Translations along the x, y, and z directions, as well as rotations, can be done with matrix multiplications, using "projective coordinates". In projective coordinates, we write the point (x, y, z) in the form [x, y, z, 1] with the understanding that if any operation causes that last "1" to change, we change back by dividing each term by it- that is, [x, y, z, a] means [x/a, y/z, z/a, 1].

In that way translation by a in the x-direction, b in the y-direction, and c in the z direction means multiplying
$$\displaystyle \begin{bmatrix}1 & 0 & 0 & a \\ 0 & 1 & 0 & b \\ 0 & 0 & 1 & c \\ 0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix}x \\ y \\ z \\ 1\end{bmatrix}= \begin{bmatrix}x+ a \\ y+ b \\ z+ c \\ 1 \end{bmatrix}$$.

And we can do the rotations at the same time by putting them into the 3 by 3 upper left corner.
$$\displaystyle \begin{bmatrix}1 & 0 & 0 & a \\ 0 & cos(\theta) & -sin(\theta) & b \\ 0 & sin(\theta) & cos(\theta) & c \\ 0 & 0 & 0 & 1\end{bmatrix}$$
rotates around the x-axis through angle $$\displaystyle \theta$$ and translates by the vector <a, b, c>.

#### defunktlemon

Thanks Country Boy, that's neat!.
I'm gonna give a step by step run through of the process which I will hopefully complete, as it seems there may be much more to this type of problem in knowing more about the original numbers and what they mean.

1: A camera is calibrated which gives 6 zero position values, 3 vectors for x,y,z translations and 3 spacial rotations a,b,c (angles) which rotate around x,y,z. These are my image window zero values.
2: An object is photographed, say I'm looking for the center of a hole on that object, and these x,y,z values become my template values, the zero values for the hole center, which will be used to analysis how off target any future same objects are when photographed.
3: When the camera is knocked or changed it will be in a different x,y,z and a,b,c location, i.e., my image window has shifted.
4: I then check the current camera location using the calibration to get x,y,z and a,b,c offsets from the original calibration.
5: Now, when another same object is photographed and hole-center x,y,z is searched for, because the image window is in a different location, these offsets need to be applied to the current x,y,z hole center-measurements so that if the hole-center is in the correct location in the real world, the x,y,z values being reported from the current photograph, even though the image window is in a different location, will be correct, i.e., the offsets will put them back to the zero position.

Phew! I think that covers it.
In terms of reading what the numbers mean exactly, do I need to be able to read those first or know what they are doing?