SHORTEST POSSIBLE WAY TO ASK MY QUESTION (edit)
I have a 3D point (x,y,z) and two coordinate systems. The rotation and translation between the coordinate systems is known, as I have a quaternion which gives the rotation from Coordinate System A to Coordinate System B. I know the location of the point (x,y,z) in terms of Coordinate System B, and I need to know its location (x',y',z') in terms of Coordinate System A. How can I do this?
See below for more information about the problem.
The problem
I'm working with a laser rangefinder and a GPS and trying to use coordinate transformations to figure out the Earth-Centered Earth-Fixed (ECEF) location of the "point in question" (by which I mean the point that the rangefinder "sees"). The endgame is LiDAR-related, but I'll go on to explain the question in a way that doesn't require LiDAR knowledge.
The rangefinder and the GPS are connected, sharing a common z-axis (positive z direction is down). The rangefinder rotates about the shared z-axis, and its azimuth (angle representing the rangefinder's rotation about the z-axis with respect to the positive x-axis of the gps, otherwise known as the rangefinder's "home" position) is known at all times. The distance from the rangefinder to the "point in question" is also known at all times.
The GPS can rotate and translate freely over time (endgame – it's attached to a drone), and its latitude, longitude and altitude are known at all times, as well as its rotation with respect to the local North-East-Down (NED) coordinate system. I can get this rotation information from the GPS as Roll, Pitch and Yaw or in the form of a quaternion. I'm relatively new to the world of quaternions, but by my understanding, using quaternions is better for this application.
I want to take my information about the range and azimuth of the "point in question" with respect to the GPS and use what I know about the position and rotation of the GPS in order to find the xyz ECEF location of the "point in question".
In short…
I have a vector constructed from azimuth and range information, with respect to a local coordinate system. I would like to take this information, along with information about the translation and rotation from the global to local coordinate systems, to find the xyz position of the "point in question" (the tip of the vector?) in terms of my global coordinate system.
How can I take this vector with respect to a local coordinate system and use the information relating the local coordinate system to the global coordinate system in order to find my point's location in the global coordinate system?
What I have tried
I have tried making a "pure" quaternion out of my vector representing the "point in question" with respect to the rangefinder, then rotating this vector by the quaternion indicating the rotation of the GPS with respect to NED, and then translating from NED to ECEF using some equations I know to be correct. This did not work, but it seemed like it could be on the right track.
My "pure" quaternion is my rangefinder vector, with an added zero at the front. My quaternion standard is $w+xi+yj+zk$. My method of rotating my vector by my quaternion is:
$$v' = q'vq$$
Where $v'$ is my translated vector (a "pure" quaternion), $q'$ is the inverse of my quaternion, $v$ is my original vector ("pure" quaternion), and $q$ is my original quaternion.
Let me know if I can make any of this clearer or if there is information I left out. I appreciate any responses that may point me in the right direction.
Best Answer
First send your point $P_B$ from reference frame $B$ to the origin.
$P_O = P_B - O_B$
Where $O_B$ is the origin of reference frame $B$.
Then rotate the vector $P_O$ using the inverse of quaternion $q$
$P'_O = q^{-1} P_O q$
If quaternion is of unit length you can use the conjugate inplace of the inverse.
Finally send the rotated vector to reference frame $A$:
$P_A = P'_O + O_A$
Where $O_A$ is the origin of reference frame $A$.