# SE3 composition operator gives unexpected results

## Given Data and Algorithm

I have a stream of SE3 poses supplied by a basic wheel encoder odometry through ROS message passing system. Odometry publishes data in traditional to ROS ENU coordinate frame (X - forward, Y - left, Z - up) with right chirality.

I present this trajectory on graphs below (TX, TY, RZ):

It should be obvious that other 3 dimensions have all zeros in them, as the wheel odometry poses have only 3DoF.

Then I rotate this stream of poses to another coordinate frame, customary for Visual SLAM (Z - forward, X - right, Y - down). The acquired result is shown below and is not what I have expected.

More Formally:
Let's say agent's camera is aligned with principal axes of agent and relative translation from camera to agent center is neglected. In other words: $P_{C_i}^{B_i} = \text{Id}$.
I have a stream of SE3 transformations from current place of agent's body to Odometry World Coordinate Frame: $$P_{B_0}^{W_O}, P_{B_1}^{W_O}, P_{B_2}^{W_O}, \dots$$ I need to observe this motion from standpoint of a Visual Slam:
$$P_{C_0}^{W_V}, P_{C_1}^{W_V}, P_{C_2}^{W_V}, \dots$$

So I do this in a following manner: $$P_{C_i}^{W_V} = P_{W_O}^{W_V} \cdot P_{B_i}^{W_O} \cdot P_{C_i}^{B_i} = P_{W_O}^{W_V} \cdot P_{B_i}^{W_O}$$ I define $P_{W_O}^{W_V} = (\text{Quaternion}(\text{Euler}(-90, -90, 0)), (0, 0, 0))$, where Euler is intrinsic and active, and order is $(z,x,y)$. The resulting quaternion $\text{qxyz}$ is $(\frac 1 2, \frac 1 2, -\frac 1 2, \frac 1 2)$ and Euler angles choice can be verified geometrically with the help of the following picture:

## Results Interpretation and Question

I have plenty of positive X and positive Y translational movement in the start of dataset, and some yaw rotation along Z axis later on.
So, when I look at this motion from Visual World coordinate frame, I expect a plenty of positive Z and negative X translational movement in the start of dataset, and some yaw rotation along -Y axis later on.
While the linear part behaves exactly as I describe, the rotation does something else. And I am bewildered by results and count on your help.
It's terrible to realize that these group operations on SO3 are still a mystery to me.
Comparison graphs are shown below (TX,TY,TZ,RX,RY,RZ):

Sorry for the longpost!

Meta:
I have long pondered where to post: in Math or Robotics. I have finally decided for the latter, for the question seems very "applied".

## 1 Answer

The mystery has been solved. In reality, the equality does not hold in the last part of the proposition, stated in the question:
$$P_{C_i}^{W_V} = P_{W_O}^{W_V} \cdot P_{B_i}^{W_O} \cdot P_{C_i}^{B_i} \ne P_{W_O}^{W_V} \cdot P_{B_i}^{W_O}$$

More specifically: $$P_{C_i}^{B_i} \ne \text{Id}$$ But In reality: $$P_{C_i}^{B_i} = \big[ P_{W_O}^{W_V} \big]^{-1} = P_{W_V}^{W_O}$$ Hence:
$$P_{C_i}^{W_V} = P_{W_O}^{W_V} \cdot P_{B_i}^{W_O} \cdot P_{C_i}^{B_i} = P_{W_O}^{W_V} \cdot P_{B_i}^{W_O} \cdot P_{W_V}^{W_O}$$

Such a transformation yields correct rotation of agent in Visual World coordinate frame: