[PYTHON] Let's cross the wall of the left-handed coordinate system and the right-handed coordinate system.

Introduction

It is a completely personal note at that time because I needed to convert the rotation in the left hand coordinate system (often used in computer vision and Unity) and the rotation in the right hand coordinate system (often used in robots and physics). Many libraries are built in the right-handed coordinate system, so I've summarized how to convert them.

Four things that represent rotation

In the first place, what represents rotation

--Euler angles (roll pitch yaw) --Rotation vector --Rotation matrix --Quaternion

There are four. I will omit each explanation here. Because there are many articles explained For example

-Quaternion is organized! -Handling the rotation and posture of 3D objects vividly- -I summarized the rotation vector, rotation matrix, quaternion, and Euler angles

I hope you can refer to. The only thing I'm curious about is the following. Is there any difference between rotating $ θ $ (around an arbitrary axis) in the left-handed coordinate system and $ θ $ (around an arbitrary axis) in a right-handed coordinate system?

Conclusion

Basically, you can think of the left-handed coordinate system as the left-handed coordinate system and the right-handed coordinate system as the right-handed coordinate system. However, if you see ~ in the right-handed coordinate system from the left-handed coordinate system, you need to convert it!

Rotation matrix

First, let's look at the rotation matrix.

Right-handed coordinate system

Let's look at the x-axis, y-axis, z-axis, and arbitrary axis circumference. This means what kind of transformation should be done to rotate θ (around an axis) in the right-handed coordinate system.

x axis

R^{right}_x = 
  \left(
    \begin{array}{ccc}
      1 & 0 & 0 \\
      0 & \cos \theta & - \sin \theta \\
      0 & \sin \theta & \cos \theta
    \end{array}
  \right)

y axis

R^{right}_y = 
  \left(
    \begin{array}{ccc}
      \cos \theta & 0 & \sin \theta \\
      0 & 1 & 0 \\
      - \sin \theta & 0 & \cos \theta
    \end{array}
  \right)

z-axis

R^{right}_z = 
  \left(
    \begin{array}{ccc}
      \cos \theta & - \sin \theta & 0 \\
      \sin \theta & \cos \theta & 0 \\
      0 & 0 & 1
    \end{array}
  \right)

Arbitrary axis circumference

If you rotate $ \ theta $ with $ n = [n_x, n_y, n_z] $,

R^{right}_n = 
 \left(
    \begin{array}{ccc}
      n_x^2(1- \cos \theta) + \cos \theta & 
      n_x n_y (1- \cos \theta) - n_z \sin \theta & 
      n_x n_z (1- \cos \theta) + n_y \sin \theta \\
      n_x n_y (1- \cos \theta) + n_z \sin \theta & 
      n_y^2(1- \cos \theta) + \cos \theta &
      n_y n_z (1- \cos \theta) - n_x \sin \theta \\
      n_x n_z (1- \cos \theta) - n_y \sin \theta & 
      n_y n_z (1- \cos \theta) + n_x \sin \theta & 
      n_z^2(1- \cos \theta) + \cos \theta
    \end{array}
  \right)

Left hand coordinate system

Let's look at the x-axis, y-axis, z-axis, and arbitrary axis circumference. This means what kind of transformation should be done to rotate θ (around an axis) in the left-handed coordinate system. ** (I made a mistake in the article I mentioned at the beginning ... It's a correction.) **

x axis

R^{left}_x = 
  \left(
    \begin{array}{ccc}
      1 & 0 & 0 \\
      0 & \cos \theta & - \sin \theta \\
      0 & \sin \theta & \cos \theta
    \end{array}
  \right)

y axis

R^{left}_y = 
  \left(
    \begin{array}{ccc}
      \cos \theta & 0 & \sin \theta \\
      0 & 1 & 0 \\
      -\sin \theta & 0 & \cos \theta
    \end{array}
  \right)

z-axis

R^{left}_z = 
  \left(
    \begin{array}{ccc}
      \cos \theta & -\sin \theta & 0 \\
      \sin \theta & \cos \theta & 0 \\
      0 & 0 & 1
    \end{array}
  \right)

Arbitrary axis circumference

If you rotate $ \ theta $ with $ n = [n_x, n_y, n_z] $,

R^{left}_n = 
 \left(
    \begin{array}{ccc}
      n_x^2(1- \cos \theta) + \cos \theta & 
      n_x n_y (1- \cos \theta) - n_z \sin \theta & 
      n_x n_z (1- \cos \theta) + n_y \sin \theta \\
      n_x n_y (1- \cos \theta) + n_z \sin \theta & 
      n_y^2(1- \cos \theta) + \cos \theta &
      n_y n_z (1- \cos \theta) - n_x \sin \theta \\
      n_x n_z (1- \cos \theta) - n_y \sin \theta & 
      n_y n_z (1- \cos \theta) + n_x \sin \theta & 
      n_z^2(1- \cos \theta) + \cos \theta
    \end{array}
  \right)

