1.4 优化估计应用

优化算法应用非常广泛，例如无人驾驶SLAM中的点云配准、VR/AR中的三维注册，以及2D/3D的相机标定等。

例如，西安兵马俑残片和甘肃敦煌塑像碎片的点云配准，即为3D空间实际工程应用。为便于演示算法效果，本节以简化版本的2D配准为例，说明优化技术的工程应用。

图1-6中，照片破损后灰色的佩奇从合照中脱落。如何将残片粘贴到合照中，即求解以下参数：给定2D的点对 ，求解平面刚体运动的平移参数 a 、 b 和弧度 θ 。

1.4.1 L-M算法

使用L-M算法求解旋转角度 θ 和平移向量三个运动参数，假设先平移然后旋转，记平移参数为 a ， b ，旋转弧度为 t 。根据齐次运动方程

由一组点对，可得方程组：

可以仅用其中一个方程，当然也可以同时使用方程组，即可对决策参数{ t ， a ， b }进行优化。

不过由于三角函数非线性太强，配准结果可能效果不佳。因此，令 ，即

则新方程组为

记优化参数为 ，则残差函数为

优化结果可以满足要求。

不过 的变换丢失了旋转角度 t 的方向信号。

为此，将 t →0的三角函数进行泰勒级数展开：

从而优化函数为

记向量 为优化参数，则残差向量 r （ x ）的分项为

关键代码如下：

残差向量 r （ x ）的雅可比矩阵 J _r 的分项元素为

关键代码如下：

     //计算雅可比矩阵的分项Jri
     //即决策向量x（t,a,b）的一阶偏导
     double Ji1,Ji2,Ji3;
     Ji1=（-1.0+*t_+*t_**t_/2.0）*（U+*a_）+（1.0+*t_-*t_**t_/2.0）*（V+*b_）；
     Ji2=-1.0-*t_+*t_* *t_/2.0+*t_* *t_* *t_/6.0;
     Ji3=-1.0+*t_+*t_* *t_/2.0-*t_* *t_* *t_/6.0;
     Jr_（i,0）=Ji1;
     Jr_（i,1）=Ji2;
     Jr_（i,2）=Ji3;

获得残差向量 r （ x ）和雅可比矩阵 J _r 之后，即可计算等效Hessian矩阵 和等效梯度 。

关键代码如下：

优化结果如图1-7所示。

1.4.2 加权L-M算法

配准结果有时整体效果良好，但是重点的局部细节失配。此时希望实现人工干预的调控功能，对于关键位置实现重点匹配。例如，图1-8中浅色圈注的佩奇耳朵部位需要重点关注，尽量实现精确配准。

考虑目标函数为向量点乘形式 f （ x ）= r ^T r ，呈现为欧氏距离，将其扩展为马氏距离 r ^T Wr ，同时将协方差矩阵 W ∈ R ^{m × m} 加强为对角阵：

其中， W 即关于样本数据的权值矩阵， w _i 对应第 i 组样本数据的权值。

记 为优化参数，则

（1）残差向量 r （ x ）可以通过分项元素 r _i （ x ）= w _i { m _i - M _i （ x ）}计算。

（2）残差向量 r （ x ）的雅可比矩阵 J _r 可以通过分项元素计算。

获得残差向量 r （ x ）和残差分量 r （ x ）的雅可比矩阵 J _r 之后，对于等效Hessian矩阵 和等效梯度 可以重新推导，其实只是多出对角阵 W 而已。具体过程如下：

（1）此时的目标函数 f （ x ）为

（2）目标函数 f （ x ）的一阶导数为

即雅可比矩阵为

（3）目标函数 f （ x ）的二阶导数为

假设其中二阶项可以忽略，即Hessian矩阵为

（4）此时增量方程为

注意： 以上仅为算法优化部分，实际工程实现时还要考虑离群点的粗差剔除，可以使用1.4.3节的迭代加权法或者1.4.4节的RANSAC法。另外，还要考虑坐标的归一化问题。

1.4.3 迭代加权M估计

1.4.2节的加权L-M算法中需要通过人工交互的方式提供权值系数，而图像优化配准中不可避免要遇到的问题是数据噪声。对于测量数据，误差是无处不在的，典型的包括两类：不精确测量导致的系统误差，服从高斯分布；错误测量导致的粗差，一般不服从高斯分布。后面这类样本数据点，一般称为离群值、粗差点、外点（Outliers）。

观测数据粗差剔除的常用算法包括RANSAC和M估计方法，M估计一般通过迭代加权法（Iterative Reweighted Least Squares，IRLS，即迭代加权最小二乘）求解。

