参考文献: Closed-form solution of absolute orientation using unit quaternions

在 SLAM 中, 轨迹会因为因为起始位姿、尺度设定的不同而不同

在轨迹形状大致相同时, 可以求取一个相似变换使得两个轨迹尽可能重合

给定两个分别具有 $n$ 个空间点的轨迹, 分别定义为:
$$r_l\in {\mathbb{R}}^{n\times 3},r_r\in {\mathbb{R}}^{n\times 3}$$

有相似变换 $\text{Sim3}(sR,t)$ 使得下式的和方差最小, 并使轨迹两个轨迹对齐:
$$\text{SSE}=\sum _{i=1}^n||sR(r_{l,i})+t-r_{r,i}|{|}^2$$

平移向量

首先，计算两个轨迹的质心，并分别对两个轨迹零均值化：
$$\begin{gathered} r_l^{\prime }=r_l-\overline{r_l},\overline{r_l}\in {\mathbb{R}}^3 \\ {r}_r^{\prime }=r_r-\overline{r_r},\overline{r_r}\in {\mathbb{R}}^3 \\ {t}^{\prime }=t+sR(\overline{r_l})-\overline{r_r} \\ \text{SSE}=\sum _{i=1}^n||sR(r_{l,i}^{\prime })+t^{\prime }-r_{r,i}^{\prime }|{|}^2 \\ =\sum _{i=1}^n||sR(r_{l,i}^{\prime })-r_{r,i}^{\prime }|{|}^2+2t^{\prime }\sum _{i=1}^n(sR(r_{l,i}^{\prime })-r_{r,i}^{\prime })+n||t^{\prime }|{|}^2 \end{gathered}$$

零均值点经过线性变换后，其均值依然为零，即 ${\sum }_{i=1}^{n}sR(r_{l,i}^{\prime })=0$, 所以第二项为 0:
$$\text{SSE}=\sum _{i=1}^n||sR(r_{l,i}^{\prime })-r_{r,i}^{\prime }|{|}^2+n||t^{\prime }|{|}^2$$

第一项只与 $s,R$ 有关, 第二项只与 $t^{\prime }$ 有关。令第二项为 0, 解得平移向量:
$$t=\overline{r_r}-sR(\overline{r_l})$$

其中只有 $s,R$ 是未知量, 后续代入数值即可

尺度因子

接上文, 继续对 $\text{SSE}$ 进行简化和分解 (点经旋转后模长不变, 即 $||R(r)|{|}^2=||r|{|}^2$):
$$\begin{gathered} S_l=\sum _{i=1}^n||R(r_{l,i}^{\prime })|{|}^2=\sum _{i=1}^n||r_{l,i}^{\prime }|{|}^2 \\ {S}_r=\sum _{i=1}^n||r_{r,i}^{\prime }|{|}^2 \\ D=\sum _{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime }) \\ \text{SSE}=\sum _{i=1}^n||sR(r_{l,i}^{\prime })-r_{r,i}^{\prime }|{|}^2=s^2{S}_l-2sD+S_r \\ =s(s{S}_l-2D+\frac{1}{s}{S}_r)=s(\sqrt{s{S}_l}-\sqrt{\frac{1}{s}{S}_r}{)}^2+2s(\sqrt{S_l{S}_r}-D) \end{gathered}$$

令第一项 $\sqrt{s{S}_l}-\sqrt{\frac{1}{s}{S}_r}=0$, 可解得尺度因子 (表达式中所有数值均为已知量):
$$s=\sqrt{\frac{S_r}{S_l}}=\sqrt{\frac{{\sum }_{i=1}^n||r_{r,i}^{\prime }|{|}^2}{{\sum }_{i=1}^n||r_{l,i}^{\prime }|{|}^2}}$$

单位四元数

由于 $S_l,S_r$ 已知为定值, 最小化 $\text{SSE}$ 等价于最大化 $D$:
$$\text{maximize}:D=\sum _{i=1}^n{r}_{r,i}^{\prime }\cdot R(r_{l,i}^{\prime })$$

为了解决这个问题, 引入四元数进行化简, 首先利用 $\dot{q},\dot{r}\in {\mathbb{R}}^{4\times 1}$ 对部分性质进行说明:
$$\begin{gathered} \dot{q}=\left[\begin{array}{cccc}w& x& y& z\end{array}\right] \\ {\dot{q}}^{\ast }=\left[\begin{array}{cccc}w& -x& -y& -z\end{array}\right] \end{gathered}$$

