CODEWITHSUNDEEP
rquaternions
cdxsza
cdxsza

Reputation: 171

Unexpected result - Quaternion between vectors

I want to estimate the rotation between two reference frames (which is well established in SO and online), but have run into unexpected results.

As shown in the R code below, I assume a rotation between 2 reference frames (represented by vectors) to simulate a real-world scenario -- think a global frame vs some rotated local frame. Note: in practice, this rotation would not be known beforehand. Then, I use established formulae to estimate the rotation between frames. However, the estimated rotation and the true rotation are not equal. In fact, the quaternion difference between estimated and true rotations has a rotation axis equal to one of the vectors used to estimate the rotation (sorry for clunky explanation, see below for a demo). For my application, I would prefer that the rotation estimate exactly equals the true rotation. If I can "back-out" the quaternion difference between rotations, then I can get the true rotation. Said a different way: since I know the axis of the rotation difference AND if I can find the rotation angle, then I can get the true rotation (see trueDifferenceAngle below). However, I don't see a way to do so...

Scouring literature and forums, I haven't found any discussion of this issue. Any ideas?

install.packages("pracma")
library(pracma)

# Build quaternion between two vectors
# This comes from http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors, towards the bottom
quatFromTwoVectors <- function(vStart, vEnd) {
  vStart <- vStart / norm(vStart, "2")
  vEnd <- vEnd / norm(vEnd, "2")

  s <- sqrt(2 + 2 * dot(vStart, vEnd))
  w <- (1/s) * cross(vStart, vEnd)
  quat <- c(s/2, w)
  return(quat)
}

# Build rotation matrix from given quaternion
rotationMatrixFromQuaternion <- function(quat) {
  qCross <- matrix(c(c(0, quat[4], -quat[3]), c(-quat[4], 0, quat[2]), c(quat[3], -quat[2], 0)), ncol = 3)

  rotMat <- (quat[1]^2 - norm(quat[2:4], "2")^2) * diag(1,3) + 2 * quat[2:4] %*% t(quat[2:4]) + 2 * quat[1] * qCross
  return(rotMat)
}

quaternionMultiplication <- function(q1, q2) {
  s <- q1[1] * q2[1] - dot(q1[2:4], q2[2:4])
  w <- q1[1] * q2[2:4] + q2[1] * q1[2:4] + cross(q1[2:4], q2[2:4])
  quat <- c(s,w)
  quat <- quat / norm(quat, "2")

  return(quat)
}

quaternionInverse <- function(quat) {
  quat <- c(quat[1], -quat[2:4])
  quat <- quat / norm(quat, "2")
  return(quat)
}

# The following quaternion (s, x, y, z) rotation corresponds to the ZYX Euler angles (12 deg, -23 deg, 34 deg)
# ASSUME THIS IS UNKNOWN BEFOREHAND
rotationFrame1ToFrame2 <- c(0.9258802, 0.1559245, -0.1596644, 0.3048618)

# Inverse of prior quaternion
# ASSUME THIS IS UNKNOWN BEFOREHAND
rotationFrame2ToFrame1 <- c(0.9258802, -0.1559245, 0.1596644, -0.3048618)

# This rotation matrix agrees with the output from https://www.andre-gaschler.com/rotationconverter/ using the above Euler quaternion
# ASSUME THIS IS UNKNOWN BEFOREHAND
rotMat <- rotationMatrixFromQuaternion(rotationFrame1ToFrame2)

# Reference vectors in frames 1 (global, known) & 2 (local, known)
vectorFrame1 <- 1:3
vectorFrame2 <- as.vector(rotMat %*% vectorFrame1)

# Estimate rotation between frames 1 & 2 using quatFromTwoVectors
rotationFrame2ToFrame1.estimate <- quatFromTwoVectors(vectorFrame2, vectorFrame1)

# The quaternion difference between estimated and true rotations is non-trivial. 
# **Why?**
rotationDifference <- quaternionMultiplication(rotationFrame2ToFrame1.estimate, quaternionInverse(rotationFrame2ToFrame1))
rotationMatrixFromQuaternion(rotationDifference)

# Furthermore, why is vectorFrame1 the rotation axis for rotationDifference? (It's an eigenvector with eigenvalue of +1)
rotationMatrixFromQuaternion(rotationDifference) %*% vectorFrame1 - vectorFrame1    # Should equal origin to machine precision
eigen(rotationMatrixFromQuaternion(rotationDifference))   # The only real eigenvector normalizes to the unit version of vectorFrame1

# If I could determine the rotation angle of rotationDifference (without already knowing the true rotation), 
# I could "subtract out" the rotation difference and calculate the true rotation
trueDifferenceAngle <- acos(rotationDifference[1]) * 2
vectorFrame1.unit <- vectorFrame1 / norm(vectorFrame1, "2")
rotationDifference.AxisAngleForm <- c(cos(trueDifferenceAngle/2), sin(trueDifferenceAngle/2) * vectorFrame1.unit)
rotationDifference - rotationDifference.AxisAngleForm   # Should equal origin to machine precision

quaternionMultiplication(quaternionInverse(rotationDifference.AxisAngleForm), rotationFrame2ToFrame1.estimate) - rotationFrame2ToFrame1   # Should equal origin to machine precision

Upvotes: 1

Views: 159

Answers (1)

St&#233;phane Laurent
St&#233;phane Laurent

Reputation: 84529

The quaternion difference between estimated and true rotations is non-trivial. Why?

That's because your function quatFromTwoVectors(u,v) returns a rotation (as a quaternion) which sends u to v. But there are others rotations which send u to v. A vector and its image are not sufficient in order to get a unique rotation.

Upvotes: 1

Related Questions