shikang999 – 第 3 页 – MathSword数值计算软件

盐水浓度与密度关系

2023年6月2日2023年11月26日 shikang999 评论

1 背景
NaCl用途广泛,对于不同浓度的NaCl溶液,其密度有所差异。有时我们需要知道NaCl溶液浓度与密度的对应关系。这时就需要找到一个数学模型来描述浓度与密度之间的关系。

而浓度的描述有两种：一种是溶液质量百分比浓度 $\bar{c}$ ，这是一个无量纲参数，其单位为%；一种是摩尔浓度 $c$ ，单位为 $mol/m^3$ 。这两种浓度有如下关系
(1-1)
$c=1000\times \rho \dfrac{\bar{c}}{100}\dfrac{1}{\alpha}=\dfrac{10\rho\bar{c}}{\alpha}$
公式里 $\rho$ 为溶液密度，单位为 $kg/m^3$ ； $\alpha$ 为溶质的摩尔质量，单位为 $g/mol$ ，对于NaCl，摩尔质量为58.44246928 $g/mol$ 。

2 理论

约定：在接下来的描述中，约定物理量单位为：密度单位为 $kg/m^3$ ，质量单位为 $kg$ ，体积单位为 $m^3$ ，百分比浓度单位为%，摩尔浓度单位为 $mol/m^3$ ，摩尔质量单位为 $g/mol$ 。

将质量为 $m_s$ 的溶质，完全溶于体积为 $v_l$ 密度为 $\rho_l$ 的溶剂里，完全溶解后形成体积为 $v$ 密度为 $\rho$ 的溶液。根据质量守恒有

(2-1)
$m_s+\rho_lv_l=\rho v$

这时溶质的百分比浓度 $\bar{c}$ 可表示为

(2-2)
$\bar{c}=\dfrac{m_s}{\rho v}\times 100=(1-\dfrac{\rho_l v_l}{\rho v})\times 100=(1-\dfrac{\rho_l}{\rho}s_p)\times 100$

公式里 $s_p=\dfrac{v_l}{v}$ 是一个与 $\rho$ 有关的函数，其表示了溶解前后液体体积比值，这是一个无量纲值。一般地，大部分溶质溶液溶剂后体积变化不明显时，可以忽略体积变化比，这时可取 $s_p=1$ 。

结合(1-1)与(2-2)可以得到

(2-3)
$c=\dfrac{1000}{\alpha}(\rho-\rho_l s_p)$

在实际使用时， $\alpha$ 与 $\rho_l$ 均为已知量，因此只需要确定出 $s_p$ 的表达式，即可确定出浓度与密度的关系。

对于20°C时，NaCl的溶液质量百分比浓度(%)与密度( $g/ml$ )数据如下^[1]
0 1.0000
1 1.00534
2 1.01246
4 1.02680
6 1.04127
8 1.05589
10 1.07068
12 1.08566
14 1.10085
16 1.11621
18 1.13190
20 1.14779
22 1.16395
24 1.18040
26 1.19717

对于其它温度下的密度浓度对应关系，可以在[1]里进行查询。

这里的模型基于第3部分的测试数据来构建，因此模型适用于20°C时的情形。由于这里构建NaCl溶液，因此(2-3)中的 $\alpha$ 取值58.44246928，而溶剂使用纯净水代替，因此 $\rho_l$ 取值为1000。由于 $s_p$ 为与 $\rho$ 有关的无量纲参数，为了使得构建模型没有量纲约束，因此这里将查找 $s_p$ 与 $\bar{\rho}$ 的关系，这里 $\bar{\rho}$ 为如下表达式

(4-1)
$\bar{\rho}=\dfrac{\rho}{1000}$

上面公式的 $\bar{\rho}$ 为无量纲参数，其表示溶液密度与密度为1000 $kg/m^3$ 纯净水密度的比值。下图为几个模型的回归情况，其中红色曲线与绿色曲线几乎重合。

这里假设 $s_p$ 与 $\bar{\rho}$ 为如下线性关系

(4-2)
$s_p=a_0+a_1\bar{\rho}$

结合(2-2)、（4-1）、(4-2)使用实验数据进行最小二乘拟合可以得到最佳系数如下

(4-3)
$\begin{cases}a_0=1.56521273161865\\a_1=-0.564020522841257\end{cases}$

上面拟合误差平方和为0.524321166155047，R相关系数为0.9997489529926。将(4-3)带回(2-3)可得到如下模型

(4-4)
$\begin{aligned}c&=b_0+b_1\rho\\ &\begin{cases}b_0=-2.67821115517837\times 10^4 \\ b_1 = 26.7617118528647 \end{cases}\end{aligned}$

这里假设 $s_p$ 与 $\bar{\rho}$ 为如下二次关系

(4-5)
$s_p=a_0+a_1\bar{\rho} +a_2{\bar{\rho}}^2$

结合(2-2)、（4-1）、(4-5)使用实验数据进行最小二乘拟合可以得到最佳系数如下

(4-6)
$\begin{cases}a_0=0.869881276386526\\a_1=0.714351079066791\\a_2=-0.585595661577201\end{cases}$

上面拟合误差平方和为0.0322690864761774，R相关系数为0.99998455125635。将(4-6)带回(2-3)可得到如下模型

(4-7)
$\begin{aligned}c&=b_0+b_1\rho+b_2\rho^2\\ &\begin{cases}b_0=-1.48844031934875\times 10^4 \\ b_1 = 4.88769424790480 \\b_2= 0.01002003626458 \end{cases}\end{aligned}$

从(2-2)可以看出，浓度与溶液密度倒数有关系，这里假设 $s_p$ 也与溶液密度倒数有关系，因此不妨假设其与 $\bar{\rho}$ 存在如下关系

(4-8)
$s_p=a_0+a_1\bar{\rho} +\dfrac{a_2}{\bar{\rho}}$

结合(2-2)、（4-1）、(4-8)使用实验数据进行最小二乘拟合可以得到最佳系数如下

(4-9)
$\begin{cases}a_0=2.96518628624602\\a_1=-1.20608614457213\\a_2=-0.760572968363339\end{cases}$

上面拟合误差平方和为0.034659103653125，R相关系数为0.99998340703216。将(4-9)带回(2-3)可得到如下模型

(4-10)
$\begin{aligned}c&=b_0+b_1\rho+\dfrac{b_2}{\rho}\\ &\begin{cases}b_0=-5.07368412522015 \times 10^4 \\ b_1 = 37.7479968206458 \\b_2= 1.30140457399123 \times 10^7 \end{cases}\end{aligned}$

参考
[1] Density Changes with Concentration

对任意三角形区域进行面积分

2023年5月12日2023年5月12日 shikang999 评论

1 背景

有时，我们需要对一个多边形区域进行面积分，而任意一个多边形区域都可以分解成有限个三角形区域，对多边形区域积分就变成对所有三角形区域积分之和。因此本文重点书写三角形区域积分。

这里假设需要在面积为 $s$ 的三角形区域对在此区域连续的函数 $f$ 进行积分，这里记积分为
(1-1)
$u=\iint_S f(x,y)dxdy=\iint_S f(\mathbf{p})dxdy$

公式中 $S$ 为三角形区域， $\mathbf{p}$ 表示输入参数的坐标点。

为了能获取对任意三角形区域进行积分的表达式，一种可行方法是将当前坐标系转换到另一个坐标系进行计算。

不失一般性，设三角形三个顶点分别为 $\mathbf{p}_0$ 、 $\mathbf{p}_1$ 、 $\mathbf{p}_2$ ，它们的坐标分别为 $(x_0,y_0)$ 、 $(x_1,y_1)$ 、 $(x_2,y_2)$ ，且 $\mathbf{p}_0$ 与 $\mathbf{p}_1$ 所在边为三角形的最长边，且三个坐标点按逆时针排列。则现在将三角形坐标系转换到以 $\mathbf{p}_0$ 为原点， $\mathbf{p}_0$ 指向 $\mathbf{p}_1$ 方向为x方向正向的方向的坐标系，这时根据二维平面的旋转表示与性质可以得到如下变换矩阵
(1-2)
$\begin{cases} \mathbf{t} = \begin{bmatrix} x_0 \\ y_0 \end{bmatrix} \\ \\ R = \begin{bmatrix} \cos \theta &- \sin\theta \\ \sin\theta & \cos \theta \end{bmatrix} \\ \\ \rho =\arctan (y_1-y_0,x_1-x_0) \\ \\ \theta =\begin{cases} 2\pi – \rho, & \rho \geq0 \\ |\rho|, &\rho < 0 \end{cases} \end{cases}$

这时为 $\mathbf{p}_0$ 、 $\mathbf{p}_1$ 、 $\mathbf{p}_2$ 在新坐标系下的位置变为
(1-3)
$\begin{cases} \mathbf{p}_0'=\begin{bmatrix} x_0' \\ y_0' \end{bmatrix}=R(\mathbf{p}_0-\mathbf{t})=\mathbf{0} \\ \\ \mathbf{p}_1'=\begin{bmatrix} x_1' \\ y_1' \end{bmatrix}=R(\mathbf{p}_1-\mathbf{t}) \\ \\ \mathbf{p}_2'=\begin{bmatrix} x_2' \\ y_2' \end{bmatrix}=R(\mathbf{p}_2-\mathbf{t}) \end{cases}$

注意：
(1) 因为 $\mathbf{p}_1$ 在新坐标系的x轴上，则。 $y_1'=0$ 。
(2) 因为三个顶点按逆时针排列，转换后的 $y_2'>0$ ，如果转换后的 $y_2'<0$ ，则三个点是按顺时针排列，这种情况不满足要求，这时应将 $\mathbf{p}_0$ 、 $\mathbf{p}_1$ 对调再进行计算。在判断点是否逆时针排列时，可利用三角形面积计算公式，即如下表达式 $g$ 为正时表示三个坐标点按逆时针排列，否则按顺时针排列
(1-4)
$g=(x_0y_1-y_0x_1) + (x_1y_2-y_1x_2) + (x_2y_0-y_2x_0)$

然后将新坐标代入公式(1)有
(1-5)
$\begin{aligned} u&=\iint_S f(R^T \begin{bmatrix} x' \\ y' \end{bmatrix}+\mathbf{t}) \begin{vmatrix} \dfrac{\partial x}{\partial x'} & \dfrac{\partial x}{\partial y'} \\ \dfrac{\partial y}{\partial x'} & \dfrac{\partial y}{\partial y'} \end{vmatrix} dx'dy' \\ &=\iint_S f(R^T \begin{bmatrix} x' \\ y' \end{bmatrix}+\mathbf{t}) \begin{vmatrix} R^T \end{vmatrix} dx'dy' \\ &=\iint_S f(R^T \begin{bmatrix} x' \\ y' \end{bmatrix}+\mathbf{t}) dx'dy' \\ &= \int_{0}^{x_2'}dx'\int_0^{(y_2'/x_2')x'} f(R^T \begin{bmatrix} x' \\ y' \end{bmatrix}+\mathbf{t})dy'+\int_{x_2'}^{x_1'}dx'\int_0^{(x-x_1')y_2'/(x_2'-x_1')} f(R^T \begin{bmatrix} x' \\ y' \end{bmatrix}+\mathbf{t})dy' \end{aligned}$

因此对于任意函数，可直接使用(1-5)进行三角形区域积分。

2 应用特例

2.1 函数为线性函数

当前分布函数满足如下线性分布
(2-1)
$f(\mathbf{p}) = \mathbf{p}^T\mathbf{a}+b$

将(2-1)代入(1-4)有

(2-2)
$\begin{aligned} u &= \iint_S (\mathbf{p}^T\mathbf{a}+b)dx'dy' \\ &=sb+\iint_S (\mathbf{a}^T\mathbf{p})dx'dy' \\ &=sb+\int_{0}^{x_2'}dx'\int_0^{(y_2'/x_2')x'} \mathbf{a}^T(R^T \begin{bmatrix} x' \\ y' \end{bmatrix}+\mathbf{t})dy'+\int_{x_2'}^{x_1'}dx'\int_0^{(x-x_1')y_2'/(x_2'-x_1')} \mathbf{a}^T(R^T \begin{bmatrix} x' \\ y' \end{bmatrix}+\mathbf{t})dy' \\ &=s(b+\mathbf{a}^T\mathbf{t})+\int_{0}^{x_2'}dx'\int_0^{(y_2'/x_2')x'} (\begin{bmatrix} x' & y' \end{bmatrix}R\mathbf{a})dy'+\int_{x_2'}^{x_1'}dx'\int_0^{(x-x_1')y_2'/(x_2'-x_1')} ( \begin{bmatrix} x' & y' \end{bmatrix}R\mathbf{a})dy' \\ &=s(b+\mathbf{a}^T\mathbf{t})+\dfrac{1}{3}\begin{bmatrix} y_2'x_2'^2 &x_2'y_2'^2/2 \end{bmatrix}R\mathbf{a}+\dfrac{1}{6}\begin{bmatrix} y_2'x_1'^2-2y_2'x_2'^2+y_2'x_1'x_2' & y_2'^2(x_1'-x_2') \end{bmatrix}R\mathbf{a} \\ &=s(b+\mathbf{a}^T\mathbf{t})+\dfrac{x_1'y_2'}{6}\begin{bmatrix} x_1' + x_2' & y_2' \end{bmatrix}R\mathbf{a} \\ &=s(b+\mathbf{a}^T\mathbf{t})+\mathbf{c}^TR\mathbf{a} \end{aligned}$

上面公式中
(2-3)
$\begin{aligned} \mathbf{c}&=\dfrac{x_1'y_2'}{6}\begin{bmatrix} x_1'+x_2' \\ y_2' \end{bmatrix} \\ &=\dfrac{x_1'y_2'}{6}(\mathbf{p}_1'+\mathbf{p}_2') \\ &=\dfrac{x_1'y_2'}{6} R(\mathbf{p}_1+\mathbf{p}_2-2\mathbf{t}) \end{aligned}$

将(2-3)代码(2-2)有
(2-4)
$\begin{aligned} u&=s(b+\mathbf{a}^T\mathbf{t})+\dfrac{x_1'y_2'}{6}(\mathbf{p}_1+\mathbf{p}_2-2\mathbf{t})^TR^TR\mathbf{a} \\ &=s(b+\mathbf{a}^T\mathbf{p}_0)+\dfrac{x_1'y_2'}{6}(\mathbf{p}_1+\mathbf{p}_2-2\mathbf{p}_0)^T\mathbf{a} \end{aligned}$
对于(2-1)的线性函数，最终的积分结果为(2-4)。

PS：上面公式中为表示方便，向量(或矩阵)中只有一个元素时，这时的向量(或矩阵)与标量数值不加以区分。

Whittaker数据平滑算法

2023年3月2日2023年3月3日 shikang999 评论

1 前言
很多工程都会遇到对数据进行平滑降噪的问题。常规的降噪除了使用滤波方式外，也可以通过优化的方式进行处理。如果场景特殊，也可以自行根据场景构建非线性模型进行优化降噪。这篇文章介绍一种构建方式，以作参考。

文献[1]作者使用优化的方式提出了Whittaker平滑器，对于一维输入序列 $\mathbf{x}$ ，假设降噪处理后的序列为 $\mathbf{y}$ ，则Whittaker算法需要最小化如下目标函数

(1)
$e = (\mathbf{x}-\mathbf{y})^TW_m(\mathbf{x}-\mathbf{y})+(D\mathbf{y})^TW_s(D\mathbf{y})$
上面公式中 $W_m$ 为逼近原始数据的权重对称矩阵, $W_s$ 为度量平滑程度的权重对称矩阵。 $D\mathbf{y}$ 表示对 $\mathbf{y}$ 求二阶导数。很明显，这个目标函数有两部分，前面部分表示输出数据与原始数据的逼近程度，后面部分表示输出数据的平滑程度。

因为这里是不带其它信息的离散数据求导(默认每个数据点之间的间隔相等)，文献[1]根据前后两点斜率作为一阶结果的方式得到二阶 $D$ 为如下值

(2)
$D=\begin{bmatrix} 1 & -2 & 1 \\ & 1 & -2 & 1 \\ && 1 & -2 & 1 \\ &&& 1 & -2 & 1 \\ &&&& 1 & -2 & 1 \\ &&&&& 1 & -2 & 1 \end{bmatrix}$

使用一阶判断方法很容易判断(1)里的问题为凸问题，要最小化这个凸问题，令其一阶导数为0，则化简得到如下结果：

(3)
$\mathbf{y} = (W_m+D^TW_sD)^{-1}W_m\mathbf{x}$
因此公式(3)就是最终得到的平滑算法。

PS: Whittaker算法默认每个点间隔一致，如果输入序列附加额外的属性，比如附带距离属性，则上面的最小化公式不再适用，建议单独针对附加属性来构建相关约束，进而求解。

一般地，我们会这样 $W_m=wI,W_s=\lambda I$ 来设置权重矩阵，这里设 $G=I+\dfrac{\lambda}{w} H$ ，这时公式(3)变成

(4)
$\begin{aligned} \mathbf{y} &=w(wI+\lambda D^TD)^{-1}\mathbf{x} \\ &=(I+\dfrac{\lambda }{w}D^TD)^{-1} \mathbf{x} \\ &=(I+\dfrac{\lambda}{w}H)^{-1} \mathbf{x} \\ &=G^{-1}\mathbf{x} \end{aligned}$

可以发现，参数 $w$ 与 $\lambda$ 同时缩放，不失一般性，这里设 $\eta = \dfrac{\lambda}{w}$ ，每次计算时只需设置参数 $\eta$ 即可，因此可以得到 $G=I+\eta H$ ，根据(2)的结果，计算出 $H$ 为如下值

(5)
$H=\begin{bmatrix} 1 & -2 & 1 \\ -2 & 5 & -4 & 1 \\ 1 & -4 & 6 & -4 & 1 \\ & 1 & -4 & 6 & -4 & 1 \\ &&1& -4 & 5 & -2 \\ &&& 1 & -2 & 1 \end{bmatrix}$

这里进一步对 $G$ 做如下三角分解( $L$ 为下三角矩阵， $U$ 为单位上三角矩阵)

(6)
$G=LU$
很明显, 公式中 $G$ 为对称带状矩阵，总存在如下形式的 $L$ 、 $U$ 矩阵，使得 $L$ 、 $U$ 满足公式(6)

(7)
$L=\begin{bmatrix} l_{00} \\ l_{10} & l_{11} \\ l_{20} & l_{21} & l_{22} \\ &l_{31} & l_{32} & l_{33} \\ &&l_{42} & l_{43} & l_{44} \\ &&&l_{53} & l_{54} & l_{55} \end{bmatrix}$
$U=\begin{bmatrix} 1 & u_{01} & u_{02} \\ & 1 & u_{12} & u_{13} \\ && 1 & u_{23} & u_{24} \\ &&& 1 & u_{34} & u_{35} \\ &&&& 1 & u_{45} \\ &&&&& 1 \end{bmatrix}$
因此对于 $n$ 阶 $G$ 矩阵，根据公式(6)(7)可以得到如下分解公式

(8)
$\begin{aligned} &\begin{cases} \begin{cases} l_{0,0} = g_{0,0} \\ u_{0,1} =\dfrac{g_{0,1}}{g_{0,0}} \\ u_{0,2} =\dfrac{g_{0,2}}{g_{0,0}} \\ l_{1,0} =g_{1,0} \\ l_{1,1} =g_{1,1}-g_{1,0}u_{0,1} \\ u_{1,2}=\dfrac{g_{1,2}-g_{1,0}u_{0,2}}{l_{1,1}} \\ u_{1,3}=\dfrac{g_{1,3}}{l_{1,1}} \end{cases} \\ \\ \begin{cases} l_{i,i-2}=g_{i,i-2} \\ l_{i,i-1}=g_{i,i-1}-l_{i,i-2}u_{i-2,i-1} \\ l_{i,i}=g_{i,i}-l_{i,i-2}u_{i-2,i}-l_{i,i-1}u_{i-1,i} \\ u_{i,i+1}=\dfrac{g_{i,i+1}-l_{i,i-1}u_{i-1,i+1}}{l_{i,i}} \\ u_{i,i+2}=\dfrac{g_{i,i+2}}{l_{i,i}} \end{cases},\ i = 2,3,4,…,n-3 \\ \\ \begin{cases} l_{n-2,n-4}=g_{n-2,n-4} \\ l_{n-2,n-3}=g_{n-2,n-3}-l_{n-2,n-4}u_{n-4,n-3} \\ l_{n-2,n-2}=g_{n-2,n-2}-l_{n-2,n-4} u_{n-4,n-2} – l_{n-2,n-3} u_{n-3,n-2} \\ u_{n-2,n-1}=\dfrac{g_{n-2,n-1}-l_{n-2,n-3} u_{n-3,n-1}}{l_{n-2,n-2}} \\ l_{n-1,n-3}=g_{n-1,n-3} \\ l_{n-1,n-2}=g_{n-1,n-2}-l_{n-1,n-3}u_{n-3,n-2} \\ l_{n-1,n-1}=g_{n-1,n-1} -l_{n-1,n-2}u_{n-2,n-1}-l_{n-1,n-3}u_{n-3,n-1} \end{cases} \end{cases} \end{aligned}$
通过公式(8)最终求得三角矩阵 $L$ 、 $U$ ，最终先求解如下方程得到 $\mathbf{z}$

(9)
$L\mathbf{z}=\mathbf{x}$
然后按如下方式求得 $\mathbf{y}$

(10)
$U\mathbf{y}=\mathbf{z}$

如果直接构建矩阵方式求解公式(8)比较耗时，仔细观察上面数据 $H$ 、 $L$ 、 $U$ 的分布情况，可以发现，这个问题可以采用压缩存储的方式进行计算。结合公式(8)的下标，这里使用新的 $\mathbf{a},\mathbf{b}$ 来存储 $L$ 中的元素， $\mathbf{c},\mathbf{d}$ 来存储 $U$ 中的元素，对LU分解可采用如下方式进行

(11)
$\begin{cases} \begin{cases} b_0 = 1+\eta \\ c_0 =\dfrac{-2\eta}{b_0} \\ d_0 =\dfrac{\eta}{b_0} \\ a_1 =-2\eta \\ b_1 =1+(5+2c_0)\eta \\ c_1=\dfrac{2d_0-4}{b_1}\eta \\ d_1=\dfrac{\eta}{b_1} \end{cases} \\ \\ \begin{cases} a_i=-(4 + c_{i-2})\eta \\ b_i=1+(6-d_{i-2})\eta -a_i c_{i-1} \\ c_i=\dfrac{-4\eta-a_i d_{i-1}}{b_i} \\ d_i=\dfrac{\eta}{b_i} \end{cases},\ i = 2,3,4,…,n-3 \\ \\ \begin{cases} a_{n-2}=-(4+c_{n-4})\eta \\ b_{n-2}=1+(5-d_{n-4})\eta – a_{n-2} c_{n-3} \\ c_{n-2}=\dfrac{-2\eta-a_{n-2} d_{n-3}}{b_{n-2}} \\ a_{n-1}=-(2+c_{n-3})\eta \\ b_{n-1}=1+(1-d_{n-3})\eta -a_{n-1}c_{n-2} \end{cases} \end{cases}$

接着按如下公式计算 $\mathbf{z}$

(12)
$\begin{cases} z_0 = \dfrac{x_0}{b_0} \\ z_1 = \dfrac{x_1-a_1z_0}{b_1} \\ \\ z_i=\dfrac{x_i-a_iz_{i-1}-\eta z_{i-2}}{b_i},\ i=2,3,4,…,n-1 \end{cases}$

最后按如下公式计算 $\mathbf{y}$

(13)
$\begin{cases} y_{n-1} = z_{n-1} \\ y_{n-2}=z_{n-2}-c_{n-2}y_{n-1} \\y_i=z_i-c_iy_{i+1}-d_iy_{i+2},\ i=n-3,n-4,n-5,…,0 \end{cases}$

最终，Whittaker算法平滑降噪的步骤为
step1：设置输入序列 $\mathbf{x}$ 以及权重参数比 $\eta$ 。

step2：根据公式(11)计算中间变量 $\mathbf{a},\mathbf{b},\mathbf{c},\mathbf{d}$ 。

step3：根据公式(12)求得中间变量 $z$ 。

step4：根据公式(13)求得最终的平滑降噪序列 $\mathbf{y}$ 。

6 应用
对于一维问题，直接使用步骤进行平滑；对于二维问题，先对每一列单独进行平滑，然后在所有列平滑完之后，对新的数据每一行再进行平滑即可。

参考:
[1] Zuliana S U , Perperoglou A . Two dimensional smoothing via an optimised Whittaker smoother[J]. Big Data Analytics, 2017, 2(1):6.

[2]https://eigenvector.com/wp-content/uploads/2020/01/WhittakerSmoother.pdf

多种相机模型

2022年11月30日2023年1月4日 shikang999 评论

现今相机传感器使用广泛，为了使相机成像更加真实，就需要了解对应相机模型以及对应参数，进而修复可能出现的成像问题。了解模型后，采用对应模型进行相机标定，然后去畸变等等。

1 针孔相机

1.1 针孔相机模型

对于薄透镜的针孔相机，其成像可简化成如下形式

上面模型中， $(x,y,z)$ 为相机坐标系下的3D点，其中以相机镜面正前方为 $z$ 方向。上面模型中 $f$ 为相机焦距， $d$ 为像距。根据相似三角形原理，不难得到如下结果

(1) $\begin{cases} \dfrac{x}{f}=\dfrac{u}{d-f} \\ \dfrac{x}{z}=\dfrac{u}{d} \end{cases} \Rightarrow f=\dfrac{d}{\dfrac{d}{z}+1} \Rightarrow \dfrac{1}{f}=\dfrac{1}{z}+\dfrac{1}{d}$

受限于相机尺寸以及为了扩大成像视野，一般 $z >> d$ ，这时 $f \approx d$ (这就是成像在焦点附近比较清晰的原因)，因此可得到像平面的坐标

(2) $p_x=f\dfrac{x}{z}$

同理可以得到

(3) $p_y=f\dfrac{y}{z}$

注意， $(p_x, p_y)$ 仅是理论上像平面的坐标。由于相机一般由感光器件来进行图像映射，感光器反馈的是像素单元的坐标，假设单个像素横向占用 $1/l_x$ 米，纵向占用 $1/l_y$ 米，则可以使用 $l_x、l_y$ 将 $(p_x, p_y)$ 缩放到像素坐标。另一方面，由于 $(p_x, p_y)$ 得到的坐标是以相机光心为原点，而常见的图像坐标以图像左上角为原点，因此需要移动原点，这时在缩放后的像素坐标上加一个偏移 $(c_x,c_y)$ 。一般 $(c_x,c_y)$ 取相机光心在像素中的坐标值。最终有如下针孔相机模型

(4) $\begin{cases} u =l_xp_x+ c_x= f l_x \dfrac{x}{z}+c_x \\ v =l_yp_y + c_y = f l_y \dfrac{y}{z} + c_y \end{cases}$

为了简洁表示，这里设归一化坐标 $(x',y')$ 如下

(5) $\begin{cases} x'=\dfrac{x}{z} \\ y'=\dfrac{y}{z} \end{cases}$

以及设置内参

(6) $\begin{cases} f_x =f l_x \\ f_y = f l_y \end{cases}$

将(5)(6)代入(4)得有针孔相机模型

(7) $\begin{cases} u = f_x x'+c_x \\ v =f_y y' + c_y \end{cases}$

1.2 图像倾斜修正

现在面阵相机比较普遍，工业上也使用线阵相机，而有些线阵相机也会按线条进行拼接成一个“面阵”，因此最终都需要呈现矩阵像素点阵。如下图a，理想中的相机传感器上的感光器件应该呈方形(或矩形)分布，然而对于一些生产工艺较差的相机，如下图b，其感光器件可能呈现如下右图的平行四边形排布。当出现图b的情形时，这时呈现的图像也跟着倾斜。如果想了解一些成像原理，可以参考文献[1]。

为了解决上图倾斜的问题，不失一般性，以上图b为原型，建立如下图的模型。

这个模型中约定真实成像点应该在 $(x_r,y_r)$ ，因为感光器原因，反馈了一个错误的位置 $(x_i,y_i)$ ，器件沿着 $x$ 轴倾斜的角度为 $\theta_x$ ，沿着 $y$ 轴倾斜角度为 $\theta_y$ ，这时需要得到模型中的坐标 $(x_r,y_r)$ 。根据模型中的几何关系，可以得到

(8) $\begin{aligned}&\begin{cases} \dfrac{l_1}{\sin\theta_y}=\dfrac{y_i}{\sin(\dfrac{\pi}{2}-\dfrac{\theta_y}{2})} \\ \dfrac{l_1}{\sin \theta_x}=\dfrac{l_2}{\sin\dfrac{\theta_y}{2}}=\dfrac{l_5}{\sin(\pi-\theta_x-\dfrac{\theta_y}{2})} \\ l_3=\dfrac{x_i-l_5}{\cos \theta_x} \\ l_4=x_i-l_2-l_3\\ x_r =x_i+l_4\cos\theta_x \\ y_r = y_i+(l_3+l_4)\sin\theta_x \end{cases}\\ &\Downarrow \\&\begin{cases} x_r =\cos\theta_xx_i+\sin\theta_yy_i \\ y_r =\sin\theta_xx_i+\cos\theta_y y_i \end{cases}\end{aligned}$

注意，上面公式中 $(x_i,y_i)$ 与 $(x_r,y_r)$ 都不是相对相机光心像素点为原点，进一步这里设置 $S$ 矩阵

(9) $S =\begin{bmatrix} \cos\theta_x & \sin\theta_y \\ \sin\theta_x & \cos\theta_y \end{bmatrix}$

将(9)代入(8)有

(10) $\begin{aligned} \begin{bmatrix} x_r \\ y_r \end{bmatrix} &=S\begin{bmatrix} x_i \\ y_i \end{bmatrix} \\ &=S\begin{bmatrix} f_xx'+c_{xi} \\ f_yy'+c_{yi} \end{bmatrix} \\ &=S\begin{bmatrix} f_xx' \\ f_yy' \end{bmatrix}+S\begin{bmatrix} c_{xi} \\ c_{yi} \end{bmatrix} \\ &=S\begin{bmatrix} f_xx' \\ f_yy' \end{bmatrix}+\begin{bmatrix} c_x \\ c_y \end{bmatrix} \\ &=\begin{bmatrix} fl_x\cos\theta_x & fl_y\sin\theta_y \\ fl_x\sin\theta_x & fl_y\cos\theta_y \end{bmatrix}\begin{bmatrix} x' \\ y' \end{bmatrix}+\begin{bmatrix} c_x \\ c_y \end{bmatrix} \end{aligned}$

公式(10)中的 $(c_x,c_y)$ 表示修正后的偏移量。进一步，为了简洁表示，这里设置如下变量

(11) $\begin{cases} f_x = fl_x\cos\theta_x \\ f_y=fl_y\cos\theta_y \\ \alpha_x =fl_x\sin\theta_x \\ \alpha_y=fl_y\sin\theta_y \end{cases}$

设置如下内参矩阵 $K$

(12) $K = \begin{bmatrix} f_x & \alpha_y & c_x \\ \alpha_x & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$

则可得倾斜修正后的模型

(13) $\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=K \begin{bmatrix} x' \\ y' \\ 1 \end{bmatrix}$

注意，内参矩阵 $K$ 里的参数 $f_x、f_y、\alpha_x、\alpha_y$ 反映了各自缩放与倾斜的信息，特别地，当 $\alpha_x = 0$ 时，内参矩阵 $K$ 变为如下形式

(14) $K = \begin{bmatrix} fl_x & fl_y\sin\theta_y & c_x \\ 0 & fl_y\cos\theta_y & c_y \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} f_x & \alpha_y & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}=\begin{bmatrix} f_x & \alpha f_x & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$

公式(14)中 $\alpha = \dfrac{l_y}{l_x}\sin\theta_y$ ，OpenCV中就使用的这种表示形式，由于一般 $l_x$ 与 $l_y$ 值很接近，因此 $\alpha \approx \sin\theta_y$ ，这时 $\alpha$ 主要反映阵列偏离 $y$ 轴的程度，这个值越远离0，则越加倾斜。

PS:

A : 上面导出的修正模型中 $\theta_x、\theta_y$ 正负均可，导出的公式一致。

B : 这里的导出方式和一般文献的导出方式正好相反(例如文献[2])，这里基于的原理是给相机一个非矩形正确的光源点位置，但是信号反馈到一个矩形错误的位置，因此对其进行修正，而一般的文献推导是正确的光源点为矩形位置，信号反馈为非矩形位置。

C : 现今的相机，一般 $\theta_x=0、\theta_y=0$ ，所以很多时候 $K$ 少了 $\alpha_x、\alpha_y$ 两个内参。

D ：常规情况下，认为工艺中仅存在左右倾斜的情况，即 $\theta_x=0$ ，所以大部分文献导出的内参没有 $\alpha_x$ 。

E : 常规情况下， $l_x \approx l_y$ ，所以大部分情况下会看到 $f_x$ 与 $f_y$ 接近。

1.3 针孔相机畸变

1.2修正的是倾斜，倾斜的模型是线性的，而有时成像扭曲，这时就需要对相机进行畸变处理。

由于相机装配或者本身形状原因，相机图像一般存在畸变，畸变呈现的图形往往扭曲。

1.3.1 径向畸变

由透镜形状引起的畸变称为径向畸变，在透镜拍摄过程中，往往一条直线在相机上显示为一条曲线，在越靠近边缘的地方，这种现象越明显。这里径向畸变主要包括桶形失真(图形上表现为外凸)与枕形失真(图形上表现为内凹)。

由于径向畸变是随着与中心之间的距离增加而增加，因此往往使用一个与径向相关的多项式来进行修正：

(15) $\begin{cases} x_d=x'(1+k_1r'^2+k_2r'^4+k_3r'^6+…) \\ y_d=y'(1+k_1r'^2+k_2r'^4+k_3r'^6+…) \end{cases}$
公式中 $k_1,k_2,k_3,…$ 表示修正系数，一般的相机使用 $k_1,k_2$ 修正系数就足够了，而对于鱼眼相机可能还需要加入 $k_3$ 作为修正项。这里径向半径 $r'=\sqrt{x'^2+y'^2}$ 。

1.3.2 切向畸变
由于相机组装过程中不能使透镜和成像面严格平行，这时会引入切向畸变，对于切向畸变，可使用如下公式修正
(16)
$\begin{cases} x_d=x'+2p_1x'y'+p_2(r'^2+2x'^2) \\ y_d=y'+p_1(r'^2+2y'^2)+2p_2x'y' \end{cases}$

1.3.3 针孔相机畸变修正模型
结合(15)(16)得到针孔相机畸变的修正模型
(17)
$\begin{cases} x_d=x'(1+k_1r'^2+k_2r'^4+k_3r'^6+…)+2p_1x'y'+p_2(r'^2+2x'^2) \\ y_d=y'(1+k_1r'^2+k_2r'^4+k_3r^6+…)+p_1(r'^2+2y'^2)+2p_2x'y' \end{cases}$

使用修正后的归一化坐标 $(x_d,y_d)$ 代入(13)的相机模型有

(18) $\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=K \begin{bmatrix} x_d \\ y_d \\ 1 \end{bmatrix}$

1.4 针孔相机内参标定

所谓内参标定，就是通过某种方式确定出内参矩阵 $K$ 以及相应的畸变参数。而这里所谓的某种方式，就是在已知输入输出的情况下，反算出内参，这里涉及到两个层面：非线性求解、给定输入输出。

非线性求解，这个需要使用非线性求解器，这里直接采用某种非线性求解算法即可。

给定输入输出，即现实中给定某个特征，这个特征成像后具有某种性质，而根据这种性质建立约束，进而使用非线性方法迭代求解。例如直线特征，现实生活中一条直线，成像后也应该是一条直线，而不是曲线；现实生活中不是特远的矩形块儿，成像后也应该保持矩形特征。所以常见使用方形格子标定板进行标定。当然，也可以不使用方形格子板，自己手动指定一些特征，也是一样的。

还需要注意的是，如果标定时夹杂了位姿(一个变换),如果能提前确定好位姿值最好，否则优化求解中会多位姿变量的求解。这个主要根据构建约束的方式来确定。

2 其它相机模型

这里提到的相机模型，均需要去畸变，它们主要不同点是去畸变基于的模型不同。去完畸变后，就简化成一个针孔相机，进而成像。

这里同样记物点在相机坐标系下坐标为 $(x,y,z)$ ，归一化坐标为 $(x',y')$ ，下面的模型主要是对 $(x',y')$ 修正得到 $(x_d,y_d)$ ，最后代入(18)式。因此每个模型中的内参除了模型需要的几个参数外，还包括 $K$ .

2.1 UCM模型^[3]

UCM模型全称叫Unified Camera Model。这个模型主要用于折反射相机。它是基于一个物点投影到单位球体上，然后再投影到像平面这么一个过程构建模型。这个模型的修正公式如下

(19) $\begin{cases} x_d=\dfrac{x}{\alpha d + (1-\alpha)z} \\ \\ y_d=\dfrac{y}{\alpha d + (1-\alpha)z} \end{cases}$

公式中 $\dfrac{\alpha}{1-\alpha}$ 表示球体中心到像平面距离，而 $d$ 则使用如下方式计算

(20) $d = \sqrt{x^2+y^2+z^2}$

UCM模型需要的内参为 $K, \alpha$ .

2.2 EUCM模型^[3]

EUCM模型全称叫Extended Unified Camera Model。UCM的镜头是一个圆形球面，为了更具一般性，EUCM将镜头模型扩展到椭圆形，因此增加了一个控制椭圆程度的参数 $\beta$ (此参数越接近1，镜头椭圆度越小，这时镜头面越接近圆球面；否则镜头的椭圆度越大，镜头面就是椭圆面了).

EUCM模型依然使用(19)式进行修正，但 $d$ 则使用如下方式计算

(21) $d = \sqrt{\beta(x^2+y^2)+z^2}$

EUCM模型需要的内参为 $K, \alpha, \beta$ .

2.3 KB模型^[4]

KB模型全称叫KannalaBrandt Camera Model，它是Juho Kannala和Sami S. Brandt根据几种鱼眼模型(等距投影模型、等立体角投影模型、正交投影模型和体视投影模型)分析得出。KB模型是一个通用模型，适合常见的普通镜头、广角镜头、鱼眼镜头。这个模型的修正公式如下

(22) $\begin{cases} x_d=\dfrac{\theta_d}{r}x \\ \\ y_d=\dfrac{\theta_d}{r}y \end{cases}$

公式中的 $r, \theta_d$ 使用如下公式获取

(23) $\begin{aligned} r &= \sqrt{x^2+y^2} \\ \theta &=\arctan(r,z) \\ \theta_d&=\theta+k_1\theta^3+k_2\theta^5+k_3\theta^7+k_4\theta^9+… \end{aligned}$

KB模型需要的内参为 $K, k_1, k_2, k_3, k_4, …$ .

2.4 FOV模型^[3]

FOV模型全称叫Field Of View Camera Model。FOV(视场相机)模型, 假设像点与主体点之间的距离近似正比于对应的3D点与光轴之间的角度，因此提出如下修正模型

(24) $\begin{cases} x_d=\dfrac{r_d}{r}x \\ \\ y_d=\dfrac{r_d}{r}y \end{cases}$

公式中 $r_d$ 按如下方式计算

(25) $r_d=\dfrac{\arctan(2r\tan(w/2), z)}{w}$

公式中的参数 $w$ 表示对应于理想鱼眼镜头的视场。

FOV模型需要的内参为 $K, w$ .

2.5 DS模型^[3]

DS模型全称叫Double Sphere Camera Model，即双球面相机模型。这个模型主体思想是，一个点会被连续投影到两个球体上。这个模型可以看成EUCM的扩展。作者说这个模型更加适合鱼眼相机。这里模型的修正公式如下

(26) $\begin{cases} x_d=\dfrac{x}{\alpha d_2+(1-\alpha)(\xi d_1+z)} \\ \\ y_d=\dfrac{y}{\alpha d_2+(1-\alpha)(\xi d_1+z)} \end{cases}$

公式中 $d_1、d_2$ 按如下方式计算

(27) $\begin{aligned}d_1 &= \sqrt{x^2+y^2 + z^2} \\ d_2&=\sqrt{x^2+y^2+(\xi d_1 + z)^2}\end{aligned}$

公式中 $\xi$ 反映了两个单位球体中心的偏移量。

因此DS模型需要的内参为 $K, \alpha, \xi$ .

参考

[1] 吴建明. 全面详细解析CMOS和CCD图像传感器

[2] 吕泽乾. 数字图像相关中相机标定技术及其应用拓展[D]. 2020, 中国科学技术大学.

[3] Usenko V , Demmel N , Cremers D . The Double Sphere Camera Model: IEEE, 10.1109/3DV.2018.00069[P]. 2018.

[4] Kannala J , Brandt S S . A generic camera model and calibration method for conventional, wide-angle, and fish-eye lenses[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 2006, 28(8):1335.

二维平面的旋转表示与性质

2022年11月18日2024年4月11日 shikang999 评论

1 定义
有时，我们需要在二维平面上处理旋转与平移问题。因此，为了方便计算，这里考虑二维平面上的表示。
在表示之前，这里对一个2维向量 $\mathbf{p}$ ，定义如下运算式
(1)
$\mathbf{p}^\wedge=\begin{bmatrix} p_x \\ p_y \end{bmatrix}^\wedge =\begin{bmatrix} 0 & 1 \\ -1 & 0 \end{bmatrix} \mathbf{p} =\begin{bmatrix} p_y \\ -p_x \end{bmatrix}$
根据(1)的定义，可以得到如下结论
(2)
$\begin{aligned} \mathbf{p} &=-\mathbf{p}^{\wedge \wedge} =\mathbf{p}^{\wedge \wedge \wedge \wedge} \\ (\mathbf{a}^\wedge)^T\mathbf{b}&=\mathbf{b}^T\mathbf{a}^\wedge=-\mathbf{a}^T\mathbf{b}^\wedge \end{aligned}$

1.1 SO(2)
1.1.1 SO(2)定义
在平面坐标系里，只要绕着垂直平面的轴旋转指定角度，就可以得到一个旋转变换，也就是说，可以只用1个旋转角度来表示一个旋转变换。为了统一，这里约定，以右手坐标系里的x-y为平面，设绕z轴逆时针旋转 $\theta$ 角度时，得到的变换为对应为旋转变换。类似SO(3)，这里定义如下SO(2)指数映射
(3)
$\begin{aligned} \exp(\theta) &= R =\begin{bmatrix} \cos \theta & -\sin\theta \\ \sin\theta &\cos\theta \end{bmatrix} \\ \log(R) &=\theta \end{aligned}$
注意:

① (3)中的 $\exp、\log$ 函数本身只表示一种映射关系，也就是说 $\theta$ 与 $R$ 一一对应。

② 如果以正北方向为起始绕z轴逆时针旋转 $\theta$ ，按(3)得到的旋转矩阵表示局部到世界的表示方式，其可以直接用于坐标变化，这时得到的值可直接在一些地图软件上显示。

1.1.2 SO(2)性质
根据(3)中的定义，对于整数 $n$ 有如下公式成立
(4)
$\begin{aligned} \\ \theta &= \arcsin \dfrac{R_{10}-R_{01}}{2}+2n\pi \\ \exp(-\theta+2n\pi) &= R^T \\ \exp(\theta_1+\theta_2+2n\pi)&=R_1R_2=R_2R_1 \\ \exp(\theta_1-\theta_2+2n\pi)&=R_1R_2^T=R_2^TR_1 \\ \exp(\theta_2-\theta_1+2n\pi)&=R_1^TR_2=R_2R_1^T \end{aligned}$

1.1.3 SO(2)求导
由于SO(2)本质上只有一个的 $\theta$ 变量，从公式(4)可以发现其有很好的性质，因此这里直接采用常规的加法更新求导即可。这里对常见的模型，采用常规加法规则，列举其求导结果如下
(5)
$\begin{aligned} \dfrac{\partial R \mathbf{p}}{\partial \theta}&= \begin{bmatrix} \sin\theta & -\cos\theta \\ \cos\theta &\sin\theta \end{bmatrix}\mathbf{p}\\ &=R^T\begin{bmatrix} 0 & 1 \\ -1 & 0 \end{bmatrix}^T\mathbf{p} \\ &=R^T(-\mathbf{p})^\wedge \\ \\ \dfrac{\partial R^T \mathbf{p}}{\partial \theta}&= \begin{bmatrix} \sin\theta & \cos\theta \\ -\cos\theta &\sin\theta \end{bmatrix}\mathbf{p} \\ &=R\begin{bmatrix} 0 & 1 \\ -1 & 0 \end{bmatrix}\mathbf{p} \\ &=R\mathbf{p}^\wedge \\ \\ \dfrac{\partial \log(R_1R_2R_3)}{\partial \theta_2}&=\dfrac{\theta_1+\theta_2+\theta_3+2n\pi}{\partial\theta_2} \\&=1 \\ \\ \dfrac{\partial \log(R_1R_2^TR_3)}{\partial \theta_2}&=\dfrac{\theta_1-\theta_2+\theta_3+2n\pi}{\partial\theta_2} \\&=-1 \end{aligned}$

1.2 SE(2)
1.2.1 SE(2)定义
由于SO(2)只能表示一个旋转，为了同时表示旋转 $\theta$ 与平移 $\mathbf{t}$ ，这里使用3维度变量 $\xi$ 来表示，即
(6)
$\xi =\begin{bmatrix} \theta \\ \mathbf{t} \end{bmatrix}$
注意，上面定义变量时直接使用旋转 $\theta$ 与平移 $\mathbf{t}$ 来表示，这与SE(3)不同。在SE(3)中，直接使用李代数向量来表示，里面并未直接使用 $\mathbf{t}$ ，因为 $\mathbf{t}$ 里面还包含有旋转部分信息。这里之所以直接使用 $\theta$ 与 $\mathbf{t}$ ，因为下面定义的SE(2)具有很好的性质，在向量与矩阵间转换中直接使用定义的映射就能快速计算。另一方面，在构建边求导过程中，这种定义也不冲突。

对于矩阵形式，参照SE(3)的构建方式，这里定义如下SE(2)映射
(7)
$\begin{aligned} \exp(\xi) &= T = \begin{bmatrix} R & \mathbf{t} \\ \mathbf{0}^T &1 \end{bmatrix} \\ \xi &=\log(T) \end{aligned}$
注意，上面的函数 $\exp$ 与 $\log$ 仅表示一种映射关系。

1.2.2 SE(2)性质
根据(7)的定义，很容易得到如下性质
(8)
$\begin{aligned} T^{-1}&= \exp(\begin{bmatrix} -\theta+2n\pi \\ -R^T\mathbf{t} \end{bmatrix}) \\ T_1T_2&=\exp(\begin{bmatrix} \theta_1+\theta_2+2n\pi \\ R_1\mathbf{t}_2+\mathbf{t}_1 \end{bmatrix}) \end{aligned}$

1.2.3 SE(2)求导
由于SO(2)使用的常规加法更新求导，且(7)、(8)对加法更新具有很好的适应度，因此这里还是采用常规的加法更新求导模式，对于常见的函数，这里列举如下求导结果
(9)
$\begin{aligned} \dfrac{\partial (T\mathbf{p}')}{\partial \xi} &=\dfrac{\partial \begin{bmatrix} R\mathbf{p}+\mathbf{t} \\ 1 \end{bmatrix}}{\partial \xi} \\ &=\begin{bmatrix} R\mathbf{p}^\wedge & \mathbf{I} \\ 0 & \mathbf{0}^T \end{bmatrix} \\ \\ \dfrac{\partial \log(T_1T_2T_3)}{\partial \xi_2} &=\dfrac{\partial \log(\begin{bmatrix} R_1R_2R_3 &R_1R_2\mathbf{t}_3+R_1\mathbf{t}_2+\mathbf{t}_1 \\ \mathbf{0}^T & 1 \end{bmatrix})}{\partial \xi_2} \\ &=\dfrac{\partial \begin{bmatrix} \theta_1+\theta_2+\theta_3+2n\pi \\R_1R_2\mathbf{t}_3+R_1\mathbf{t}_2+\mathbf{t}_1 \end{bmatrix}}{\partial \xi_2} \\ &=\begin{bmatrix} 1 & \mathbf{0}^T \\ R_1R_2^T(-\mathbf{t}_3)^\wedge & R_1 \end{bmatrix} \\ \\ \dfrac{\partial \log(T_1T_2^{-1}T_3)}{\partial \xi_2} &=\dfrac{\partial \log(\begin{bmatrix} R_1R_2^TR_3 &R_1R_2^T(\mathbf{t}_3-\mathbf{t}_2)+\mathbf{t}_1 \\ \mathbf{0}^T & 1 \end{bmatrix})}{\partial \xi_2} \\ &=\dfrac{\partial \begin{bmatrix} \theta_1-\theta_2+\theta_3+2n\pi \\R_1R_2^T(\mathbf{t}_3-\mathbf{t}_2)+\mathbf{t}_1 \end{bmatrix}}{\partial \xi_2} \\ &=\begin{bmatrix} -1 & \mathbf{0}^T \\ R_1R_2(\mathbf{t}_3-\mathbf{t}_2)^\wedge & -R_1R_2^T \end{bmatrix} \end{aligned}$

IMU预积分

2022年11月15日2023年7月25日 shikang999 评论

1 背景
在一般的车载传感器上，都会提供IMU惯性测量单元，它主要提供各个时刻的线性加速度(沿着x、y、z的加速度)以及角速度(绕着x、y、z方向的角速度)测量值。由于这些测量值之间与车本身的位姿有一定关系，因此可以通过IMU测量值来估计车本身的位姿。

2 原理
学过初中物理就应该知道，一个物体，已知速度与加速度，这时经过一段时间后，可以计算出各个时间点的速度以及位移值，在整个过程计算中速度、加速度在不断变化，因此求每个时刻的值需要用到积分。由于处理的离散数据，这里的积分可直接累计（累加）的方式进行积分近似。因此所有公式均是基于此推导出来。唯一不同的是，公式里会涉及到旋转矩阵。这里根据文献^[1]，提供如下的IMU的数学模型
(1)
$\begin{aligned} \bar{\omega}_b(t) &= \omega_b(t) + b^\omega(t)+\eta^\omega(t) \\ \bar{a}_b(t)&=R_{wb}^T(a_w(t)-g_w(t)) + b^a(t)+\eta^a(t) \end{aligned}$
公式中参数含义如下：
$\bar{\omega}_b(t)$ :表示 $t$ 时刻测量到车载坐标系下的角速度。
$\omega_b(t)$ :表示 $t$ 时刻计算到的车载坐标系下的角速度。
$b^\omega(t)$ :表示 $t$ 时刻角速度的偏置，这个参数需要优化。
$\eta^\omega(t)$ :表示 $t$ 时刻角速度的白噪声。
$\bar{a}_b(t)$ :表示 $t$ 时刻测量到车载坐标系下的线性加速度。
$a_w(t)$ :表示 $t$ 时刻计算到的世界坐标系下的线性加速度。
$g_w(t)$ :表示 $t$ 时刻世界坐标系下的重力加速度。
$b^a(t)$ :表示 $t$ 时刻线性加速度的偏置，这个参数需要优化。
$\eta^a(t)$ :表示 $t$ 时刻线性加速度的白噪声。
$R_{wb}(t)$ :表示 $t$ 时刻从车载坐标系到世界坐标系的旋转变换。

对于世界坐标系下的速度 $v_w(t)$ 以及位置 $p_w(t)$ ，根据运动方程有
(2)
$\begin{aligned} \dot{v}_w(t) &= a_w(t) \\ \dot{p}_w(t) &=v_w(t) \\ \dot{R}_{wb}&=R_{wb}\omega_b^\wedge \end{aligned}$
对上面的公式采用欧拉积分有
(3)
$\begin{aligned} v_w(t+\Delta t) &= v_w(t) + a_w(t)\Delta t \\ p_w(t+\Delta t)&=p_w(t)+v_w(t)\Delta t+\dfrac{1}{2}a_w(t)\Delta t^2 \\ R_{wb}(t+\Delta t)&= R_{wb}(t)\exp(\omega_b(t)\Delta t) \end{aligned}$
将公式(1)代入公式(3)有
(4)
$\begin{aligned} v_w(t+\Delta t) &= v_w(t)+g_w(t)\Delta t+R_{wb}(t)(\bar{a}_b(t)-b^a(t)-\eta^a(t))\Delta t \\ p_w(t+\Delta t)&=p_w(t)+v_w(t)\Delta t+\dfrac{1}{2}g_w(t)\Delta t^2+\dfrac{1}{2}R_{wb}(t)(\bar{a}_b(t)-b^a(t)-\eta^a(t))\Delta t^2 \\ R_{wb}(t+\Delta t)&= R_{wb}(t)\exp((\bar{\omega}_b(t)-b^\omega(t) – \eta^\omega(t))\Delta t) \end{aligned}$
这里记
(5)
$\begin{aligned} R_i&=R_{wb}(t)|_{t=i} \\ p_i&=p_w(t)|_{t=i} \\ v_i&=v_w(t)|_{t=i} \\ \bar{\omega}_i&=\bar{\omega}_b(t)|_{t=i} \\ \omega_i&=\omega_b(t)|_{t=i} \\ \bar{a}_i&=\bar{a}_b(t)|_{t=i} \\ a_i&=a_w(t)|_{t=i} \\ b^\omega_i&=b^\omega(t)|_{t=i} \\ \eta^\omega_i&=\eta^\omega(t)|_{t=i} \\ b^a_i&=b^a(t)|_{t=i} \\ \eta^a_i&=\eta^a(t)|_{t=i} \\ g&=g_w(t) \\ \Delta t_{ij}&=\sum_{k=i}^{j-1} \Delta t \end{aligned}$
上面假设重力加速度 $g_w(t)$ 为常量。
这里定义如下增量公式
(6)
$\begin{aligned} \Delta R_{ij} &=R_i^TR_j \\ &=\prod_{k=i}^{j-1}\exp((\bar{\omega}_k-b^\omega_k – \eta^\omega_k)\Delta t) \\ \\ \Delta v_{ij}&=v_j-v_i \\ &=R_i^T(v_j-v_i-g\Delta t_{ij}) \\ &= \sum_{k=i}^{j-1}\Delta R_{ik}(\bar{a}_k-b^a_k-\eta^a_k)\Delta t \\ \\ \Delta p_{ij}&=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t_{ij}^2) \\ &=\sum_{k=i}^{j-1}[\Delta v_{ik}\Delta t+\dfrac{1}{2}\Delta R_{ik}(\bar{a}_k-b^a_k-\eta^a_k)\Delta t^2] \end{aligned}$
最终根据文献[1]得到如下预积分模型(实际使用的就是如下的模型,即如下约等于符号后的表达式)
(7)
$\begin{aligned} \Delta \bar{R}_{ij}&=R_i^TR_j\exp(\delta \phi_{ij}) \\ &\approx R_i^TR_j \\ \\ \Delta \bar{v}_{ij}&=R_i^T(v_j-v_i-g\Delta t_{ij})+\delta v_{ij} \\ &\approx R_i^T(v_j-v_i-g\Delta t_{ij}) \\ \\ \Delta \bar{p}_{ij}&=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t^2_{ij})+\delta p_{ij} \\ &\approx R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t^2_{ij}) \end{aligned}$
公式中 $\Delta \bar{R}_{ij}$ 、 $\Delta \bar{v}_{ij}$ 、 $\Delta \bar{p}_{ij}$ 为定义的预积分测量变量， $\delta R_{ij}$ 、 $\delta v_{ij}$ 、 $\delta p_{ij}$ 为对应白噪声。