两个四元数之间的相乘, 可以等价为一个 4×4 矩阵和一个长度为 4 的向量之间的叉乘:
$$\begin{gathered} \dot{q}\dot{r}=Q\dot{r}=\left[\begin{array}{cccc}w& -x& -y& -z\\ x& w& -z& y\\ y& z& w& -x\\ z& -y& x& w\end{array}\right]\dot{r} \\ \dot{r}\dot{q}=\bar{Q}\dot{r}=\left[\begin{array}{cccc}w& -x& -y& -z\\ x& w& z& -y\\ y& -z& w& x\\ z& y& -x& w\end{array}\right]\dot{r} \end{gathered}$$

$Q,\bar{Q}$ 为正交矩阵, 四元数 $\dot{q}$ 的共轭 ${\dot{q}}^{\ast }$ 则分别对应 $Q^T,{\bar{Q}}^T$:
$$\dot{q}\dot{r}{\dot{q}}^{\ast }=(Q\dot{r}){\dot{q}}^{\ast }={\bar{Q}}^{T}Q\dot{r}=\left[\begin{array}{cccc}{A}_{11}& & & \\ & A_{22}& A_{23}& A_{24}\\ & A_{32}& A_{33}& A_{34}\\ & A_{42}& A_{43}& A_{44}\end{array}\right]\dot{r}$$

如果四元数 $\dot{r}$ 为纯虚数 (即 $w=0$, 看作空间点), 则 $\dot{q}\dot{r}{\dot{q}}^{\ast }$ 也为纯虚数 (看作旋转), 将 $D$ 等价为:
$$\begin{gathered} \text{maximize}:D=\sum _{i=1}^n\dot{q}{\dot{r}}_{l,i}{\dot{q}}^{\ast }\cdot {\dot{r}}_{r,i}=\sum _{i=1}^n({\bar{Q}}^{T}Q{\dot{r}}_{l,i}{)}^T{\dot{r}}_{r,i} \\ =\sum _{i=1}^n({\dot{r}}_{l,i}^T{Q}^T)(\bar{Q}{\dot{r}}_{r,i})=\sum _{i=1}^n({\bar{R}}_{l,i}\dot{q}{)}^T(R_{r,i}\dot{q}) \\ ={\dot{q}}^T(\sum _{i=1}^n{\bar{R}}_{l,i}^T{R}_{r,i})\dot{q}={\dot{q}}^{T}N\dot{q} \end{gathered}$$

为了更快地计算 $N$, 定义查找表:
$$\begin{gathered} \text{LUT}=\left[\begin{array}{ccc}{S}_{xx}& S_{xy}& S_{xz}\\ S_{yx}& S_{yy}& S_{yz}\\ S_{zx}& S_{zy}& S_{zz}\end{array}\right] \\ {S}_{xx}=\sum _{i=1}^n{x}_{l,i}^{\prime }{x}_{r,i}^{\prime },S_{xy}=\sum _{i=1}^n{x}_{l,i}^{\prime }{y}_{r,i}^{\prime } \\ N=\sum _{i=1}^n{\bar{R}}_{l,i}^T{R}_{r,i}=\left[\begin{array}{cccc}{S}_{xx}+S_{yy}+S_{zz}& S_{yz}-S_{zy}& S_{zx}-S_{xz}& S_{xy}-S_{yx}\\ S_{yz}-S_{zy}& S_{xx}-S_{yy}-S_{zz}& S_{xy}+S_{yx}& S_{zx}+S_{xz}\\ S_{zx}-S_{xz}& S_{xy}+S_{yx}& -S_{xx}+S_{yy}-S_{zz}& S_{yz}+S_{zy}\\ S_{xy}-S_{yx}& S_{zx}+S_{xz}& S_{yz}+S_{zy}& -S_{xx}-S_{yy}+S_{zz}\end{array}\right] \end{gathered}$$

$N$ 是个对称阵, 在特征值分解后可以得到 4 个特征向量 (单位向量), 而且:
$$x^{T}Nx=\lambda$$

所以, 最大特征值所对应的特征向量即为使 $D$ 最大的单位四元数 $\dot{q}$, 根据以下公式可求得旋转矩阵:
$$R=({\bar{Q}}^{T}Q{)}_{2:4,2:4}$$

C++ 实现

