高斯牛顿法进行三维球拟合(含C++代码)

最小二乘法

\quad 我们假设目标函数为$F(x)$，误差函数为 $f(x)$，则最小二乘可表示为$$minF(x)=\frac{1}{2}\parallel f(x)\parallel {}_2^2$$ \quad 那么如何求解这个问题呢？
\quad 常规的思路就是对目标函数$F(x)$进行求导，当其导数为0的时候，求$x$的最优解，最终求得极值，这里需要注意导数为0的点，不一定就是极值点，可能会是鞍点，也就是我们所说的局部最优解。然而，有些时候，若目标函数$F(x)$不是很容易进行求导，这时就需要使用一些迭代的方法，来使得目标函数$F(x)$下降，这样就可以逐步迭代，求得最小值了。整个的迭代流程如下所示：
\quad 因此，上述问题就变成了不断寻找下降增量$\Delta x_k$的问题。为了方便求解，我们只需要关心误差函数$f(x)$在迭代值处的局部性质，而不用考虑目标函数$F(x)$在迭代值处的全局性质。
\quad 下面介绍常见的两种优化算法：最速下降法(梯度下降法)，它是用于找到可微函数的局部最小值的一阶迭代优化算法，如果实值函数$f(x)$在点$x=a$处可微且有定义，那么函数$f(x)$在点$x=a$处，沿着梯度的反方向$-\Delta f(a)$下降的最快，即之前提到需求解的增量$\Delta x_k=-\Delta f(a)$。
当然，我们还需要该方向上取一个步长 λ，求得最快的下降方式，即$x_{n+1}=x_n-\lambda \Delta f(a)$。它的缺点是过于贪心，容易走出锯齿路线，反而增加了迭代次数。
而牛顿法则是一个二阶梯度算法，它的增量的解可以从$H\Delta x_k=-J(x{)}^T$(具体推导过程可以查阅相关资料)解出。为了求解这个方程，此时需要计算目标函数的Hessian矩阵，这在问题规模较大时计算量非常大并且很难求解。

高斯牛顿法

\quad 前两种算法都是针对目标函数$F(x)$求解，不可避免遇到求Hessian矩阵的问题，而高斯牛顿法则选取了误差函数$f(x)$来进行优化求解,它的思想是将 $f(x)$ 进行一阶的泰勒展开:$$f(x+\Delta x)\approx f(x)+{\mathbf{J}(\mathbf{x})}^T\Delta x$$ \quad 上式中的${\mathbf{J}(\mathbf{x})}^T$为$f(x)$的雅可比矩阵，或者简单来说，就是$f(x)$关于$x$的一阶导数。那么，我们从上面的迭代步骤2中可以看到,当前的目标是为了寻找下降矢量$\Delta x$，使得$\parallel f(x+\Delta x)\parallel {}_2^2$达到最小。因此，我们的最小二乘问题变成如下问题：$$\Delta x^{\ast }=argmin\frac{1}{2}\parallel f(x+\Delta x)\parallel {}_2^2\approx argmin\frac{1}{2}\parallel f(x)+{\mathbf{J}(\mathbf{x})}^T\Delta x\parallel {}_2^2$$
对上式进行关于$\Delta x$求导，并令其为0，具体推到可以查阅资料，于是得到如下方程组：
$${\mathbf{J}(\mathbf{x})}^T\mathbf{J}(\mathbf{x})\Delta x=-{\mathbf{J}(\mathbf{x})}^{T}f(x)$$这个线性方程有很多种名称，如：增量方程，高斯牛顿方程或正规方程。我们把${\mathbf{J}(\mathbf{x})}^T\mathbf{J}(\mathbf{x})$定义为$H(x)$,把$-{\mathbf{J}(\mathbf{x})}^{T}f(x)$定义为$g(x)$，此时变成了：$$H\Delta x=g$$这样，就可以优化求解了。上面的最小二乘的优化步骤就可以变为：

对比之前的牛顿法，两个式子中$H(x)$含义不一样了。当然，这样的算法也有一定的问题，例如：如果求出来的步长$\Delta x_k$太大，会导致其局部近似不精确，严重的时候，可能无法保证迭代收敛。L-M算法又在这个基础上进行了改进，不过它不在这篇文章的讨论范围。

三维球拟合

