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}\)）：
-
\(\mathbf{r} + \epsilon \mathbf{d}\) 中的“非 Dual 部” \(\mathbf{r}\) 表示旋转。
-
\(\mathbf{r} + \epsilon \mathbf{d}\) 中的“Dual 部” \(\mathbf{d}\) 与平移量 \(\mathbf{t}\) 的关系是 \(\mathbf{d}={1 \over 2}\mathbf{t}\mathbf{r}\)。
-
\(\mathbf{r} + \epsilon \mathbf{d}\) 的 conjugate 是 \(\mathbf{r}^* - \epsilon \mathbf{d}^*\)，其中 \(\mathbf{r}^*, \mathbf{d}^*\) 分别是四元数的 \(\mathbf{r}, \mathbf{d}\) conjugate，四元数的 conjugate 与 inverse 的区别在于前者考虑了四元数的模，在单位四元数的情况，两者是一致的。
-
\(\mathbf{r} + \epsilon \mathbf{d}\) 对一个三维点 \(\mathbf{p}\) 进行变换可以将这个三维点写到 Dual 部的虚部且旋转部分为单位四元数，即 \(1+\epsilon\mathbf{p}\)，过程类似 Quaternion 旋转点的过程，\((\mathbf{r} + \epsilon \mathbf{d})(1+\epsilon\mathbf{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{1\over2}(\mathbf{t}_xi+\mathbf{t}_yj+\mathbf{t}_zk)(1+0i+0j+0k) =1+\epsilon{1\over2}(\mathbf{t}_xi+\mathbf{t}_yj+\mathbf{t}_zk) = 1+\epsilon{1 \over 2}\mathbf{t}\)。
于是证明了 \(\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}\)。
取 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+\epsilon{1 \over 2}\mathbf{t}\)；
-
纯旋转 \(\mathbf{r} + \epsilon\mathbf{0}\)。
按照 Dual Quaternion 对点 \(\mathbf{p}\) 的 transform 方式，\((\mathbf{r} + \epsilon \mathbf{d})(1+\epsilon\mathbf{p})(\mathbf{r}^* - \epsilon \mathbf{d}^*)\)。
先旋转后平移得到的结果是 Dual Quaternion，计算过程如下。
其中 \(\mathbf{d}\) 是 Dual 部，以上证明了的 Dual 部与平移量的关系。
现在做一个先旋转后平移的过程，以证明 Dual Quaternion \(\mathbf{r}+\epsilon{1 \over 2}\mathbf{t}\mathbf{r}\) 可以正确 transform 点 \(\mathbf{p}\)。
以上的推导假设了 \(\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);
}
