3D变换函数求导

文章目录

前言
一、3D变换函数求导

前言

矩阵求导

一、3D变换函数求导

1欧拉角旋转完整版本

3D变换函数表示为：

$T_E(\mathbf{p}_6, \mathbf{x}) = R_x R_y R_z \mathbf{x} + \mathbf{t}$

其中 $\mathbf{p}_6 = [t_x, t_y, t_z, \phi_x, \phi_y, \phi_z]^T$ ， $\mathbf{t} = [t_x, t_y, t_z]^T$ 是平移向量， $R_x$ 、 $R_y$ 、 $R_z$ 是旋转矩阵。
（1） 绕 $\ x$ 轴的旋转矩阵 $R_x$ ：
$R_x = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \phi_x & -\sin \phi_x \\ 0 & \sin \phi_x & \cos \phi_x \end{bmatrix}$

（2） 绕 $\ y$ 轴的旋转矩阵 $R_y$ ：
$R_y = \begin{bmatrix} \cos \phi_y & 0 & \sin \phi_y \\ 0 & 1 & 0 \\ -\sin \phi_y & 0 & \cos \phi_y \end{bmatrix}$

（3） 绕 $\ z$ 轴的旋转矩阵 $R_z$ ：
$R_z = \begin{bmatrix} \cos \phi_z & -\sin \phi_z & 0 \\ \sin \phi_z & \cos \phi_z & 0 \\ 0 & 0 & 1 \end{bmatrix}$

结合这些旋转矩阵，3D变换函数的表达式为：

$T_E(\mathbf{p}_6, \mathbf{x}) = R_x R_y R_z \mathbf{x} + \mathbf{t}$

雅可比矩阵 $J_E$

雅可比矩阵的每个元素是变换函数对每个参数的偏导数。

平移部分

对于平移参数 $t_x, t_y, t_z$ ：

$\frac{\partial T_E}{\partial t_x} = \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}, \quad \frac{\partial T_E}{\partial t_y} = \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix}, \quad \frac{\partial T_E}{\partial t_z} = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}$

旋转部分

对于旋转角度 $\phi_x, \phi_y, \phi_z$ ：

根据文中给出的公式，雅可比矩阵的形式为：

$J_E = \begin{bmatrix} 1 & 0 & 0 & 0 & c & f \\ 0 & 1 & 0 & a & d & g \\ 0 & 0 & 1 & b & e & h \end{bmatrix}$

其中：

$a = x_1(-s_x s_z + c_x s_y c_z) + x_2(-s_x c_z - c_x s_y s_z) + x_3(-c_x c_y)$
$b = x_1(c_x s_z + s_x s_y c_z) + x_2(-s_x s_y s_z + c_x c_z) + x_3(-s_x c_y)$
$c = x_1(-s_y c_z) + x_2(s_y s_z) + x_3(c_y)$
$d = x_1(s_x c_y c_z) + x_2(-s_x c_y s_z) + x_3(s_x s_y)$
$e = x_1(-c_x c_y c_z) + x_2(c_x c_y s_z) + x_3(-c_x s_y)$
$f = x_1(-c_y s_z) + x_2(-c_y c_z)$
$g = x_1(c_x c_z - s_x s_y s_z) + x_2(-c_x s_z - s_x s_y c_z)$
$h = x_1(s_x c_z + c_x s_y s_z) + x_2(c_x s_y c_z - s_x s_z)$

海森矩阵 $H_E$

海森矩阵的元素是雅可比矩阵的导数。根据文中的描述，海森矩阵的形式如下：

$H_E = \begin{bmatrix} \begin{array}{cccccc} \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{a} & \mathbf{b} & \mathbf{c} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{b} & \mathbf{d} & \mathbf{e} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{c} & \mathbf{e} & \mathbf{f} \end{array} \end{bmatrix}$

其中：

