1
$\begingroup$

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):
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: enter image description here

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):
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".

$\endgroup$
1
$\begingroup$

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:
rx ry rz

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.