高斯牛顿法去畸变（C++实现）

1. 背景

在实际三维重建时，通常会遇到如下问题：
已知有畸变的点坐标$x^{\prime },y^{\prime }$，已知畸变模型$f(x,y),g(x,y)$, 求无畸变的点坐标$x,y$。畸变模型为
$$\begin{array}{c}{x}^{\prime }=x+f(x,y)\\ y^{\prime }=y+g(x,y)\end{array}$$
对于畸变模型通常是非线性函数来说，通过左边的有畸变坐标解算无畸变坐标，解析方法难度很大。因此，数值求解的方法更适用于去畸变的情况。

2. 一维高斯牛顿法

先来看该问题的一维表示
$$y=x+f(x)$$
相当于已知$y$和$f(x)$，求解x。令
$$F(x)=x+f(x)-y$$
该问题等价求$F(x)=0$时的x, 在数值分析上就是求最小化时的x，即
$$x=argmin||F(x)||$$
为了求导的方便，通常等价为如下形式
$$x=arg\mathrm{m}\mathrm{i}\mathrm{n}\frac{1}{2}||\mathrm{F}(\mathrm{x})|{|}^2$$
把$F(x)$在$x$处进行泰勒展开
$$F(x+\Delta x)=F(x)+F^{\prime }(x)\Delta x+\frac{F^{\prime \prime }(x)}{2}\Delta x^2+...$$
高斯牛顿法的思想是取其一阶线性近似为
$$F(x+\Delta x)\approx F(x)+F^{\prime }(x)\Delta x$$
用雅可比矩阵$J(x)$来表示一阶导数
代入式(5)中，求最小化的问题需要对目标求导，求一阶导数为0时的x
$$\frac{1}{2}||F(x+\Delta x)|{|}^2=\frac{1}{2}[F(x{)}^{T}F(x)+2F(x{)}^{T}J(x)\Delta x+\Delta x^{T}J(x{)}^{T}J(x)\Delta x]$$
对上式求导，并令导数为0，得到
$$J(x{)}^{T}J(x)\Delta x=-J(x{)}^{T}F(x)$$
那么
$$\Delta x=(J(x{)}^{T}J(x){)}^{-1}(-J(x{)}^{T}F(x))$$

这样获得了增量$\Delta x$的大小，则x的迭代为
$$x_{k+1}=x_k+\Delta x=x_k+(J(x_k{)}^{T}J(x_k){)}^{-1}(-J(x_k{)}^{T}F(x_k))$$
对该迭代过程限制最大迭代次数或者增量小于最小改变量来求解得到最终的x。

3. 高斯牛顿去畸变求解过程

参照一维高斯牛顿法的过程，对于式(1)的畸变模型，令残差
$$r(x,y)=\left[\begin{array}{c}F(x,y)\\ G(x,y)\end{array}\right]$$
其中，
$$\begin{gathered} F(x,y)=x+f(x,y)-x^{\prime } \\ G(x,y)=y+g(x,y)-y^{\prime } \end{gathered}$$

那么去畸变的结果就是
$$(x,y)=arg\mathrm{m}\mathrm{i}\mathrm{n}\frac{1}{2}||r(x,y)|{|}^2$$
根据式(10)，
$$\left[\begin{array}{c}\Delta x\\ \Delta y\end{array}\right]=(J(x,y{)}^{T}J(x,y){)}^{-1}(-J(x,y{)}^{T}r(x,y))$$
其中，
$$J(x)=\left[\begin{array}{cc}\frac{\partial F(x,y)}{\partial x}& \frac{\partial F(x,y)}{\partial y}\\ \frac{\partial G(x,y)}{\partial x}& \frac{\partial G(x,y)}{\partial y}\end{array}\right]$$
同样地，根据式(11)迭代计算最后去畸变的结果。

为了更好说明计算过程，用具体的数据来进行推导。通常畸变模型是高阶非线性的，但为了说明方便，这里假设畸变模型为双线性，如下：

$$\begin{gathered} f(x,y)=（1-x-y+xy)\ast k_1+(x-xy)\ast k_2+(y-xy)\ast k_3+xy\ast k_4 \\ g(x,y)=（1-x-y+xy)\ast k_5+(x-xy)\ast k_6+(y-xy)\ast k_7+xy\ast k_8 \end{gathered}$$