$\mathbf{a} = \begin{bmatrix} 0 \\ x_1(-c_x s_z - s_x s_y c_z) + x_2(-c_x c_z + s_x s_y s_z) + x_3(s_x c_y) \\ x_1(-s_x s_z + c_x s_y c_z) + x_2(-s_x s_y s_z - c_x c_z) + x_3(-c_x c_y) \end{bmatrix}$
$\mathbf{b} = \begin{bmatrix} 0 \\ x_1(c_x c_y c_z) + x_2(-c_x c_y s_z) + x_3(c_x s_y) \\ x_1(s_x c_y c_z) + x_2(-s_x c_y s_z) + x_3(s_x s_y) \end{bmatrix}$
$\mathbf{c} = \begin{bmatrix} 0 \\ x_1(-s_x c_z - c_x s_y s_z) + x_2(-s_x s_z - s_x s_y c_z) \\ x_1(c_x c_z - s_x s_y s_z) + x_2(-s_x s_y c_z - s_x s_z) \end{bmatrix}$
$\mathbf{d} = \begin{bmatrix} x_1(-c_y c_z) + x_2(c_y s_z) + x_3(-s_y) \\ x_1(-s_x s_y c_z) + x_2(s_x s_y s_z) + x_3(s_x c_y) \\ x_1(c_x s_y c_z) + x_2(-c_x s_y s_z) + x_3(-c_x c_y) \end{bmatrix}$
$\mathbf{e} = \begin{bmatrix} x_1(s_y s_z) + x_2(s_y c_z) \\ x_1(-s_x c_y s_z) + x_2(-s_x c_y c_z) \\ x_1(c_x c_y s_z) + x_2(c_x c_y c_z) \end{bmatrix}$
$\mathbf{f} = \begin{bmatrix} x_1(-c_y c_z) + x_2(c_y s_z) \\ x_1(-c_x s_z - s_x s_y c_z) + x_2(-c_x c_z + s_x s_y s_z) \\ x_1(-s_x s_z + c_x s_y c_z) + x_2(-c_x s_y s_z - s_x s_z) \end{bmatrix}$

2. 小角度近似的欧拉旋转

使用欧拉角序列 $\ z-y-x$ 和小角度近似，3D 变换 $\ T_E(\mathbf{p}_6, \mathbf{x})$ 可近似为：

$T_E(\mathbf{p}_6, \mathbf{x}) = \begin{bmatrix} c_y c_z & -c_y s_z & s_y \\ c_x s_z + s_x s_y c_z & c_x c_z - s_x s_y s_z & -s_x c_y \\ s_x s_z - c_x s_y c_z & s_x s_y s_z + c_x c_z & c_x c_y \end{bmatrix} \mathbf{x} + \begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix}$

在小角度近似下，变换函数简化为：

$\tilde{T}_E(\mathbf{p}_6, \mathbf{x}) = \begin{bmatrix} 1 & -\phi_z & \phi_y \\ \phi_z & 1 & -\phi_x \\ -\phi_y & \phi_x & 1 \end{bmatrix} \mathbf{x} + \begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix}$

解释：在小角度近似下，欧拉旋转的变换函数中，角度的正弦和余弦可以用角度本身代替，这简化了计算，更易于求导。

雅可比矩阵

$\tilde{J}_E = \begin{bmatrix} 1 & 0 & 0 & 0 & x_3 & -x_2 \\ 0 & 1 & 0 & -x_3 & 0 & x_1 \\ 0 & 0 & 1 & x_2 & -x_1 & 0 \end{bmatrix}$

解释：雅可比矩阵的每一列对应于变换参数对输出的偏导数，表示输入参数对输出的影响。

二阶导数

