Search code examples
mathquaternionsroboticsslam

Find the next 3D point given a starting point, a orientation quaternion, and a distance travelled

What is the formula I need to use to find the second 3D point (P1) given:

  1. The first point P0 = [x0, y0, z0]
  2. An orientation quaternion Q0 = [q0, q1, q2, q3]
  3. The distance traveled S

I'm guessing that the distance traveled S needs to be split up into it's constituent X, Y and Z components. Is there an easy way to do this using quaternions?

Solution

  • Components of direction vector (forward-vector) are:

    x = 2 * (q1*q3 + q0*q2)
y = 2 * (q2*q3 - q0*q1)
z = 1 - 2 * (q1*q1 + q2*q2)

    This formula is calculated from Quaternion-to-Matrix (below) with multiplication by (0,0,1) vector.

    Normalize D=(x,y,z) if it is not unit, and calculate P_New.x= P0.x + S * D.x and other components.

    To get up- and left- vector of orientation (perhaps your orientation refers to another base frame orientation - OX or OY as forward), use another columns of the matrix cited below:

    Link: Quaternion multiplication and orthogonal matrix multiplication can both be used to represent rotation. If a quaternion is represented by qw + i qx + j qy + k qz , then the equivalent matrix, to represent the same rotation, is:

    1 - 2*qy2 - 2*qz2   2*qx*qy - 2*qz*qw   2*qx*qz + 2*qy*qw
2*qx*qy + 2*qz*qw   1 - 2*qx2 - 2*qz2   2*qy*qz - 2*qx*qw
2*qx*qz - 2*qy*qw   2*qy*qz + 2*qx*qw   1 - 2*qx2 - 2*qy2