如果公式(1)当中的偏置不为常数，则每次需要更新预积分测量变量，这里采用如下公式更新
(8)
$\begin{aligned} \Delta \bar{R}_{ij}&\approx \Delta \bar{R}_{ij} \exp (\dfrac{ \partial \Delta \bar{R}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i) \\ \Delta \bar{v}_{ij}&\approx \Delta \bar{v}_{ij}+\dfrac{ \partial \Delta \bar{v}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i+\dfrac{ \partial \Delta \bar{v}_{ij}}{ \partial b^a_i} \delta b^a_i \\ \Delta \bar{p}_{ij}&\approx \Delta \bar{p}_{ij}+\dfrac{ \partial \Delta \bar{p}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i+\dfrac{ \partial \Delta \bar{p}_{ij}}{ \partial b^a_i} \delta b^a_i \end{aligned}$
上面公式中的偏导数可以参考文献[2].

接下来，我们可以将计算的 $\Delta \bar{R}_{ij}$ 、 $\Delta \bar{v}_{ij}$ 、 $\Delta \bar{p}_{ij}$ 作为测量值，和外部其它变量建立约束。对相关变量进行更新，在实际优化求解中需要优化的变量为 $R_i$ 、 $p_i$ 、 $v_i$ 、 $R_j$ 、 $p_j$ 、 $v_j$ 、 $\delta b^\omega_i$ 、 $\delta b^a_i$ 。注意变量按如下方式更新
(9)
$\begin{aligned} R_i &= R_i \exp(\delta \phi_i) \\ p_i &= p_i + R_i \delta p_i \\ v_i &= v_i + \delta v_i \\ R_j &= R_j \exp(\delta \phi_j) \\ p_j &= p_j + R_j \delta p_j \\ v_j &= v_j + \delta v_j \\ \delta b^\omega_i &= \delta b^\omega_i + \bar{\delta b^\omega_i} \\ \delta b^a_i &= \delta b^a_i + \bar{\delta b^a_i} \end{aligned}$

参考
[1] Forster C , Carlone L , Dellaert F , et al. IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation (supplementary material)[J]. Georgia Institute of Technology, 2015.

[2] 苏笑云.IMU预积分的理解和推导

[3] 高翔.简明预积分推导

坐标系转换之LLA、ECEF、ENU互转

2022年11月15日2024年5月14日 shikang999 3 条评论

1 名词解释
1.1 ECEF坐标系
也叫地心地固直角坐标系。其原点为地球的质心，x轴延伸通过本初子午线（0度经度）和赤道（0deglatitude）的交点。 z轴延伸通过的北极（即，与地球旋转轴重合）。 y轴完成右手坐标系，穿过赤道和90度经度。

1.2 ENU坐标系
也叫东北天坐标系，坐标系定义为： X轴指向东边，Y轴指向北边，Z轴指向天顶。

1.3 LLA坐标系
也就是也叫经纬高坐标系(经度(longitude)，纬度(latitude)和海拔(altitude)LLA坐标系)。也叫做全球地理坐标系、大地坐标系、WGS-84坐标系。

1.4 GCJ02坐标系
GCJ02坐标系，也叫做火星坐标系，是中国国家测绘局制定的一种地理坐标系，它的值与LLA比较接近，LLA与GCJ02坐标系本质上可看成相互之间的微小偏移。

2 坐标转换
在经纬高坐标系下，数据并不直观，因此我们更希望将坐标数值转换为常规的直角坐标系（笛卡尔坐标系）下的数据。一种很常见的思想是，假设地球为椭球，然后以地心为原点建立直角坐标系，然后将地球表面的经纬高数据直接转换为对应的直角坐标系数据，也就是常说的LLA转ECEF。虽然这时的ECEF坐标数值已经是直角坐标系下的数值，因为这时的数值很大，受限于程序语言数值计算范围与精度，ECEF的数值在后续数据处理中容易产生数值异常或精度不足的问题。由于我们处理的数据，往往基于地球某一个区域，而这个区域相对于地球来说本身较小，因此这时会在这个区域选择一个锚点，然后以这个锚点为中心建立对应的坐标系，这时以这个区域建立的坐标数值就会小很多，不会带来计算溢出等问题，这时的转换就是ECEF转ENU。ECEF转ENU坐标系本身是直角坐标系下的平移与旋转，因此这种转换本身不破坏数据间的相对关系。

为了后续表示方便，这里选取WGS-84坐标系下经纬度参数，地球基准椭球体的极扁率 $f=$ 1/298.257223563，基准椭球体的长半径 $a=$ 6378137.0m，短半径 $b=a\sqrt{1-e^2}$ ，其中 $e^2=f(2-f)$ 。

2.1 LLA转ECEF
这里约定LLA的经度为 $\alpha$ ，纬度为 $\beta$ ，海拔为 $h$ 。LLA转ECEF的坐标 $(x,y,z)$ 为
(1)
$\begin{aligned} x &= (n+h)\cos \beta \cos \alpha \\ y &= (n + h)\cos \beta \sin \alpha \\ z &= (n(1-e^2)+h)\sin \beta \end{aligned}$

上面公式中的 $n$ 为如下值
(2)
$n = \dfrac{a}{\sqrt{1-e^2\sin^2\beta}}$
2.2 ECEF转LLA
ECEF坐标 $(x,y,z)$ 转LLA的经度 $\alpha$ ，纬度 $\beta$ ，海拔 $h$ 。
ECEF转LLA，本质上是求解公式(1)的非线性方程组，通过观察(1)中的第1与第2个等式，明显可以得到经度的计算表达式，即
(3)
$\alpha = \arctan(y, x)=\arctan(\dfrac{y}{x})$
接下来，还剩计算纬度与海拔。计算方法上，本质上还是求解非线性方程组。本文提供三种求解方法。

2.2.1 二分法求解

对(1)进行变形有
(4)
$\begin{aligned} x^2+y^2 &= (n+h)^2(1-\sin^2 \beta) \\ z &= (n(1-e^2)+h)\sin \beta \end{aligned}$
为了便于计算，这里设置如下中间量
(5)
$\begin{aligned} r &=\sqrt{x^2+y^2} \\ t &= \sin \beta \end{aligned}$
将(5)代入(4)消去 $h$ 可以得到
(6)
$t=(\dfrac{z}{r}+\dfrac{a}{r}\dfrac{e^2t}{\sqrt{1-e^2t^2}})\sqrt{1-t^2}$
这里(6)看似一个不动点迭代式，可惜这个算式并不是对每个值都迭代收敛。进一步，我们设置如下形式
(7)
$f(t)=(\dfrac{z}{r}+\dfrac{a}{r}\dfrac{e^2t}{\sqrt{1-e^2t^2}})\sqrt{1-t^2}-t$
从公式(7)我们可以发现，如下等式成立
(8)
$\begin{cases} f(1)\ \ \ =-1 < 0 \\ f(-1)=1 \ \ \ > 0 \end{cases}$
由于 $t$ 在[-1,1]区间取值，公式(8)满足使用二分法求解的条件。因此，使用二分法可以求解出满足精度的 $t$ 。最终在二分结束后，使用如下方式计算纬度与海拔
(9)
$\begin{aligned} \beta &=\arcsin(t) \\ h&=\begin{cases} \dfrac{r}{\sqrt{1-t^2}}-\dfrac{a}{\sqrt{1-e^2t^2}},\ |t|<0.1 \\ \\ \dfrac{z}{t}-\dfrac{a(1-e^2)}{\sqrt{1-e^2t^2}},\ |t| \geq 0.1 \end{cases} \end{aligned}$

因此，使用二分法求解步骤为：使用(7)进行二分求解出 $t$ ；使用(9)计算出纬度与海拔；使用(3)计算出经度。

二分法在求解出满意精度时，一般需要分割40次。

注意: 在(9)式计算海拔时，为了防止计算除以一个接近0的值，因此以0.1数值作为分段求解。

2.2.2 不动点迭代求解
二分法虽然有指数级收敛速度，但是依然不够快，这里考虑不动点迭代算法。

为了构建更快的迭代收敛式，这里设置如下中间变量
(10)
$s = (n+h)\sin\beta$
将(10)算式代入公式(4)的第1个方程有
(11)
$\sin\beta = \dfrac{s}{\sqrt{r^2+s^2}}$
将(11)代入公式(4)的第2个方程有
(12)
$s=z+\dfrac{ae^2s}{\sqrt{r^2+s^2-e^2s^2}}$
上面表达式即为不动点迭代式，为了证明这个迭代式满足不动点收敛迭代，这里设
(13)
$g(s)=z+\dfrac{ae^2s}{\sqrt{r^2+s^2-e^2s^2}}$
对于(13)我们有如下结果
(14)
$\begin{aligned} g(s_+)&=z+\dfrac{ae^2}{\sqrt{\dfrac{r^2}{s_+^2}+1-e^2}} <z+\dfrac{ae^2}{\sqrt{1-e^2}} \leq |z|+\dfrac{ae^2}{\sqrt{1-e^2}}<|z|+a \\ g(s_{-}) &=z-\dfrac{ae^2}{\sqrt{\dfrac{r^2}{s_-^2}+1-e^2}} >z-\dfrac{ae^2}{\sqrt{1-e^2}} \geq -|z|-\dfrac{ae^2}{\sqrt{1-e^2}}>-|z|-a \end{aligned}$
因为 $g(s)$ 在区间 $[-\infty, \infty]$ 内的取值在 $(-|z|-a, |z|+a)$ ，说明(12)算式在区间 $[-\infty,\infty]$ 存在不动点。

进一步
(15)
$\begin{aligned} g'(s)&=\dfrac{ar^2e^2}{(r^2+s^2-e^2s^2)^{1.5}} \\ &=\dfrac{ae^2}{\sqrt{r^2+s^2(1-e^2)}}\dfrac{1}{1+(\dfrac{s}{r})^2(1-e^2)} \\ &<\dfrac{ae^2}{\sqrt{r^2+s^2(1-e^2)}} \\ &<\dfrac{ae^2}{r} \leq \dfrac{ae^2}{b}=\dfrac{e^2}{\sqrt{1-e^2}} \\ &<\dfrac{1}{148.88}\end{aligned}$
因此有 $0<g'(s)<\dfrac{1}{148.88}$ , 结合(14)的结果，说明(12)算式在区间 $[-\infty, \infty]$ 存在不动点迭代，且迭代收敛到唯一值。从(15)可以发现迭代收敛速度较快。在实际迭代时，可以取 $s=0$ 或 $s=z$ 作为初值，然后迭代到满意精度。

最终，迭代完成后，可以使用如下方式获取纬度与海拔：
(16)
$\begin{aligned} \beta &= \arctan(s,r)=\arctan(\dfrac{s}{r}) \\ h&=\begin{cases} \sqrt{r^2+s^2}-n,\ |\beta|<\dfrac{7\pi}{16} \\ \\ \dfrac{z}{\sin\beta}-n(1-e^2),\ |\beta|\geq \dfrac{7\pi}{16} \end{cases} \end{aligned}$

因此，使用不动点迭代法求解步骤为：使用(12)迭代求解出 $s$ ；使用(16)计算出纬度与海拔；使用(3)计算出经度。

不动点迭代法在求解出满意精度时，一般迭代4-7次。

2.2.3 直接求解
使用不动点迭代速度已经很快，但如果还想确保每次计算调用时间可控，那就使用直接求解方法。

对公式(12)进行变形，可得到如下结果：
(17)
$(1-e^2)s^4-2z(1-e^2)s^3+(r^2+z^2(1-e^2)-a^2e^4)s^2-2zr^2s+z^2r^2=0$

很明显，(17)是一个关于 $s$ 的一元四次方程，由于一元四次方程有单独的求根公式，因此使用求根公式可以求出对应的解。

由于(12)式变到(17)式忽略了正负信息，因此对(17)求根，会存在多解的情况，这时可以将解代入(12)式进行验证，只有一个唯一解满足(12)式。因此最终求得 $s$ 。由于(12)式有一定计算量，为了快速筛掉不满足条件的解，通过公式(1)与(10)可以发现， $s$ 与 $z$ 的正负性完全一致，因此可以通过解的正负快速判断当前解是否一定是非解。

因此，使用直接法求解步骤为：对(17)式直接求解一元四次方程的解 $s$ ；使用(16)计算出纬度与海拔；使用(3)计算出经度。

由于使用的直接法求解，求解耗时主要在解一元四次方程的根。但本质上，这种方法求解最稳定、速度也很快，最主要是可控。

2.3 ECEF与ENU互转
ENU坐标，即将地心坐标经过平移旋转到地表指定点，让新坐标系的z轴指向天空，新坐标的y指向北极，新坐标x轴指向东方。ENU本质上需要一个参考锚点(这里称为Anchor点)，然后以这个锚点为原点创建的局部坐标系。假设Anchor的LLA坐标为 $(\alpha_0,\beta_0,h_0)$ ，Anchor的ECEF坐标为 $(x_0,y_0,z_0)$ ，一个点p的ECEF坐标为 $(x,y,z)$ ，p基于Anchor的ENU坐标为 $(x_{enu},y_{enu},z_{enu})$ ，则ECEF与ENU互转过程如下
(18)
$\begin{aligned} R_\alpha &=\begin{bmatrix} \cos(\dfrac{\pi}{2}+\alpha_0) &\sin(\dfrac{\pi}{2}+\alpha_0) &0 \\ -\sin(\dfrac{\pi}{2}+\alpha_0) & \cos(\dfrac{\pi}{2}+\alpha_0) & 0 \\ 0 &0&1 \end{bmatrix} \\ \\ R_\beta &=\begin{bmatrix} 1 &0 &0 \\ 0 &\cos(\dfrac{\pi}{2}-\beta_0) &\sin(\dfrac{\pi}{2}-\beta_0) \\ 0 &-\sin(\dfrac{\pi}{2}-\beta_0) & \cos(\dfrac{\pi}{2}-\beta_0) \end{bmatrix}\\ \\ S &=R_\beta R_\alpha=\begin{bmatrix} -\sin\alpha_0 & \cos \alpha_0 & 0 \\ -\sin \beta_0 \cos \alpha_0 & -\sin\beta_0\sin\alpha_0 &\cos\beta_0 \\ \cos\beta_0\cos\alpha_0 & \cos\beta_0\sin\alpha_0 & \sin\beta_0 \end{bmatrix} \\ \\ \begin{bmatrix} x_{enu} \\ y_{enu} \\ z_{enu} \end{bmatrix} &=S\begin{bmatrix} x-x_0\\ y-y_0\\ z-z_0 \end{bmatrix} \\ \\ \begin{bmatrix} x \\ y \\ z \end{bmatrix} &=S^T\begin{bmatrix} x_{enu}\\ y_{enu}\\ z_{enu} \end{bmatrix}+\begin{bmatrix} x_0 \\ y_0 \\ z_0 \end{bmatrix} \end{aligned}$

很明显上面的S矩阵就是一个旋转变换矩阵，

2.4 LLA与ENU互转
LLA与ENU互转，本质上可通过中间的ECEF坐标系进行转换。这里不再赘述。

3 不同锚点选择对数据间相对关系的影响

在数据处理过程中，难免会遇到对数据相对关系进行处理，比如两个点的空间距离，两个点在平面上的距离，两条线在空间的夹角，两个点在平面夹角…… 。

由于ECEF转ENU包含3D旋转操作，因此在不同锚点情况下构建的局部ENU坐标系，单独使用某一个轴（仅考虑转换后的x、y、z数据），或者某两个轴的数据(比如仅考虑所谓平面操作)，这时的处理会存在差异。比如以锚点A建立坐标系，这时处理这个坐标系下的平面数据(x-y)，其本质上来说锚点A坐标系下x与y的数据含有以锚点B坐标系下的(x-y-z)信息，因此如果以锚点B建立坐标系，这时如果也只处理平面数据(x-y), 在相同处理流程下，两套锚点得到的结果有所差异。

而对于3D空间数据，因为ECEF转ENU本质上是一个刚体变换(不带缩放的3D旋转+平移)，由于刚体变换不影响原始数据的相对关系，因此不同锚点的选取，并不影响结果。

因此最终有如下结论：

(1) 如果数据处理流程，包含单独针对某个轴的数据(x或y或z)的处理, 不同锚点的选取，对结果有所影响。

(2) 如果数据处理流程，包含单独针对平面(x-y或x-z或y-z)的处理, 不同锚点的选取，对结果有所影响。

(3) 如果数据处理流程，所有处理均在3D空间进行，不同锚点的选取，对结果没有影响。

4 坐标转换测试

目前MathSword软件提供了如下函数供坐标系数据转换: CoorECEFtoENU、CoorECEFtoLLA、CoorENUtoECEF、CoorENUtoLLA、CoorLLAtoECEF、CoorLLAtoENU

对极约束-2D点对匹配建立相机间相对测量约束

2022年11月6日2022年12月4日 shikang999 评论

1 约定
这里约定对于一个3D点位置为 $\mathbf{p}$ ，其坐标为( $x,y,z$ )，则 $\mathbf{p}$ 采用如下齐次坐标表示
(1-1)
$\mathbf{p}'=\begin{bmatrix} \mathbf{p} \\ 1 \end{bmatrix}=\begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}$
对于相机内参 $K$ ，约定 $K'$ 其为如下4 $\times$ 4齐次矩阵
(1-2)
$K'=\begin{bmatrix} K & \mathbf{0} \\ \mathbf{0}^T & 1 \end{bmatrix}=\begin{bmatrix} K_{00} & K_{01} & K_{02} & 0 \\ K_{10} & K_{11} & K_{12} & 0 \\ K_{20} & K_{21} & K_{22} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
对于相机位姿 $T$ ，这里约定的表示为世界坐标到相机坐标的变换，其满足如下定义
(1-3)
$T=\begin{bmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix}$

对于相机平面上的像素位置( $u,v$ )，深度为 $s$ ，用点 $\mathbf{g}$ 表示，其坐标形式为
$\mathbf{g} = \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}$

对于元素个数为1的向量，其表示形式与数值等价。

2 理论

2.1 问题
在空间中有个3D点 $\mathbf{p}$ 在相机1(位姿为 $T_1$ )的里的像素位置为 $\mathbf{g}_1$ 以及深度值为 $s_1$ ， $\mathbf{p}$ 在相机2(位姿为 $T_2$ )的里的像素位置为 $\mathbf{g}_2$ 以及深度值为 $s_2$ 。现在想建立相机1与相机2之间的相对测量信息。

2.2 相对测量
两个相机位姿间的相对测量不应该随着同一个刚体变换而改变，根据(1-3)的约定，这里两个相机的相对测量 $T$ 使用如下定义
(2-1)
$T = T_1T_2^{-1}$

注意：这里的相对测量是定义在整体刚体变换情况下，相对测量不变的基础上。也就是说，不管怎么移动，他们之间的相对测量值不再改变。

2.3 已知推导
根据2.1的信息有
(2-2)
$\begin{cases} s_1\begin{bmatrix} \mathbf{g}_1 \\ 1/s_1 \end{bmatrix}=K_1'T_1\mathbf{p}' \\ \\ s_2\begin{bmatrix} \mathbf{g}_2 \\ 1/s_2 \end{bmatrix}=K_2'T_2\mathbf{p}' \end{cases} \Rightarrow s_1K_1'^{-1}\begin{bmatrix} \mathbf{g}_1 \\ 1/s_1 \end{bmatrix}=T_1T_2^{-1}(s_2K_2'^{-1}\begin{bmatrix} \mathbf{g}_2 \\ 1/s_1 \end{bmatrix})=T(s_2K_2'^{-1}\begin{bmatrix} \mathbf{g}_2 \\ 1/s_1 \end{bmatrix})$
展开上面公式有
(2-3)
$s_1K_1^{-1}\mathbf{g}_1=R(s_2K_2^{-1}\mathbf{g}_2)+\mathbf{t}$
上面公式中的 $R$ 与 $t$ 来自 $T$ 。进一步，这里设如下中间变量
(2-4)
$\mathbf{a}_1=K_1^{-1}\mathbf{g}_1 \\ \mathbf{a}_2=K_2^{-1}\mathbf{g}_2$
将(2-4)带入公式(2-3)有
(2-5)
$\begin{aligned} s_1\mathbf{a}_1&=s_2R\mathbf{a}_2+\mathbf{t} \\ \Downarrow \\ s_1 \mathbf{t}^\wedge\mathbf{a}_1&=s_2 \mathbf{t}^\wedge R \mathbf{a}_2 \\ \Downarrow \\ s_1\mathbf{a}_1^T\mathbf{t}^\wedge\mathbf{a}_1&=s_2\mathbf{a}_1^T\mathbf{t}^\wedge R \mathbf{a}_2 \end{aligned}$
因为 $\mathbf{a}_1^T\mathbf{t}^\wedge\mathbf{a}_1 = 0$ ，因此代入(2-5)可以得到如下表达式
(2-6)
$\mathbf{a}_1^T\mathbf{t}^\wedge R \mathbf{a}_2 = 0$
因此，最终公式(2-6)就是我们导出的2D匹配点间相机间相关测量约束关系。

需要特别注意的是，(2-6)公式存在缺陷，如果使用这个约束作为优化的边，需要谨慎。

仔细观察(2-6)可以发现，当 $\mathbf{t}=\mathbf{0}$ 时， $R$ 取任意值，均满足上面的表达式，很明显，这种满足无穷组特解的模型对优化有一定误导作用。针对这种情况，需要增加有意义的约束，这里设 $\mathbf{t}=\mathbf{0}$ ，代入公式(2-5)可得到如下形式
(2-7)
$\mathbf{a}_1^\wedge R \mathbf{a}_2=\mathbf{0}$
因此，当 $\mathbf{t}=\mathbf{0}$ 时，直接使用公式(2-7)作为约束。

2.4 解的限定

虽然(2-6)与(2-7)给出了限定的约束条件，但这时使用的边依然存在多解的情况，而这些多解中有些并不满足实际的条件。在实际使用中，只有深度 $s_1,s_2$ 为正数，才符合我们预期，因此可以将这个判定条件，进一步加入约束。这里设置如下变量

(2-8) $\begin{aligned} \mathbf{b} &=\mathbf{a}_1 \\ \mathbf{c} &=R\mathbf{a}_2 \end{aligned}$

根据公式(2-5)，在 $||\mathbf{t}||_2^2 > 0$ 时有

(2-9)
$s_1\mathbf{b}=s_2\mathbf{c}+\mathbf{t}$

求解(2-9)有

(2-10) $\begin{cases} s_1&=\dfrac{t_2c_0-t_0c_2}{b_2c_0-b_0c_2}=\dfrac{t_2c_1-t_1c_2}{b_2c_1-b_1c_2}=\dfrac{t_1c_0-t_0c_1}{b_1c_0-b_0c_1} \\ s_2&=-\dfrac{b_0t_2-b_2t_0}{b_0c_2-b_2c_0}=-\dfrac{b_1t_2-b_2t_1}{b_1c_2-b_2c_1}=-\dfrac{b_0t_1-b_1t_0}{b_0c_1-b_1c_0}>0 \end{cases}$

根据(2-10)可得到如下等效判别式

(2-11)

$\begin{cases} (t_2c_0-t_0c_2)(b_2c_0-b_0c_2)>0 \\ (t_2c_1-t_1c_2)(b_2c_1-b_1c_2)>0 \\ (t_1c_0-t_0c_1)(b_1c_0-b_0c_1)>0 \\ (b_0t_2-b_2t_0)(b_0c_2-b_2c_0)<0 \\ (b_1t_2-b_2t_1)(b_1c_2-b_2c_1)<0 \\ (b_0t_1-b_1t_0)(b_0c_1-b_1c_0)<0 \end{cases}$

对于 $||\mathbf{t}||_2^2 = 0$ 时，这时公式(2-5)变成

(2-12)

$\begin{aligned} \dfrac{s_1}{s_2}\mathbf{b}&=\mathbf{c} \\ &\Downarrow \\ \dfrac{s_1}{s_2}&=\dfrac{c_0}{b_0}=\dfrac{c_1}{b_1}=\dfrac{c_2}{b_2}>0 \end{aligned}$

根据(2-12)可得到如下等效判别式

(2-13) $\begin{cases} b_0c_0>0 \\ b_1c_1>0 \\ b_2c_2>0 \end{cases}$

因此，当使用(2-6)算式约束时,使用(2-11)作为辅助判别约束；当使用(2-7)算式约束时，使用(2-13)作为辅助判别约束。

SE3在2D平面上的约束

2022年10月31日2022年11月1日 shikang999 评论

1 问题
有时，我们需要将一个3D空间的SE3变量映射到指定的2D平面，然后再加以求解。

为了方便说明，这里假设一个SE3的表现形式为 $T$ , 假设 $T$ 以如下值来表示

(1)
$T_1 =\begin{bmatrix} -0.69492055764131 & 0.71352099052778 & 0.08929285886191 & 4 \\ -0.19200697279199 & -0.30378504433947 & 0.93319235382364 & 5 \\ 0.69297816774177 & 0.63134969938371 & 0.34810747783026 & 8 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
1.1 x-y平面
为了取出x-y对应的旋转部分分量，这里定义如下变换矩阵
(2)
$T_{xy1} =\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}$
则经过 $T_{xy1}TT_{xy1}$ 可以取到 $T$ 中2*2旋转部分，执行后得到类似如下结果
(3)
$T_{xy1}TT_{xy1} =\begin{bmatrix} -0.69492055764131 & 0.71352099052778 & 0 & 0 \\ -0.19200697279199 & -0.30378504433947 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}$
为了取出x-y偏移部分，这里定义如下矩阵
(4)
$T_2 =\begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
则经过 $T_{xy1}TT_2$ 可以取到 $T$ 中2*1偏移部分，执行后得到类似如下结果
(5)
$T_{xy1}TT_2 =\begin{bmatrix} 0 & 0 & 0 & 4 \\ 0 & 0 & 0 & 5 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}$

这里定义矩阵
(6)
$\begin{aligned} T_{xy3}=T_{xy1}+T_2&= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \\ T_{xy4} &=\begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}$
最终可以按如下方式得到我们想要的矩阵
(7)
$T_{xy}'=T_{xy1}TT_{xy3}+T_{xy4}=\begin{bmatrix} -0.69492055764131 & 0.71352099052778 & 0 & 4 \\ -0.19200697279199 & -0.30378504433947 & 0 & 5 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
因此，对于一个SE3 $T$ ，通过公式(7)取出了x-y平面的变换量。

同样，对于一个3D点 $\mathbf{p}$ 可以通过如下变换矩阵得到x-y部分分量。
(8)
$\mathbf{p}'_{xy} = T_{xy1}\mathbf{p}+\mathbf{c}$

上面的向量 $\mathbf{c}$ 为如下向量

(9)

$\mathbf{c} = \begin{bmatrix}0\\0\\0\\1\end{bmatrix}$

1.2 x-z平面
为了取出x-z对应的旋转部分分量，这里定义如下变换矩阵
(10)
$T_{xz1} =\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}$
则经过 $T_{xz1}TT_{xz1}$ 可以取到 $T$ 中2*2旋转部分，执行后得到类似如下结果
(11)
$T_{xz1}TT_{xz1} =\begin{bmatrix} -0.69492055764131 & 0 & 0.08929285886191 & 0 \\ 0 & 0 & 0 & 0 \\ 0.69297816774177 & 0 & 0.34810747783026 & 0 \\ 0 & 0 & 0 & 0 & \end{bmatrix}$
则经过 $T_{xz1}TT_2$ 可以取到T中2*1偏移部分，执行后得到类似如下结果
(12)
$T_{xz1}TT_2 =\begin{bmatrix} 0 & 0 & 0 & 4 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 8 \\ 0 & 0 & 0 & 0 \end{bmatrix}$

这里定义矩阵
(13)
$\begin{aligned} T_{xz3}=T_{xz1}+T_2&= \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \\ T_{xz4} &=\begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}$
最终可以按如下方式得到我们想要的矩阵
(14)
$T_{xz}'=T_{xz1}TT_{xz3}+T_{xz4}=\begin{bmatrix} -0.69492055764131 & 0 & 0.08929285886191 & 4 \\ 0 & 1 & 0 & 0 \\ 0.69297816774177 & 0 & 0.34810747783026 & 8 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
因此，对于一个SE3 $T$ ，通过公式(13)取出了x-z平面的变换量。

同样，对于一个3D点 $\mathbf{p}$ 可以通过如下变换矩阵得到x-z部分分量。
(15)
$\mathbf{p}'_{xz} = T_{xz1}\mathbf{p}+\mathbf{c}$

1.3 y-z平面
为了取出y-z对应的旋转部分分量，这里定义如下变换矩阵
(16)
$T_{xz1} =\begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}$
则经过 $T_{yz1}TT_{yz1}$ 可以取到 $T$ 中2*2旋转部分，执行后得到类似如下结果
(17)
$T_{yz1}TT_{yz1} =\begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & -0.30378504433947 & 0.93319235382364 & 0 \\ 0 & 0.63134969938371 & 0.34810747783026 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}$
则经过 $T_{yz1}TT_2$ 可以取到 $T$ 中2*1偏移部分，执行后得到类似如下结果
(18)
$T_{yz1}TT_2 =\begin{bmatrix} 0 & 0 & 0 & 0 \\0 & 0 & 0 & 5 \\0 & 0 & 0 & 8 \\0 & 0 & 0 & 0 \end{bmatrix}$

这里定义矩阵
(19)
$\begin{aligned} T_{yz3}=T_{yz1}+T_2&= \begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \\ T_{yz4} &=\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}$
最终可以按如下方式得到我们想要的矩阵
(20)
$T_{yz}'=T_{yz1}TT_{yz3}+T_{yz4}=\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & -0.30378504433947 & 0.93319235382364 & 5 \\ 0 & 0.63134969938371 & 0.34810747783026 & 8 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
因此，对于一个SE3 $T$ ，通过公式(19)取出了y-z平面的变换量。

同样，对于一个3D点 $\mathbf{p}$ 可以通过如下变换矩阵得到y-z部分分量。
(21)
$\mathbf{p}'_{yz} = T_{yz1}\mathbf{p}+\mathbf{c}$

2 结论
1. 对于x-y平面映射，位姿或者变换矩阵使用公式(7)进行转换、空间点使用(8)进行转换。
2. 对于x-z平面映射，位姿或者变换矩阵使用公式(13)进行转换、空间点使用(14)进行转换。
3. 对于y-z平面映射，位姿或者变换矩阵使用公式(19)进行转换、空间点使用(20)进行转换。

3 案例
3.1 问题
空间中有一个射线A(其表示位姿为 $T_A$ )、与射线B(其表示位姿为 $T_B$ )，将射线A通过某个变换 $T$ 可以使得射线A与射线B在x-y平面完全重合。现在想建立 $T_A$ 、 $T_B$ ，以及变换 $T$ 的关系式。

3.2 求解
实际可以使用如下公式构建求解目标
(22)
$F=T_{xy1}(T_AT^{-1}-T_B)T_{xy3}=\mathbf{0}_{4\times4}$
为了方便李代数计算，分别取出第1、2、4列数据作为误差项，使用如下方式等效(21)的方程
(23)
$\begin{cases} \mathbf{e}_1=F\mathbf{a}=T_{xy1}(T_AT^{-1}-T_B)T_{xy3}\mathbf{a}=\mathbf{0} \\ \mathbf{e}_2=F\mathbf{b}=T_{xy1}(T_AT^{-1}-T_B)T_{xy3}\mathbf{b}=\mathbf{0} \\ \mathbf{e}_3=F\mathbf{c}=T_{xy1}(T_AT^{-1}-T_B)T_{xy3}\mathbf{c}=\mathbf{0} \end{cases}$
或者取出第1、2行数据作为误差项，使用如下方式等效(22)的方程
(24)
$\begin{cases} \mathbf{e}_1=F^T\mathbf{a}=T_{xy3}^T((T^{-1})^T T_A^T-T_B^T)T_{xy1}^T\mathbf{a}=\mathbf{0} \\ \mathbf{e}_2=F^T\mathbf{b}=T_{xy3}^T((T^{-1})^TT_A^T-T_B^T)T_{xy1}^T\mathbf{b}=\mathbf{0} \end{cases}$

上面的 $\mathbf{a},\mathbf{b}$ 定义如下
(25)
$\mathbf{a}=[1,0,0,0]^T \\ \mathbf{b}=[0,1,0,0]^T$
因此最终可以使用(23)或(24)算式来进行求解。

空间直线匹配建立相关约束

2022年10月28日2022年11月18日 shikang999 评论

1 空间中两条直线匹配建立关系
现在已知空间直线A方程为
(1)
$\mathbf{p}_1 = \mathbf{n}_1\alpha + \mathbf{p}_1'$

现在已知空间直线B方程为
(2)
$\mathbf{p}_2 = \mathbf{n}_2\beta + \mathbf{p}_2'$

上面公式中 $\mathbf{n}_1$ 为直线A单位方向向量， $\mathbf{n}_2$ 为直线B单位方向向量。现在将直线A进行( $R,\mathbf{t}$ )变换后，能与直线B完全重合。现在想建立直线A、B、以及( $R,\mathbf{t}$ )的关系。

构建关系有多种方式，比如将线上变换后的点到目标线的距离作为约束；另一种法方式是将变换后的点投到目标线参数方程上, 如果点在线上，则满足参数方程约束。本文建立关系则基于后一种方式。

1.1 考虑方向向量匹配
有时，我们能够确定直线A与直线B在A通过( $R,\mathbf{t}$ )变换后,其变换后的直线与直线B完全重合,并且表示直线的单位方向向量也完全重合, 则这时必有如下关系成立
(3)
$\mathbf{n}_2 = R\mathbf{n}_1$
根据直线变换后完全重合的条件有
(4)
$\begin{aligned} R\mathbf{p}_1+\mathbf{t}&= R(\mathbf{n}_1\alpha + \mathbf{p}_1')+\mathbf{t} \\ &=\mathbf{p}_2 \\ &= \mathbf{n}_2\beta + \mathbf{p}_2' \\ &= R\mathbf{n}_1\beta+\mathbf{p}_2' \\ &\Downarrow \\ R\mathbf{n}_1(\beta-\alpha)&=R\mathbf{p}_1'+\mathbf{t} – \mathbf{p}_2' \\ &\Downarrow \\ (\beta-\alpha)\mathbf{a} &= \mathbf{b} \end{aligned}$
上面的中间变量 $\mathbf{a}$ 与 $\mathbf{b}$ 定义如下
(5)
$\begin{aligned} \mathbf{a} &= R \mathbf{n}_1 \\ \mathbf{b} &= R\mathbf{p}_1'+\mathbf{t} – \mathbf{p}_2' \end{aligned}$
在公式(4)中，任意取一个 $\beta$ 均有一个唯一对应的 $\alpha$ .即个 $\beta-\alpha$ 为一个常量,由于个 $\beta$ 的任意性,很明显个 $\mathbf{a}$ 与个 $\mathbf{b}$ 满足如下关系
(6)
$\dfrac{1}{|\mathbf{a}|}\mathbf{a}=\dfrac{1}{|\mathbf{b}|}\mathbf{b}$
虽然上面公式(6)可以直接构建约束,但由于涉及到向量取模,一种等效的方式是使用如下方程来约束
(7)
$\begin{cases} a_0b_1=a_1b_0 \\ a_0b_2=a_2b_0 \\ a_1b_2=a_2b_1 \end{cases}$
公式(7)中有效约束实际为2个方程，实际使用时，可以任意选则两个方程进行构建。很明显，公式(7)的计算量比公式(6)小了很多，因此实际情况下可使用公式(7)来构建约束。

另一种方式是使用如下方式构建，针对公式(4)有
(8)
$\begin{aligned} R\mathbf{n}_1(\beta-\alpha)&=R\mathbf{p}_1'+\mathbf{t} – \mathbf{p}_2' \\ &\Downarrow \\ \mathbf{p}_2'^\wedge R\mathbf{n}_1(\beta-\alpha) &= \mathbf{p}_2'^\wedge (R\mathbf{p}_1'+\mathbf{t}) \\ &\Downarrow \\ 0 = (R\mathbf{n}_1)^T \mathbf{p}_2'^\wedge R\mathbf{n}_1(\beta-\alpha) &= (R\mathbf{n}_1)^T \mathbf{p}_2'^\wedge (R\mathbf{p}_1'+\mathbf{t}) \end{aligned}$
同理，可以得到如下约束表达式
(9)
$\begin{cases} (R\mathbf{n}_1)^T \mathbf{p}_2'^\wedge (R\mathbf{p}_1'+\mathbf{t}) = 0 \\ (R\mathbf{n}_1)^T \mathbf{t}^\wedge (R\mathbf{p}_1'-\mathbf{p}_2')=0 \end{cases}$

因此最终可以使用公式(3)与(7)来构建约束方程或者 (3)与(9)来构建约束方程。

1.2 不考虑方向向量匹配
有的情况下，我们无法保证提供的两个直线方向向量在变换后是否一致，这时，我们基于的理论是：A直线上的点变换后，一定在B直线上。则根据变换条件有
(10)
$\begin{aligned} R\mathbf{p}_1+\mathbf{t}&= R(\mathbf{n}_1\alpha + \mathbf{p}_1')+\mathbf{t} \\ &=\mathbf{p}_2 \\ &= \mathbf{n}_2\beta + \mathbf{p}_2' \\ &\Downarrow \\ \mathbf{n}_2\beta&=\alpha R\mathbf{n}_1+R\mathbf{p}_1'+\mathbf{t} – \mathbf{p}_2' \\ &\Downarrow \\ \beta\mathbf{n}_2&=\alpha \mathbf{a} + \mathbf{b} \\ &\Downarrow \\ \beta \mathbf{b}^\wedge \mathbf{n}_2 &= \alpha \mathbf{b}^\wedge \mathbf{a} \\ &\Downarrow \\ \beta \mathbf{n}_2^T \mathbf{b}^\wedge \mathbf{n}_2 &= \alpha \mathbf{n}_2^T \mathbf{b}^\wedge \mathbf{a} \\ &\Downarrow \\ \mathbf{n}_2^T \mathbf{b}^\wedge \mathbf{a} &= 0 \end{aligned}$
即从(10)中我们可以得到一个约束
(11)
$\mathbf{n}_2^T \mathbf{b}^\wedge \mathbf{a} = 0$
虽然(11)算式反应了两条直线匹配的关系，但是还有一个隐藏约束没有用到，即两条直线上的点一一对应。即，每个 $\beta$ 对应着一个唯一的 $\alpha$ ，上面公式在约去 $\beta$ 的过程中，并没有反映其一一对应性，为了进一步找出约束关系，这里设
(12)
$\mathbf{n}_2=\begin{bmatrix} x \\ y \\ z \end{bmatrix}$

1.2.1 消去x
当 $\mathbf{n}_2$ 中绝对值元素最大的值为 $x$ ,则消去(10)中的 $\beta$ 有
(13)
$\begin{aligned} \begin{bmatrix} \dfrac{y}{x} \\ \\ \dfrac{z}{x} \end{bmatrix}&= \begin{bmatrix} \dfrac{\alpha a_1+b_1}{\alpha a_0 + b_0} \\ \\ \dfrac{\alpha a_2+b_2}{\alpha a_0 + b_0} \end{bmatrix} \\ &\Downarrow \\ &\begin{cases} \alpha(a_0y-a_1x)+(b_0y-b_1x)=0 \\ \alpha(a_0z-a_2x) +(b_0z-b_2x)=0 \end{cases} \end{aligned}$
由于每个 $\beta$ 对应着唯一的 $\alpha$ , 当 $\beta$ 取任意值时, $\alpha$ 也可以看成可取任意值.由于 $\beta$ 取任意值均需要满足上面表达式,则可以得到如下约束方程
(14)
$\begin{cases} a_0y=a_1x \\ b_0y=b_1x \\ a_0z=a_2x \\ b_0z=b_2x \end{cases}$
上面公式中隐含了 $a_0b_1=a_1b_0$ 与 $a_0b_2=a_2b_0$ 。因此最终使用(14)算式构建约束。

1.2.2 消去y
当 $\mathbf{n}_2$ 中绝对值元素最大的值为 $y$ ,则消去(10)中的 $\beta$ 有
(15)
$\begin{aligned} \begin{bmatrix} \dfrac{x}{y} \\ \\ \dfrac{z}{y} \end{bmatrix}&= \begin{bmatrix} \dfrac{\alpha a_0+b_0}{\alpha a_1 + b_1} \\ \\ \dfrac{\alpha a_2+b_2}{\alpha a_1 + b_1} \end{bmatrix} \\ &\Downarrow \\ &\begin{cases} \alpha(a_1x-a_0y)+(b_1x-b_0y)=0 \\ \alpha(a_1z-a_2y) +(b_1z-b_2y)=0 \end{cases} \end{aligned}$
由于每个 $\beta$ 对应着一个 $\alpha$ , 当 $\beta$ 取任意值时, $\alpha$ 也可以看成可取任意值.由于 $\beta$ 取任意值均需要满足上面表达式,则可以得到如下约束方程
(16)
$\begin{cases} a_1x=a_0y \\ b_1x=b_0y \\ a_1z=a_2y \\ b_1z=b_2y \end{cases}$
上面公式中隐含了 $a_0b_1=a_1b_0$ 与 $a_1b_2=a_2b_1$ 。因此最终使用(16)算式构建约束。

1.2.3 消去z
当 $\mathbf{n}_2$ 中绝对值元素最大的值为 $z$ ,则消去(10)中的 $\beta$ 有
(17)
$\begin{aligned} \begin{bmatrix} \dfrac{x}{z} \\ \\ \dfrac{y}{z} \end{bmatrix}&= \begin{bmatrix} \dfrac{\alpha a_0+b_0}{\alpha a_2 + b_2} \\ \\ \dfrac{\alpha a_1+b_1}{\alpha a_2 + b_2} \end{bmatrix} \\ &\Downarrow \\ &\begin{cases} \alpha(a_2x-a_0z)+(b_2x-b_0z)=0 \\ \alpha(a_2y-a_1z) +(b_2y-b_1z)=0 \end{cases} \end{aligned}$
由于每个 $\beta$ 对应着一个 $\alpha$ , 当 $\beta$ 取任意值时, $\alpha$ 也可以看成可取任意值.由于 $\beta$ 取任意值均需要满足上面表达式,则可以得到如下约束方程
(18)
$\begin{cases} a_2x=a_0z \\ b_2x=b_0z \\ a_2y=a_1z \\ b_2y=b_1z \end{cases}$
上面公式中隐含了 $a_0b_2=a_2b_0$ 与 $a_1b_2=a_2b_1$ 。因此最终使用(18)算式构建约束。