拟合三维空间一组点云到另一组点云的刚体变换之一：梯度下降

假设对应关系已知，拟合求得一组三维点云到另一组三维点云的刚体变换矩阵。

下面介绍的方法比较自娱自乐，更高效的方法可以参考这里。

问题分析

所谓的刚体变换，是线性变换的一种特例，它在变换前后不改变任意两点之间的相互距离，可以认为是一种没有图像扭曲的变换。

三维空间中点和向量的线性变换可以由一个4*4的具有12个自由元的仿射变换矩阵表示；刚体变换则在此基础上有一些额外的要求。这里要解决的问题是，求得一组三维点云到另一组三维点云的一个最接近的刚体变换矩阵。我们假设点的对应关系是已知的，即是由两个4*V的三维坐标矩阵P和Q，求一个最接近的4*4的刚体变换矩阵A，使得P中的点能够映射得到Q中的点，即

$AP \rightarrow Q$

这里点采用齐次坐标表示，即P和Q的前3行分别为XYZ坐标，而第4行为1表示顶点；A等效于一个3*3的旋转变换矩阵加上平移变换，即

$A=\begin{bmatrix} & & & tx\\ & R_zR_yR_x & & ty\\ & & & tz\\ 0 & 0 & 0 & 1 \end{bmatrix}$
而三维空间中任意旋转变换矩阵的具体表示如下（来源见水印），可以看出是绕x、y、z轴的三个矩阵的乘积。~~但愿这哥们没有乘错，否则后面的推导就阿西吧了。~~

可以看出刚体变换矩阵包含了6个变量——yaw、pitch、roll、tx、ty、tz。接下来，我们可以通过常规的数值优化方法求得这6个变量。

能量

考察P经过刚体变换A后，点坐标与Q中对应点坐标的差异，我们可以定义一个能量用于优化。例如，取坐标差的平方和作为能量时，记 $P_i=[p_i^x, p_i^y,p_i^z, 1]^T$ 和 $Q_i=[q_i^x, q_i^y,q_i^z, 1]^T$ 为对应矩阵的第i列，则我们可以定义

$E=\sum_{i}\left \| AP_i-Q_i \right \|^2$

从元素角度来讲，记 $AP_i = [u_i, v_i, w_i, 1]^T$ ，那么可以得到

$E=\sum_{i}((u_i - q_i^x)^2 + (v_i - q_i^y)^2 + (w_i - q_i^z)^2)$

展开可以得到

$\begin{aligned} u_i &= p_i^x[cos(yaw)cos(pitch)]+p_i^y[cos(yaw)sin(pitch)sin(roll) -sin(yaw)cos(roll)]+p_i^z[cos(yaw)sin(pitch)cos(roll) + sin(yaw)sin(roll)] + tx \\ v_i &= p_i^x[sin(yaw)cos(pitch)]+p_i^y[sin(yaw)sin(pitch)sin(roll) + cos(yaw)cos(roll)]+p_i^z[sin(yaw)sin(pitch)cos(roll) - cos(yaw)sin(roll)]+ty\\ w_i &= p_i^x[-sin(pitch)] + p_i^y[cos(pitch)sin(roll)]+p_i^z[cos(pitch)cos(roll)]+tz \end{aligned}$

梯度

接下来我们对能量E分别求它对6个变量——yaw、pitch、roll、tx、ty、tz——的偏导，从而得到取倒数的负方向作为梯度 $\nabla _E$ 。

为此，我们先计算u、v、w对这6个变量共计18个偏导，因为根据链式求导法则，这是迟早的事情。这里中间只涉及到了sin和cos的求导，即是由

$\begin{aligned} \frac{dsinx}{dx} &= cosx \\ \frac{dcosx}{dx} &= -sinx \end{aligned}$

可以得到下列18个偏导数（滑稽）：

$\begin{aligned} \frac{\partial u_i}{\partial roll} &= p_i^y[cos(yaw)sin(pitch)cos(roll) + sin(yaw)sin(roll)] + p_i^z[-cos(yaw)sin(pitch)sin(roll) + sin(yaw)cos(roll)] \\ \frac{\partial u_i}{\partial pitch} &= p_i^x[-cos(yaw)sin(pitch)] + p_i^y[cos(yaw)cos(pitch)sin(roll)] + p_i^z[cos(yaw)cos(pitch)cos(roll)] \\ \frac{\partial u_i}{\partial yaw} &= p_i^x[-sin(yaw)cos(pitch)] + p_i^y[-sin(yaw)sin(pitch)sin(roll) - cos(yaw)cos(roll)] + p_i^z[-sin(yaw)sin(pitch)cos(roll) + cos(yaw)sin(roll)] \\ \frac{\partial u_i}{\partial tx} &= 1 \\ \frac{\partial u_i}{\partial ty} &= \frac{\partial u_i}{\partial tz} = 0 \\ \end{aligned}$

$\begin{aligned} \frac{\partial v_i}{\partial roll} &= p_i^y[sin(yaw)sin(pitch)cos(roll) - cos(yaw)sin(roll)] + p_i^z[-sin(yaw)sin(pitch)sin(roll) - cos(yaw)cos(roll)] \\ \frac{\partial v_i}{\partial pitch} &= p_i^x[-sin(yaw)sin(pitch)] + p_i^y[sin(yaw)cos(pitch)sin(roll)] + p_i^z[sin(yaw)cos(pitch)cos(roll)] \\ \frac{\partial v_i}{\partial yaw} &= p_i^x[cos(yaw)cos(pitch)] + p_i^y[cos(yaw)sin(pitch)sin(roll) - sin(yaw)cos(roll)] + p_i^z[cos(yaw)sin(pitch)cos(roll) + sin(yaw)sin(roll)] \\ \frac{\partial v_i}{\partial ty} &= 1 \\ \frac{\partial v_i}{\partial tx} &= \frac{\partial v_i}{\partial tz} = 0 \\ \end{aligned}$

$\begin{aligned} \frac{\partial w_i}{\partial roll} &= p_i^y[cos(pitch)cos(roll)] + p_i^z[-cos(pitch)sin(roll)] \\ \frac{\partial w_i}{\partial pitch} &= p_i^x[-cos(pitch)] + p_i^y[-sin(pitch)sin(roll)] + p_i^z[-sin(pitch)cos(roll)] \\ \frac{\partial w_i}{\partial yaw} &= 0 \\ \frac{\partial w_i}{\partial tz} &= 1 \\ \frac{\partial w_i}{\partial tx} &= \frac{\partial w_i}{\partial ty} = 0 \\ \end{aligned}$

于是乎，得到上面18个偏导后，能量E对6个变量偏导，根据链式求导规则可统一记为

$\frac{\partial E}{\partial \square } = \sum_{i}(2(u_i - p_i^x)\frac{\partial u_i}{\partial \square} + 2(v_i - p_i^y)\frac{\partial v_i}{\partial \square} + 2(w_i - p_i^z)\frac{\partial w_i}{\partial \square}), \square \in \{ roll, pitch, yaw, tx, ty, tz \}$

这个式子也就是能量对刚体变换6个变量——yaw、pitch、roll、tx、ty、tz——的梯度了。取梯度负方向的迭代更新6个变量，即可通过简单的梯度下降法求得P到Q最接近的刚体变换。