IRLS原理是在迭代过程中通过残差更新权值，给粗差数据赋予接近于零的小权值，给可靠数据赋予接近于单位1的大权值。具体步骤为：首先初始化权值为单位权，进行最小二乘平差；然后根据平差结果，基于选定的权函数更新观测数据的权，利用新权值最小二乘平差；重复该过程直至系统收敛。

设问题包含 n 维决策参数 x ∈ R ⁿ ，给定 m 组观测数据 ，记第 i 组样本的残差为 r _i = r _i （ x ），则M估计问题为

其中， ρ （·）为对称的、正定的权函数， ρ （·）计算得到的是每个残差分项对系统的影响。

对照非线性优化最小二乘问题：

可见它就是 版本的M估计，即标准最小二乘是M估计的一种特殊退化。此外如果取 ρ （ r _i ）=| r _i |，此时M估计就退化为最小一乘法估计，即最小绝对偏差（Least Absolute Deviation，LAD）。

除了 的最小二乘和| r _i |的最小一乘外，回归M估计常用权函数还有：

（1）Welsh函数。

其中， c 为经验常数。

（2）Tukey函数。

其中， c 为残差阈值。

Tukey函数的图像很好：如果残差较小，| r _i |≤ c ，则 ρ （ r _i ）随着| r _i |增加；如果残差较大，| r _i |＞ c ，则函数 ρ （ r _i ）为固定值。

例如，Halcon的圆弧拟合fit_circle_contour_xld算子，提供有Huber和Tukey两种权函数，默认采用Tukey函数。

以下通过一个具体示例说明M估计的应用。

对于3D空间中的平面拟合问题，给定点云数据 ，M估计就是优化问题：

其中， r _i 为第 i 点距离拟合平面的距离，权函数 ρ （·）采用Welsh函数 。

点 p _i 到平面的距离为 p _i 与平面任意一点 q 所组成的向量与法线 n 的数量积，将 q 利用质心 进行中心化，记中心化之后的向量为 ，则 p _i 到平面的距离为 r _i = ，从而平面拟合问题为

此为带范数约束的最小二乘问题，引入拉格朗日乘子：

对式（1-65）求导，并令导数为零，可得协方差矩阵：

通过对方阵 C 进行奇异值分解，则最小奇异值对应向量即为平面法向量 ，具体算法如算法1-5所示。

1.4.4 粗差剔除RANSAC

基于测量所得数据点对，无论是单目标定的投射矩阵，或是多目标定的本质矩阵、基础矩阵，以及点云配准的外参矩阵，都属于广泛意义上的模型估计。除1.4.3节的迭代加权M估计方法，随机采样一致性（RANdom SAmple Consensus，RANSAC）是另外一种外点检测的迭代方法，该方法可以从一组包含离群值的观测数据中估计数学模型（的参数），不使离群值对估计产生影响，在一定概率下得到合理结果。

RANSAC最初用于解决图1-9所示的位置确定问题（Location Determination Problem，LDP），即SLAM中相机的位姿估计问题：给定3D世界坐标系下 m 个坐标已知的控制点（地标），以及图像中对应的 m 个投影，确定3D空间中的相机位姿。

RANSAC基本假设如下：

（1）整个数据集由内点和外点组成；

（2）内点尽管存在噪声，但是其分布可以由参数化模型解释；

（3）离群值来自错误测量方法、数据错误假设等，不适合模型解释。

RANSAC可以理解为一种采样方式：使用比较小的数据子集，然后尽可能地使用一致的数据来扩大原来初始化的数据集，因此对于多项式拟合、混合高斯模型等在理论上都是适用的。

以下通过一个具体示例说明RANSAC的应用。

对于2D空间中的圆弧拟合，RANSAC首先选择三个点构成一个子集，计算中心及半径，从而确定弧线，然后计算其他点是否足够靠近该圆弧，其他点的偏差足够小可以认为是测量的误差。如果有足够符合这个圆弧的点，则RANSAC将会使用平滑技术（最小二乘）来更好地估计这个圆的参数，从而相互一致的点的集合也就确定了，具体算法如算法1-6所示。

RANSAC除了数据和模型之外还需要根据特定问题和数据集通过实验确定合适的参数 n 、 k 、 t 、 d ，其中 n 、 t 、 d 可以基于经验得到， k 可以根据公式计算。

图1-10为利用算法1-6计算单应矩阵时的匹配效果，其中图1-10（a）为未剔除粗差的匹配结果，图1-10（b）为使用RANSAC剔除粗差之后的匹配结果，可见效果改善明显。