Sophus::Sim3f align_trajectory(const std::vector<Eigen::Vector3f> &pts1,
                               const std::vector<Eigen::Vector3f> &pts2) {
  assert(pts1.size() == pts2.size());
  int n = pts1.size();
  // 计算质心
  Eigen::Vector3d centroid1 = Eigen::Vector3d::Zero(), centroid2 = Eigen::Vector3d::Zero();
  for (int i = 0; i < n; ++i) {
    Eigen::Vector3d p1 = pts1[i].cast<double>(), p2 = pts2[i].cast<double>();
    centroid1 += p1;
    centroid2 += p2;
  }
  centroid1 /= n;
  centroid2 /= n;
  // 零均值化, 计算尺度因子、lut
  Eigen::Matrix3<long double> lut = Eigen::Matrix3<long double>::Zero();
  long double Sl = 0, Sr = 0;
  for (int i = 0; i < n; ++i) {
    Eigen::Vector3d p1 = pts1[i].cast<double>() - centroid1, p2 = pts2[i].cast<double>() - centroid2;
    // 如果 pts 数值较大, 可能会溢出
    lut += (p1 * p2.transpose()).cast<long double>();
    Sl += p1.squaredNorm();
    Sr += p2.squaredNorm();
  }
  lut /= n;
  double Sxx = lut(0, 0), Sxy = lut(0, 1), Sxz = lut(0, 2),
      Syx = lut(1, 0), Syy = lut(1, 1), Syz = lut(1, 2),
      Szx = lut(2, 0), Szy = lut(2, 1), Szz = lut(2, 2);
  double s = std::sqrt(Sr / Sl);
  // 单位四元数
  Eigen::Matrix4d N;
  N << (Sxx + Syy + Szz) / 2, Syz - Szy, Szx - Sxz, Sxy - Syx,
      0, (Sxx - Syy - Szz) / 2, Syx + Sxy, Szx + Sxz,
      0, 0, (-Sxx + Syy - Szz) / 2, Szy + Syz,
      0, 0, 0, (-Sxx - Syy + Szz) / 2;
  N += N.transpose().eval();
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eig(N);
  Eigen::Vector4d q = eig.eigenvectors().col(3);
  // 代入数值
  float w = q[0], x = q[1], y = q[2], z = q[3];
  Eigen::Matrix3f R;
  R << w * w + x * x - y * y - z * z, -2 * w * z + 2 * x * y, 2 * w * y + 2 * x * z,
      2 * w * z + 2 * x * y, w * w - x * x + y * y - z * z, -2 * w * x + 2 * y * z,
      -2 * w * y + 2 * x * z, 2 * w * x + 2 * y * z, w * w - x * x - y * y + z * z;
  Eigen::Vector3f t = centroid2.cast<float>() - s * R * centroid1.cast<float>();
  return Sophus::Sim3f(s, Eigen::Quaternionf(R), t);
}

Python 实现

import numpy as np
from scipy.spatial import transform

SO3 = transform.Rotation  # 特殊正交群


def align_trajectory(pts1: np.ndarray, pts2: np.ndarray):
    """ Closed-form solution of absolute orientation using unit quaternions """
    assert pts1.ndim == 2 and pts1.shape[1] == 3 and pts1.shape == pts2.shape
    pts1, pts2 = map(np.float64, (pts1, pts2))
    # 尺度因子
    centroid1, centroid2 = pts1.mean(axis=0), pts2.mean(axis=0)
    pts1, pts2 = pts1 - centroid1, pts2 - centroid2
    s = np.sqrt(np.square(pts2).sum() / np.square(pts1).sum())
    # 单位四元数
    Sxx, Sxy, Sxz, Syx, Syy, Syz, Szx, Szy, Szz = (pts1.T @ pts2).flatten()
    sumn = np.array([[(Sxx + Syy + Szz) / 2, Syz - Szy, Szx - Sxz, Sxy - Syx],
                     [0, (Sxx - Syy - Szz) / 2, Syx + Sxy, Szx + Sxz],
                     [0, 0, (- Sxx + Syy - Szz) / 2, Szy + Syz],
                     [0, 0, 0, (- Sxx - Syy + Szz) / 2]])
    sumn += sumn.T
    eig_val, eig_vec = np.linalg.eig(sumn)
    w, x, y, z = eig_vec[:, eig_val.argmax()]
    # 代入数值
    R = SO3.from_matrix(
        [[w ** 2 + x ** 2 - y ** 2 - z ** 2, -2 * w * z + 2 * x * y, 2 * w * y + 2 * x * z],
         [2 * w * z + 2 * x * y, w ** 2 - x ** 2 + y ** 2 - z ** 2, -2 * w * x + 2 * y * z],
         [-2 * w * y + 2 * x * z, 2 * w * x + 2 * y * z, w ** 2 - x ** 2 - y ** 2 + z ** 2]]
    )
    t = centroid2 - s * R.as_matrix() @ centroid1
    return s, R, t


if __name__ == "__main__":
    s = 2.5
    R = SO3.from_quat([0.8006, 0.1601, 0.3202, 0.4804]).as_matrix()
    t = np.array([0.1, 0.2, 0.3])

    pts1 = np.random.uniform(-1, 1, (100, 3))
    pts2 = (s * R @ pts1.T).T + t
    s, R, t = align_trajectory(pts1, pts2)

    e = (s * R.as_matrix() @ pts1.T).T + t - pts2
    print(np.square(e).sum())
    print(s, R.as_quat(), t)