How to perform this reference system transformation?

Question

I have two quaternions that indicate the initial orientation of a four wheel robot, each one in relative to one reference systems.

The robot's orientation given by a quaternion q is not the same in the two reference systems: For one reference system the quaternion q1 might refer to the robot looking at positive x while the same quaternion components q1 in the second reference system might refer to the robot looking at the negative x.

I have two matrices which indicate the position of the robot in time in its correspondent reference system.

I want to find the correspondent points of the first matrix in to the second reference system. Each matrix is built with a different sensor, so the results will be similar but not exactly the same.

I think I should find the transformation from the first reference system to the second and then apply it for each point of the first matrix. How can I find this transformation? The translation part I think is clear, but the rotation not at all.

Edit:

@WildCrustacean

The solution proposed does not solve the problem, I think that the reason is because each system represents the robot in a different way.

In the initial one (A) the robot moving forward with no rotation would increase in the X axis. In the goal referential system (B) the robot moving forward with no rotation would increase in the Z axis. (See figure for more details)

First system (A)
 ______
|\  T  \
| \_____\     z
|B |    | : y ^
 \ | R  |    \|
  \|____|     +--> x


Second system (B)
 ______
|\  T  \
| \_____\     x
|B |    | :   ^
 \ | R  |     |
  \|____|     +--> z
               \
                y

R: Right side
B: Back side
T: Top

I think I have to apply an extra rotation to change the initial quaternion that belongs to the first system to be in accordance with the second system before applying the transformation of your post.

One rotation of 180 degrees around x followed by one of 90 around y. Would rotate from A to B

This is how I tried to solve it:

# Quaternion to adjust reference system
first_quat = make_quaternion(unitary_x, pi) # Generates the quaternion that rotates pi around X 
second_quat = make_quaternion(unitary_y, pi/2.0) # Generates the quaternion that rotates pi/2 around Y 
composed_fs_q = first_quat*second_quat 

# Quaternion to rotate from one reference system to the other
quaternion_ini_A = quaternion_ini_A*composed_fs_q
A2B_quaternion = quaternion_ini_B*(quaternion_ini_A.inverse())

A2B_quaternion is the quaternion that I use for the rotation but still doesn't perform the right rotation. Any idea?

Answer 1

So you need to find the rotation that gets you from the first coordinate system to the second, and then combine that with the translation to get the final transformation matrix.

Since you have quaternions q1 and q2, you can write this rotation as:

$q = q_2 * q_1^{-1}$

You can then convert from the quaternion form to a rotation matrix to make the final transformation matrix, there are equations for this.

The final transformation would be of the form:

$\begin{bmatrix}\boldsymbol{R} & \boldsymbol{t}\\\boldsymbol{0} & 1\end{bmatrix}$

Where $\boldsymbol{R}$ is the 3x3 rotation matrix and $\boldsymbol{t}$ is the 3x1 translation vector.

Answer 2

In your particular case, isn't it easiest to just transpose the axes:

Bx =  Az
By = -Ay
Bz =  Ax

Or if you want to write that as a matrix transform:

B = A * [ [  0,  0,  1  ],
          [  0, -1,  0  ],
          [  1,  0,  0  ] ]