Question

我想估计两个参考帧之间的旋转（在SO和在线中已经很好地建立），但是已经遇到了意想不到的结果。

如下面的R代码所示，我假设在2个参考帧（由向量表示）之间进行旋转以模拟真实场景 - 想想全局帧与某些旋转的本地帧。注意：实际上，事先不知道这种旋转。然后，我使用已建立的公式来估计帧之间的旋转。 但是，估计的旋转和真实旋转不相等。实际上，估计旋转和真实旋转之间的四元数差异的旋转轴等于用于估计旋转的矢量之一（抱歉笨重解释，见下面的演示）。对于我的应用，我希望旋转估计完全等于真实旋转。如果我可以＆＃34;退出＆＃34;旋转之间的四元数差异，那么我可以获得真正的旋转。用不同的方式说：因为我知道旋转差异的轴和如果我能找到旋转角度，那么我可以得到真正的旋转（参见下面的 trueDifferenceAngle ）。但是，我没有办法这么做......

搜索文献和论坛，我还没有找到任何关于这个问题的讨论。有什么想法吗？

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

Answer 1

估计的旋转数与真实的旋转数之间的四元数差异不小。 为什么？

那是因为您的函数quatFromTwoVectors(u,v)返回 a 旋转（作为四元数），该旋转将u发送到v。 但是还有其他轮换将u发送到v 。向量及其图像不足以获得唯一的旋转。

意外结果 - 向量之间的四元数

1 个答案: