四元数插值Eigen源码解析

最新推荐文章于 2025-02-28 20:38:40 发布

VoladorL

最新推荐文章于 2025-02-28 20:38:40 发布

阅读量1.1k

点赞数 1

文章标签： c++

本文链接：https://blog.csdn.net/weixin_44368449/article/details/130610866

版权

文章详细介绍了四元数的Slerp（球面线性插值）公式，并提供了Eigen库中实现四元数插值的源码分析。Slerp确保了在四维空间中的旋转遵循最短路径，源码中通过处理四元数的点积和角度计算来实现这一过程。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

四元数插值公式
${\displaystyle \operatorname {Slerp} (q_{0},q_{1};t)={\frac {\sin {[(1-t)\Omega }]}{\sin \Omega }}q_{0}+{\frac {\sin[t\Omega ]}{\sin \Omega }}q_{1}}, 0 ≤ t ≤ 1$
原理可以看这篇博客
根据公式我们可以比较清晰的分析四元数插值Eigen源码，源码如下所示：

/** \returns the spherical linear interpolation between the two quaternions
  * \c *this and \a other at the parameter \a t in [0;1].
  * 
  * This represents an interpolation for a constant motion between \c *this and \a other,
  * see also http://en.wikipedia.org/wiki/Slerp.
  */
  /** 
  * 返回两个四元数之间的球面线性插值
  * onst Scalar& t : 插值的比例系数，插值时刻距离起始四元数的时间
  * 占两个四元数时间间隔的多少
  * other：插值的另一个四元数$q_{1}$
  **/
template <class Derived>
template <class OtherDerived>
EIGEN_DEVICE_FUNC Quaternion<typename internal::traits<Derived>::Scalar>
QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const
{
// 使用std::acos和std::sin
  EIGEN_USING_STD_MATH(acos)
  EIGEN_USING_STD_MATH(sin)
  // Scalar类型的1，NumTraits<Scalar>::epsilon()，参考这个https://eigen.tuxfamily.org/dox/structEigen_1_1NumTraits.html
  const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
  // 两个四元数向量的点积，因为四元数的模长都为1所以点积就是夹角的余弦值
  Scalar d = this->dot(other);
  /**
  *  如果四元数点积的结果是负值（夹角大于90°），那么后面的插值就会在4D球面上绕远路。
  * 为了解决这个问题，将余弦值取绝对值保证这个旋转走的是最短路径。
  **/
  Scalar absD = numext::abs(d);

  Scalar scale0;
  Scalar scale1;
// 如果两个四元数的夹角的cos值接近1的话，为了避免除法出问题，直接算极限 lim_{t->0}{sint/t} = 1
  if(absD>=one)
  {
    scale0 = Scalar(1) - t;
    scale1 = t;
  }
  else
  {
    // theta is the angle between the 2 quaternions
    // 计算四元数向量之间夹角
    Scalar theta = acos(absD);
    Scalar sinTheta = sin(theta);

    scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
    scale1 = sin( ( t * theta) ) / sinTheta;
  }
  if(d<Scalar(0)) scale1 = -scale1;
// coeffs返回的是以(x,y,z,w)顺序的四元数，因为如果通过double直接对四元数初始化赋值的话它的顺序是(w,x,y,z)
  return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
}