Dual Quaternion representing Rigid Transformation

Dual Quaternion 是 Dual Number 与 Quaternion（虚数） 的结合。对我而言，这东西可以表示 3 维空间的 Rigid Transformation。

Dual Number 在前面的博客 [ceres-solver] AutoDiff 已经讲到，就是有一个幂零元（dual unit） (epsilon)，有 (epsilon^2=0)。

Dual Number 包含两个四元数，可以写作 (mathbf{r} + epsilon mathbf{d})，其中 (mathbf{r}, mathbf{d}) 都是四元数。

Dual Number 可以进行 +, -, * 三种运算，+, - 运算满足交换律与结合律。* 运算因为涉及到四元数的虚部 (i, j, k) 所以不满足的交换律，满足结合律。在 * 运算时注意遵守虚部乘法规则（Hamilton 四元数）—— (i^{2}=j^{2}=k^{2}=ijk=-1)，与幂零元的运算规则—— (epsilon^2=0) 。

这些基本的运算参考 [1] 。

接下来介绍 Dual Quaternion 用于刚体变换，也就是旋转与平移。参考 [2]。

先不加证明地给出一些关于 Dual Quaternion 用于刚体变换的结论（统一习惯，刚体变换是先旋转再平移，即 (mathbf{R}mathbf{X}+mathbf{t})）：

1. (mathbf{r} + epsilon mathbf{d}) 中的“非 Dual 部” (mathbf{r}) 表示旋转。

2. (mathbf{r} + epsilon mathbf{d}) 中的“Dual 部” (mathbf{d}) 与平移量 (mathbf{t}) 的关系是 (mathbf{d}={1 over 2}mathbf{t}mathbf{r})。

3. (mathbf{r} + epsilon mathbf{d}) 的 conjugate 是 (mathbf{r}^* - epsilon mathbf{d}^*)，其中 (mathbf{r}^*, mathbf{d}^*) 分别是四元数的 (mathbf{r}, mathbf{d}) conjugate，四元数的 conjugate 与 inverse 的区别在于前者考虑了四元数的模，在单位四元数的情况，两者是一致的。

4. (mathbf{r} + epsilon mathbf{d}) 对一个三维点 (mathbf{p}) 进行变换可以将这个三维点写到 Dual 部的虚部且旋转部分为单位四元数，即 (1+epsilonmathbf{p})，过程类似 Quaternion 旋转点的过程，((mathbf{r} + epsilon mathbf{d})(1+epsilonmathbf{p})(mathbf{r}^* - epsilon mathbf{d}^*))，计算完成之后取 Dual 部的虚部。

1. 纯平移

在空间中平移一个点 (mathbf{p} = [X, Y, Z]^T) 以 (mathbf{t}) 得到点 (mathbf{p}^{prime})，过程是 (mathbf{p}^{prime} = mathbf{p} + mathbf{t})。

纯平移，无旋转。即表示的旋转的四元数是单位四元数 ([1, 0, 0, 0]^T)。

纯平移的 Dual Quaternion 可以表示为 (1+0i+0j+0k + epsilon{1over2}(mathbf{t}_xi+mathbf{t}_yj+mathbf{t}_zk)(1+0i+0j+0k) =1+epsilon{1over2}(mathbf{t}_xi+mathbf{t}_yj+mathbf{t}_zk) = 1+epsilon{1 over 2}mathbf{t})。

[egin{align} mathbf{p}^{prime} &= [1+epsilon{1over2}(mathbf{t}_xi+mathbf{t}_yj+mathbf{t}_zk)][1+epsilon (Xi+Yj+Zk)] otag \ &{phantom{=}}[1-epsilon{1over2}(-mathbf{t}_xi-mathbf{t}_yj-mathbf{t}_zk)] otag \ &= [1+epsilon Xi+epsilon Yj+epsilon Zk + epsilon{1over2}(mathbf{t}_xi+mathbf{t}_yj+mathbf{t}_zk)] otag \ &{phantom{=}}[1+epsilon{1over2}(mathbf{t}_xi+mathbf{t}_yj+mathbf{t}_zk)] otag \ &= 1+epsilon Xi+epsilon Yj+epsilon Zk + epsilon{1over2}(mathbf{t}_xi+mathbf{t}_yj+mathbf{t}_zk) otag \ &{phantom{=}} + epsilon{1over2}(mathbf{t}_xi+mathbf{t}_yj+mathbf{t}_zk) otag \ &= 1+epsilon[(X+mathbf{t}_x)i+(Y+mathbf{t}_y)j+(Z+mathbf{t}_z)k] end{align}]