$\frac{\partial^2 \mathbf{x'}}{\partial p_i \partial p_j} = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}$

解释：所有二阶偏导数均为零，表示在小角度近似下，变换的非线性影响被忽略。

3. 轴/角旋转

使用轴/角表示法会导致七维的优化问题：三个平移参数、三个旋转轴参数和一个旋转角度。变换函数为：

$T_A(\mathbf{p}_7, \mathbf{x}) = \begin{bmatrix} t_r^2 + c & t_r t_y t_r - s t_z & t_r t_z + s t_y \\ t_r t_y t_r + s t_z & t_r^2 + c & -s t_x \\ t_r t_z - s t_y & t_r t_y + s t_x & t_r^2 + c \end{bmatrix} \mathbf{x} + \begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix}$

其中 $\ \mathbf{p}_7 = [\mathbf{t} | \mathbf{r} | \phi]^T$ ， $\ \mathbf{t} = [t_x, t_y, t_z]^T$ 是平移， $\ \mathbf{r} = [r_x, r_y, r_z]^T$ 是旋转轴， $\ s = \sin \phi$ ， $\ c = \cos \phi$ ， $\ t = 1 - \cos \phi$ 。

解释：使用轴/角表示法，变换函数将平移和旋转结合在一起，形成一个复杂的变换矩阵。

雅可比矩阵

$J_A = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ t(2r_x x_1 + r_y x_2 + r_z x_3) & t r_y x_1 - s x_3 & t r_z x_1 + s x_2 \\ t r_x x_2 + s x_3 & t(r_x x_1 + 2 r_y x_2 + r_z x_3) & t r_z x_2 - s x_1 \\ t r_x x_3 - s x_2 & t r_y x_3 + s x_1 & t(r_x x_1 + r_y x_2 + 2 r_z x_3) \end{bmatrix}$

解释：雅可比矩阵用于计算变换参数对输出的影响，包含了平移和旋转的复杂关系。

海森矩阵

$H_A = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \mathbf{a} & \mathbf{b} & \mathbf{c} & \mathbf{d} \\ 0 & 0 & 0 & \mathbf{b} & \mathbf{e} & \mathbf{f} & \mathbf{g} \\ 0 & 0 & 0 & \mathbf{c} & \mathbf{f} & \mathbf{b} & \mathbf{i} \\ 0 & 0 & 0 & \mathbf{d} & \mathbf{g} & \mathbf{j} & \mathbf{j} \end{bmatrix}$

海森矩阵元素

元素 $\ \mathbf{a}$ ：
$\mathbf{a} = \begin{bmatrix} 2t_x x_1 \\ 0 \\ 0 \end{bmatrix}$
元素 $\ \mathbf{b}$ ：
$\mathbf{b} = \begin{bmatrix} t_x x_2 \\ t_x x_1 \\ 0 \end{bmatrix}$
元素 $\ \mathbf{c}$ ：
$\mathbf{c} = \begin{bmatrix} t_x x_3 \\ 0 \\ t_x x_1 \end{bmatrix}$
元素 $\ \mathbf{d}$ ：
$\mathbf{d} = \begin{bmatrix} s(2r_x x_1 + r_y x_2 + r_z x_3) \\ s r_y x_1 - c x_3 \\ s r_z x_1 + c x_2 \end{bmatrix}$
元素 $\ \mathbf{e}$ ：
$\mathbf{e} = \begin{bmatrix} 0 \\ 2t_x x_2 \\ 0 \end{bmatrix}$
元素 $\ \mathbf{f}$ ：
$\mathbf{f} = \begin{bmatrix} 0 \\ t_x x_3 \\ t_x x_2 \end{bmatrix}$
元素 $\ \mathbf{g}$ ：
$\mathbf{g} = \begin{bmatrix} s r_x x_2 + c x_3 \\ s(r_x x_1 + 2 r_y x_2 + r_z x_3) \\ s r_z x_2 - c x_1 \end{bmatrix}$
元素 $\ \mathbf{h}$ ：
$\mathbf{h} = \begin{bmatrix} 0 \\ 0 \\ 2t_x x_3 \end{bmatrix}$
元素 $\ \mathbf{i}$ ：
$\mathbf{i} = \begin{bmatrix} s r_x x_3 - c x_2 \\ s r_y x_3 + c x_1 \\ s(r_x x_1 + r_y x_2 + 2 r_z x_3) \end{bmatrix}$
元素 $\ \mathbf{j}$ ：
$\mathbf{j} = \begin{bmatrix} c A + s B \\ c C + s D \\ c E + s F \end{bmatrix}$

解释：这些元素代表了海森矩阵的各个部分，描述了不同参数对变换的二阶影响，帮助优化算法更有效地收敛和调整。

文章目录

前言

一、3D变换函数求导

1欧拉角旋转完整版本

雅可比矩阵 J E J_E JE​

平移部分

旋转部分

海森矩阵 H E H_E HE​

2. 小角度近似的欧拉旋转

雅可比矩阵

二阶导数

3. 轴/角旋转

雅可比矩阵

海森矩阵

海森矩阵元素

猜你喜欢

雅可比矩阵 $J_E$

海森矩阵 $H_E$