\quad 任何拟合算法都会有一定的局限性，回到主题，我们将利用高斯牛顿法拟合三维球，空间三维球的方程为：$$(x-a{)}^2+(y-b{)}^2+(z-c{)}^2=r^2$$其中，$a,b,c$分别为球心的xyz坐标，$r$为球半径。
对于这个方程，我们的目标是：找到参数$a,b,c,r$合适的取值来求解下面这个最小二乘问题$$\underset{a,b,c,r}{\min }\sum _{i=1}^n\frac{1}{2}\parallel f_i(a,b,c,r)\parallel {}_2^2=\underset{a,b,c,r}{\min }\sum _{i=1}^n\frac{1}{2}\parallel (x_i-a{)}^2+(y_i-b{)}^2+(z_i-c{)}^2-r_i^2\parallel {}_2^2$$为了好看，写成矩阵形式，令$F(a,b,c,r)={\left[\begin{array}{c}{f}_1(a,b,c,r),f_2(a,b,c,r),\cdots ,f_n(a,b,c,r)\end{array}\right]}^T$,上式变为$$(a,b,c,r{)}^{\ast }=arg\min \parallel F(a,b,c,r)\parallel {}_2^2$$按照高斯牛顿法对其进行一阶泰勒展开：
$F(a_{k+1},b_{k+1},c_{k+1},r_{k+1})\approx F(a_k,b_k,c_k,r_k)+\delta \ast J_k(a,b,c,r)$
其中，$\delta =(a_{k+1},b_{k+1},c_{k+1},r_{k+1})-(a_k,b_k,c_k,r_k)$，更加直白一点就是目标参数两次迭代前后的差值。于是，最小二乘问题简化成为：$${\mathbf{\delta }}^{\ast }=arg\min \parallel F(a_k,b_k,c_k,r_k)+\delta \ast J_k(a,b,c,r)\parallel {}_2^2$$现在，我们可以套用高斯牛顿里面的增量方程，得到$$J_k(a_k,b_k,c_k,r_k{)}^T{J}_k(a_k,b_k,c_k,r_k)\mathbf{\delta }=-J_k(a_k,b_k,c_k,r_k{)}^{T}F(a_k,b_k,c_k,r_k)$$其中，$J_k(a_k,b_k,c_k,r_k)$为F的雅可比矩阵，即：$$J(a,b,c,r)=\frac{\partial F}{\partial (a,b,c,r)}={\left[\begin{array}{c}\frac{\partial f_1(a,b,c,r)}{\partial (a,b,c,r)},\frac{\partial f_2(a,b,c,r)}{\partial (a,b,c,r)},\cdots ,\frac{\partial f_n(a,b,c,r)}{\partial (a,b,c,r)}\end{array}\right]}^T$$而$\frac{\partial f(a,b,c,r)}{\partial (a,b,c,r)}={\left[\begin{array}{c}-2(x-a),-2(y-b),-2(z-c),-2r\end{array}\right]}^T$,将增量方程中的$\mathbf{\delta }$解出，然后更新参数$(a_{k+1},b_{k+1},c_{k+1},r_{k+1})=(a_k,b_k,c_k,r_k)+\mathbf{\delta }$，直至$\mathbf{\delta }$足够小。
至此，全部理论部分已经说明完毕。如有错误，欢迎指正。
基于PCL的C++代码如下：

int main()
{
            pcl::PointCloud<pcl::PointXYZ> src;
			pcl::io::loadPLYFile("test.ply", src);
			std::cout << "高斯牛顿曲线拟合" << "\n";
			double ae = 2.0, be = -1.0, ce = 5.0, re = 600.0; //估计参数值
			int N = src.size();                          //数据点的个数
	
			/*** 开始 Gauss-Newton ***/
			int iterations = 100;   //最高的迭代次数
									//本次迭代的cost和上一次迭代的cost
									//用于比较是不是误差越来越小,类似于开口向上的二次函数,去寻找极值点,不一定是最小值
			double cost = 0, lastCost = 0;

			for (int iter = 0; iter < iterations; iter++) {
				//每次求解一次 Hδx = b;
				Eigen::Matrix4d H = Eigen::Matrix4d::Zero();
				Eigen::Vector4d b = Eigen::Vector4d::Zero();
				cost = 0;

				for (int i = 0; i < N; ++i) {
					double xi = src.points[i].x;
					double yi = src.points[i].y;
					double zi = src.points[i].z;
					//构建一个误差方程 f(x)
					double error = std::pow((xi - ae), 2) + std::pow((yi - be), 2) + std::pow((zi - ce), 2) - re*re;
					//定义雅克比
					Eigen::Vector4d J;
					J[0] = (-2.0*(xi - ae));
					J[1] = (-2.0*(yi - be));
					J[2] = (-2.0*(zi - ce));
					J[3] = (-2.0*re);

					H += J * J.transpose(); //J*J^t
					b += -error * J; // -f(x)*J

					cost += (error * error); //整个误差的方差
				}
				//采用ldlt 求解线性方程 Hδx = b
				Eigen::Vector4d dx = H.ldlt().solve(b);
				if (isnan(dx[0]))
				{
					std::cout << "result is nan!!!" << "\n";
					break;
				}
				//当前的误差比之前的误差还要大,说明找到了一个极值点,停止迭代
				if (iter > 0 && cost >= lastCost)
				{
					std::cout << "iter:" << cost << " >= last cost: " << lastCost << ",break." << "\n";
					break;
				}
				//令 Xk+1 = Xk + δx
				ae += dx[0];
				be += dx[1];
				ce += dx[2];
				re += dx[3];

				lastCost = cost;
				std::cout << "total cost: " << cost << ",\t\tupdate: " << dx.transpose() << "\t\testimated params: " << ae << " , " << be << " , " << ce << " , " << re << "\n";
			}
			std::cout << " estimated params: " << ae << " , " << be << " , " << ce << " , " << re << "\n";
			return 0;
}

参考链接.