于是证明了 (mathbf{d}) 与 (mathbf{t}) 的关系。

2. 纯旋转

在空间中旋转一个点 (mathbf{p} = [X, Y, Z]^T) 以四元数 (mathbf{q}) 得到点 (mathbf{p}^{prime})，过程是 (mathbf{p}^{prime} = mathbf{q}mathbf{p}mathbf{q}^*)。

纯旋转，无平移，则 (mathbf{t} = mathbf{0}, mathbf{d} = {1 over 2}mathbf{t}mathbf{r} = mathbf{0})。Dual Quaternion 可以表示为 (mathbf{r} + epsilon mathbf{0})。

[egin{align} mathbf{p}^{prime} &= (mathbf{r} + epsilon mathbf{0})(1 + epsilon mathbf{p})(mathbf{r}^* - epsilon mathbf{0}^*) otag \ &= mathbf{r} (1 + epsilon mathbf{p}) mathbf{r}^* otag \ &= mathbf{r}mathbf{r}^* + epsilon mathbf{r} mathbf{p} mathbf{r}^* end{align}]

取 Dual 部，结果是 (mathbf{r} mathbf{p} mathbf{r}^*)，之后的过程与普通四元数旋转过程一致，即取虚部的过程。所以纯旋转能够退化成普通四元数的旋转。

3. 先旋转后平移

在空间中先旋转后平移一个点 (mathbf{p} = [X, Y, Z]^T)，旋转量为 (mathbf{q})，平移量为 (mathbf{t})，过程是 (mathbf{p}^{prime} = mathbf{R}{mathbf{q}}mathbf{p} + mathbf{t})。

前面两节已经给出了纯旋转与纯平移的 Dual Quaternion 表示：

1. 纯平移 (1+epsilon{1 over 2}mathbf{t})；

2. 纯旋转 (mathbf{r} + epsilonmathbf{0})。

按照 Dual Quaternion 对点 (mathbf{p}) 的 transform 方式，((mathbf{r} + epsilon mathbf{d})(1+epsilonmathbf{p})(mathbf{r}^* - epsilon mathbf{d}^*))。

先旋转后平移得到的结果是 Dual Quaternion，计算过程如下。

[egin{align} &{phantom{=}}(1+epsilon{1 over 2}mathbf{t})(mathbf{r} + epsilonmathbf{0}) otag \ &= mathbf{r}+epsilon{1 over 2}mathbf{t}mathbf{r} otag \ &= mathbf{r}+epsilonmathbf{d} end{align}]

其中 (mathbf{d}) 是 Dual 部，以上证明了的 Dual 部与平移量的关系。

现在做一个先旋转后平移的过程，以证明 Dual Quaternion (mathbf{r}+epsilon{1 over 2}mathbf{t}mathbf{r}) 可以正确 transform 点 (mathbf{p})。

[egin{align} mathbf{p}^{prime} &= (mathbf{r}+epsilon{1 over 2}mathbf{t}mathbf{r})(1+epsilonmathbf{p})(mathbf{r}^*-epsilon{1 over 2}mathbf{r}^*mathbf{t}^*) otag \ &= (mathbf{r} + epsilon{1 over 2}mathbf{t}mathbf{r} + epsilonmathbf{r}mathbf{p})(mathbf{r}^* - epsilon{1 over 2}mathbf{r}^*mathbf{t}^*) otag \ &= mathbf{r}mathbf{r}^* - epsilon{1 over 2}mathbf{r}mathbf{r}^*mathbf{t}^* + epsilon{1 over 2}mathbf{t}mathbf{r}mathbf{r}^* + epsilonmathbf{r}mathbf{p}mathbf{r}^* otag \ &= mathbf{r}mathbf{r}^* + epsilon(mathbf{r}mathbf{p}mathbf{r}^*+{1 over 2}mathbf{t}mathbf{r}mathbf{r}^*-{1 over 2}mathbf{r}mathbf{r}^*mathbf{t}^*) otag \ &= mathbf{r}mathbf{r}^* + epsilon(mathbf{r}mathbf{p}mathbf{r}^*+{1 over 2}mathbf{t}-{1 over 2}mathbf{t}^*) otag \ &= mathbf{r}mathbf{r}^* + epsilon(mathbf{r}mathbf{p}mathbf{r}^* + mathbf{t}) end{align}]

以上的推导假设了 (mathbf{r}) 的模为 1，即单位四元数，在用四元数的时候最好时常 normalize。

取 Dual 部的虚部 (mathbf{r}mathbf{p}mathbf{r}^* + mathbf{t}) 与正常形式得到的结果一致。

4. 代码

class DualQuaternion {
private:
  Eigen::Quaterniond r_;
  Eigen::Vector3d t_;
  Eigen::Quaterniond d_;
  friend DualQuaternion operator*(const DualQuaternion &_lhs,
                                  const DualQuaternion &_rhs);

public:
  DualQuaternion(const Eigen::Quaterniond &_r, const Eigen::Vector3d &_t)
      : r_{_r}, t_{_t}, 
        d_{Eigen::Quaterniond(0, _t[0] / 2, _t[1] / 2, _t[2] / 2) * _r} {}

  DualQuaternion(const Eigen::Quaterniond &_r)
      : DualQuaternion(_r, Eigen::Vector3d(0, 0, 0)) {}

  DualQuaternion(const Eigen::Vector3d &_t)
      : DualQuaternion(Eigen::Quaterniond(1, 0, 0, 0), _t) {}

  DualQuaternion(const Eigen::Quaterniond &_r, const Eigen::Quaterniond &_d)
      : r_{_r}, d_{_d} {
    const Eigen::Quaterniond qt = this->d_ * this->r_.inverse();
    t_ = Eigen::Vector3d(2 * qt.x(), 2 * qt.y(), 2 * qt.z());
  }

  const Eigen::Quaterniond &r() const { return r_; }

  const Eigen::Quaterniond &d() const { return d_; }

  const Eigen::Vector3d &t() const { return t_; }

  /// Transform inverse.
  DualQuaternion inverse() const {
    return DualQuaternion(this->r_.inverse(), this->r_.inverse() * (-this->t_));
  }

  DualQuaternion conjugate() const {
    const Eigen::Quaterniond d_tmp(-this->d_.w(), this->d_.x(), this->d_.y(), this->d_.z());
    return DualQuaternion(this->r_.inverse(), d_tmp);
  }

  Eigen::Vector3d transformPoint(const Eigen::Vector3d &_p) const {
    const DualQuaternion dp0(Eigen::Quaterniond(1, 0, 0, 0),
                             Eigen::Quaterniond(0, _p[0], _p[1], _p[2]));
    const DualQuaternion dp1 = (*this) * dp0 * (this->conjugate());
    return Eigen::Vector3d(dp1.d_.x(), dp1.d_.y(), dp1.d_.z());
  }
};

DualQuaternion operator*(const DualQuaternion &_lhs,
                         const DualQuaternion &_rhs) {
  const Eigen::Quaterniond r = (_lhs.r_ * _rhs.r_).normalized();
  const Eigen::Quaterniond tmp0 = _lhs.r_ * _rhs.d_;
  const Eigen::Quaterniond tmp1 = _lhs.d_ * _rhs.r_;
  const Eigen::Quaterniond qd(tmp0.w() + tmp1.w(), tmp0.x() + tmp1.x(),
                              tmp0.y() + tmp1.y(), tmp0.z() + tmp1.z());
  const Eigen::Quaterniond qt = qd * r.inverse();

  const Eigen::Vector3d t(2 * qt.x(), 2 * qt.y(), 2 * qt.z());

  return DualQuaternion(r, t);
}

Reference

[1] Kenwright, Ben. "A beginners guide to dual-quaternions: what they are, how they work, and how to use them for 3D character hierarchies." (2012).

[2] Maths - Dual Quaternions - Martin Baker.