求上式的一阶偏导数
$$\begin{array}{c}\frac{\partial F(x,y)}{\partial x}=1+\frac{\partial f(x,y)}{\partial x}=1+(-1+y)\ast k_1+(1-y)\ast k_2+(-y)\ast k_3+y\ast k_4\\ \frac{\partial F(x,y)}{\partial y}=\frac{\partial f(x,y)}{\partial y}=(-1+x)\ast k_1+(-x)\ast k_2+(1-x)\ast k_3+x\ast k_4\\ \frac{\partial G(x,y)}{\partial x}=\frac{\partial g(x,y)}{\partial x}=(-1+y)\ast k_5+(1-y)\ast k_6+(-y)\ast k_7+y\ast k_8\\ \frac{\partial G(x,y)}{\partial y}=1+\frac{\partial g(x,y)}{\partial y}=1+(-1+x)\ast k_5+(-x)\ast k_6+(1-x)\ast k_7+x\ast k_8\end{array}$$
带入式(16)得到雅可比矩阵，而残差为
$$r(x,y)=\left[\begin{array}{c}F(x,y)\\ G(x,y)\end{array}\right]=\left[\begin{array}{c}x+（1-x-y+xy)\ast k_1+(x-xy)\ast k_2+(y-xy)\ast k_3+xy\ast k_4-x^{\prime }\\ y+（1-x-y+xy)\ast k_5+(x-xy)\ast k_6+(y-xy)\ast k_7+xy\ast k_8-y^{\prime }\end{array}\right]$$
假设待定参数$x$, $y$的初始值就是有畸变的值，即初始值为$x=x^{\prime }$, $y=y^{\prime }$。将$x$, $y$的初始值带入雅可比矩阵，以及残差矩阵中，得到式(15)的增量值，然后利用如式(11)迭代去畸变。

4. c++实现

利用Eigen矩阵库辅助完成去畸变的c++实现

完整工程：
csdn: 三维重建中高斯牛顿法求解去畸变坐标
github：DistortionRemoval

int RemoveDistortion(double &distX, double &distY, double srcX, double srcY, double *Kx, double *Ky)
{
	// 去畸变坐标的初始值设置为有畸变的坐标
	distX = srcX;
	distY = srcY;

	int iMaxItetNum = 5;
	int iIterCnt = 0;
	Eigen::MatrixXd delta;
	delta.setOnes(2, 1);

	// 迭代去畸变
	do{
		// 雅可比矩阵
		Eigen::Matrix2d Jacobi;
		Jacobi(0, 0) = 1 + (-1 + distY)*Kx[0]
			+ (1 - distY)*Kx[1]
			+ (-distY)*Kx[2]
			+ distY*Kx[3];
		Jacobi(0, 1) = (-1 + distX)*Kx[0]
			+ (-distX)*Kx[1]
			+ (1 - distX)*Kx[2]
			+ distX*Kx[3];
		Jacobi(1, 0) = (-1+distY)*Ky[0]
			+ (1 - distY)*Ky[1]
			+ (-distY)*Ky[2]
			+ distY*Ky[3];
		Jacobi(1, 1) = 1 + (-1 + distX)*Ky[0]
			+ (-distX)*Ky[1]
			+ (1 - distX)*Ky[2]
			+ distX*Ky[3];

		// 残差矩阵
		Eigen::MatrixXd R;
		R.setZero(2, 1);
		R(0, 0) = distX + (1 - distX - distY + distX*distY)*Kx[0]
			+ (distX-distX*distY)*Kx[1]
			+ (distY-distX*distY)*Kx[2]
			+ distX*distY*Kx[3]
			- srcX;
		R(1, 0) = distY + (1 - distX - distY + distX*distY)*Ky[0]
			+ (distX - distX*distY)*Ky[1]
			+ (distY - distX*distY)*Ky[2]
			+ distX*distY*Ky[3]
			- srcY;

		// 求解增量
		delta = (Jacobi.transpose() * Jacobi).inverse();
		delta *= Jacobi.transpose() * R *(-1);

		// 下一次迭代
		iIterCnt++;
		distX = distX + delta(0, 0);
		distY = distY + delta(1, 0);
	} while (iIterCnt < iMaxItetNum && (abs(delta(0, 0)) > 1e-3 || abs(delta(1, 0)) > 1e-3));

	return 0;
}