Is the same. I think this is because the coordinate system itself is set so that it makes sense. (I think this is because θ is set to reverse rotation in the first place, and the axes are also swapped.) (I'm a little unsure, so if you have any comments, please.)

Convert the rotation matrix of the right-handed coordinate system to the left-handed coordinate system

What happens when the rotation matrix of the right-handed coordinate system is viewed from the left-handed coordinate system? It is a story.

** It's okay if you transpose! ** **

So all you have to do is transpose. Easy.

Quaternion

Right-handed coordinate system

In the right-handed coordinate system, a quaternion (meaning to rotate around an arbitrary axis) is

q^{right} = [q_w, q_x, q_y, q_z]

is.

Left hand coordinate system

In the left-handed coordinate system, the quaternion (meaning to rotate around an arbitrary axis) is

q^{left} = [q_w, q_x, q_y, q_z]

is. Well, here, isn't it the same? I think it will be, but it is the same. Obviously, it's the same. (Because the quaternion is not something like that in the left-handed coordinate system ...)

Convert from right-handed quaternion to left-handed quaternion

But what if we look at the quaternion in the right-handed coordinate system in the left-handed coordinate system? It is different if it is a story.

q^{right} = [q_{w1}, q_{x1}, q_{y1}, q_{z1}]

Is

q^{left} = [q_{w1}, -q_{x1}, q_{y1}, -q_{z1}]

Will be.

Rotation vector

The same is true for this! The definition is the same!

Right-handed coordinate system

The rotation vector in the right-handed coordinate system (meaning to rotate around an arbitrary axis) is

n^{right} = [u, v, w]

Left hand coordinate system

The rotation vector (meaning to rotate around an arbitrary axis) in the left-handed coordinate system is

n^{left} = [u, v, w]

Convert from a rotating vector in a right-handed coordinate system to a rotating vector in a left-handed coordinate system

But what if we look at the rotating vector in the right-handed coordinate system in the left-handed coordinate system? It is different if it is a story.

n^{right} = [u_1, v_1, w_1]

Is

n^{left} = [-u_1, v_1, -w_1]

Will be.

Outer product (cross product)

Right-handed coordinate system

In the right-handed coordinate system, taking the cross product of two vectors, $ a = [a_x, a_y, a_z] $, $ b = [b_x, b_y, b_z] $

a \times b = 
  \left(
    \begin{array}{c}
      a_y b_z - a_z b_y \\
      a_z b_x - a_x b_z \\
      a_x b_y - a_y b_x
    \end{array}
  \right)

It will be. With numpy

>>> a = np.array([1., 2., 3.])
>>> b = np.array([4., 5., 6.])

>>> np.cross(a, b)
# array([-3.,  6., -3.])

You can calculate with!

Left hand coordinate system

The direction of the outer product is opposite, but θ is also opposite, so it should be usable as it is after all. .. ..

a \times b = 
  \left(
    \begin{array}{c}
      a_z b_y - a_y b_z \\
      a_x b_z - a_z b_x \\
      a_y b_x - a_x b_y
    \end{array}
  \right)

So it's the same!

>>> a = np.array([1., 2., 3.])
>>> b = np.array([4., 5., 6.])

>>> np.cross(a, b)
# array([-3.,  6., -3.])

atan

Right-handed coordinate system

Taking arctan in the right-handed coordinate system is numpy

>>> a = 1.
>>> b = 2.
>>> np.arctan2(a, b)
# 0.4636476090008061

Can be calculated.

Left hand coordinate system

Taking arctan in the left-handed coordinate system is the opposite of the calculation in the right-handed coordinate system, but the vector in the left-handed coordinate system is converted to the right-handed coordinate system (y-axis inversion), and then arctan is taken. Here, θ has a minus sign, but when you return the angle to the left-hand coordinate system (because θ is set in the opposite direction). .. .. After all, it will remain the same.

>>> a = 1.
>>> b = 2.
>>> np.arctan2(a, b)
# 0.4636476090008061

Convert from quaternion to rotation matrix

Right-handed coordinate system

I want to convert the quaternion $ q ^ {right} = [q_ {w1}, q_ {x1}, q_ {y1}, q_ {z1}] $ in the right-handed coordinate system to the rotation matrix $ R ^ {right} $. In general, the quaternion to rotation matrix conversion is:

R^{right}_{q^{right}} = 
 \left(
    \begin{array}{ccc}
      q_w^2 + q_x^2 - q_y^2 - q_z^2 & 
      2(q_x q_y - q_w q_z) & 
      2(q_z q_x - q_w q_y) \\
      2(q_x q_y - q_w q_z) & 
      q_w^2 - q_x^2 + q_y^2 - q_z^2 &
      2(q_y q_z - q_w q_x) \\
      2(q_z q_x - q_w q_y) & 
      2(q_y q_z - q_w q_x) & 
      q_w^2 - q_x^2 + q_y^2 - q_z^2
    \end{array}
  \right)

Left hand coordinate system

I want to convert the quaternion $ q ^ {left} = [q_ {w1}, q_ {x1}, q_ {y1}, q_ {z1}] $ in the left-handed coordinate system to the rotation matrix $ R ^ {left} $. You can use it as usual.

R^{left}_{q^{left}} = 
 \left(
    \begin{array}{ccc}
      q_w^2 + q_x^2 - q_y^2 - q_z^2 & 
      2(q_x q_y - q_w q_z) & 
      2(q_z q_x - q_w q_y) \\
      2(q_x q_y - q_w q_z) & 
      q_w^2 - q_x^2 + q_y^2 - q_z^2 &
      2(q_y q_z - q_w q_x) \\
      2(q_z q_x - q_w q_y) & 
      2(q_y q_z - q_w q_x) & 
      q_w^2 - q_x^2 + q_y^2 - q_z^2
    \end{array}
  \right)^T

Convert from rotation matrix to square axis equivalent vector

A certain document talks about calculating the difference in the rotation matrix with the rotation vector. It was out. Specifically, I would like you to see Appendix A, but the point is that the rotation matrix is a square axis equivalent vector (slightly different from the rotation vector). The literature itself describes the calculation method in the right-handed coordinate system. Can be used as it is in the above flow.

Summary

If you use the above

I want to make the quaternion of the right-handed coordinate system the rotation matrix of the right-handed coordinate system and the rotation matrix of the left-handed coordinate system.

You can also say!

It's just a memo, so please feel free to comment if there are any mistakes.