定向点（XYZ +偏航/俯仰/滚动）帧间转换

Question

I have a 3D oriented point (XYZ + Yaw/Pitch/Roll). Lets say this point is based on "user frame 1" (UF1)(the frame is defined in relation to the world frame (WF)).

I also have another "user frame", lets call it "user frame 2" (UF2) that is also defined in reference to the world frame.

How would I take my 3d oriented point (6 coordinates; 3 location + 3 rotation) from one frame to another ?

For example; Since all user frame are related to the world frame, How would I calculate (transform) my 3d oriented point from UF1 to WF ? Or vice-versa or from any combination ? (UF1->WF, WF->UF2, etc.)

The resulting point must have the 6 coordinates (3 location + 3 rotation) in relation to the destination frame.

PS: I'm mainly working in C#, so if possible code sample or pseudo-code to accomplish this would be appreciated.

Answer 1

What you are looking for is the transition matrix from one coordinate system to an other.

It is a 4x4 matrix fully determined by the 3 parameters of translation and the 6 angles of ratotion between the 2 coordinate system.

The 9 coefficients depend on the rotation angles (Euler angles or quaternion depending on what you are using). Be aware that if you are using Euler angles, the order is important : a vector of rotation (rx, ry, rz) does not describe the same rotation if you turn around X axis first then Y then Z or if you turn first around Z, then Y then X for example.

I suggest you read this, it will help you compute the matrix: http://brainvoyager.com/bv/doc/UsersGuide/CoordsAndTransforms/SpatialTransformationMatrices.html

Note that all this is using homogenous coordinate.