迭代重加权最小二乘(IRLS)算法

迭代重加权最小二乘算法

1. 概述

对于数据点集合中，如果存在少量离群点，使用标准的最小二乘法进行拟合，往往可以得到满意的结果。但当离群点的比重增加时，例如当需拟合的圆部分被遮挡，使用标准的最小二乘法进行拟合，结果便不可靠，如下图所示，因为即便是离群点，其为偏差$E$的贡献权值$w$仍为1。

针对上述情况，使用==迭代重加权最小二乘法（IRLS）==可以在一定程度上解决上述问题。IRLS方法引入一个距离权重函数$w(\delta )$，权重可通过已经计算出来的距离来计算。

2. 两个权重函数

2.1 Huber权重函数

$$w(\delta )=\{\begin{array}{l}1,|\delta |\le \gamma \\ \gamma /|\delta |,|\delta |>\gamma \end{array}$$

参数$\gamma$为削波函数，它定义了哪些点为离群点。

2.2 Tukey权重函数

$$w(\delta )=\{\begin{array}{l}(1-(|\delta |/\gamma {)}^2{)}^2,|\delta |\le \gamma \\ 0,|\delta |>\gamma \end{array}$$

3. 基于IRLS方法拟合圆

圆的方程式为$(x-a{)}^2+(y-b{)}^2=c^2$

建立最小化误差函数$E$
$$\begin{gathered} E=\sum _{i=0}^{n-1}((x_i-a{)}^2+(y_i-b{)}^2-c^2{)}^2 \\ =\sum _{i=0}^{n-1}(x_i^2+y_i^2-2a{x}_i-2b{y}_i+a^2+b^2-c^2{)}^2 \\ =\sum _{i=0}^{n-1}(x_i^2+y_i^2+A{x}_i+B{y}_i+C{)}^2 \end{gathered}$$
其中，$A=-2a$，$B=-2b$，$C=a^2+b^2-c^2$。

引入距离权值$w_i$
$$E=\sum _{i=0}^{n-1}{w}_i(x_i^2+y_i^2+A{x}_i+B{y}_i+C{)}^2$$
基于式(4)，对$A$，$B$，$C$分别求偏导：
$$\begin{gathered} \frac{\partial E}{\partial A}=\sum w_i{x}_i(x_i^2+y_i^2+A{x}_i+B{y}_i+C) \\ =A\Sigma w_i{x}_i^2+B\Sigma w_i{x}_i{y}_i+C\Sigma w_i{x}_i+\Sigma w_i{x}_i^3+\Sigma w_i{x}_i{y}_i^2 \end{gathered}$$

$$\begin{gathered} \frac{\partial E}{\partial B}=\sum w_i{x}_i(x_i^2+y_i^2+A{x}_i+B{y}_i+C) \\ =A\Sigma w_i{x}_i{y}_i+B\Sigma w_i{y}_i^2+C\Sigma w_i{y}_i+\Sigma w_i{y}_i^3+\Sigma w_i{x}_i^2{y}_i \end{gathered}$$

$$\begin{gathered} \frac{\partial E}{\partial C}=\sum w_i(x_i^2+y_i^2+A{x}_i+B{y}_i+C) \\ =A\Sigma w_i{x}_i+B\Sigma w_i{y}_i+C\Sigma w_i+\Sigma w_i{x}_i^2+\Sigma w_i{y}_i^2 \end{gathered}$$

式(5~7)变换为矩阵形式：
$$\left[\begin{array}{ccc}\Sigma w{x}^2& \Sigma wxy& \Sigma wx\\ \Sigma wxy& \Sigma w{y}^2& \Sigma wy\\ \Sigma wx& \Sigma wy& \Sigma w_i\end{array}\right]\left[\begin{array}{c}A\\ B\\ C\end{array}\right]=-\left[\begin{array}{c}\Sigma w{x}^3+\Sigma wx{y}^2\\ \Sigma w{y}^3+\Sigma w{x}^{2}y\\ \Sigma w{x}^2+\Sigma w{y}^2\end{array}\right]$$
通过上式可求解出$A$、$B$、$C$，再进而求解出$a$、$b$、$c$。

在进行第一次迭代时，采用标准的最小二乘法进行拟合圆，即$w_i=1$。

4. 关键代码

void getCenterPoint(const Mat& src, Point& centerP, int& radius)
{

	Mat meanImg;
	Mat binaryImg;

	boxFilter(src, meanImg, CV_8UC1, Size(11, 11));
	cv::subtract(meanImg, src, binaryImg);

	threshold(binaryImg, binaryImg, 3, 255, THRESH_BINARY);

	ipt.selectLargestRegionByArea(binaryImg);

	vector<vector<Point>> contours;
	findContours(binaryImg, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);

	Mat outCircleImg = Mat::zeros(src.size(), CV_8UC1);
	drawContours(outCircleImg, contours, 0, Scalar(255), 1);

	vector<double> vWeights;
	vector<Point> selectPoints;
	for (size_t i = 0; i < contours[0].size();i=i+3)
	{
		selectPoints.push_back(contours[0].at(i));
		vWeights.push_back(1);
	}

	fitCircle(selectPoints, vWeights, centerP, radius);

	Mat colorImg;
	cvtColor(src, colorImg, CV_GRAY2BGR);
	for (size_t i = 0; i < selectPoints.size(); i = i + 3)
	{
		circle(colorImg, selectPoints[i], 2, Scalar(255, 255, 0), CV_FILLED);
	}
	for (int k = 0; k < 10; k++)
	{
		vector<double> vWeights;
		vector<double> vDist;
		for (size_t i = 0; i < selectPoints.size();i++)
		{
			double dist = (selectPoints[i].x - centerP.x)*(selectPoints[i].x - centerP.x)
				+ (selectPoints[i].y - centerP.y)*(selectPoints[i].y - centerP.y)/* - radius*radius*/;

			dist = abs(dist);
			dist = sqrt(dist);
			dist = abs(dist - radius);

			vDist.push_back(dist);
		}
		vector<double> vDistCopy;
		vDistCopy.assign(vDist.begin(), vDist.end());

		sort(vDistCopy.begin(), vDistCopy.end());
		double sigma = vDistCopy[vDistCopy.size() / 2] / 0.675;
		sigma *= 2;

		vWeights.clear();
		for (size_t i = 0; i < vDist.size(); i++)
		{
			double weight;


			 huber
			//if (vDist[i] <= sigma)
			//{
			//	weight = 1;
			//}
			//else
			//{
			//	weight = sigma / vDist[i];
			//}

			//Tukey
			if (vDist[i] <= sigma)
			{
				double rate = vDist[i] / sigma;
				weight =pow((1-rate*rate),2) ;
			}
			else
			{
				weight = 0;
			}
			vWeights.push_back(weight);
		}	
		fitCircle(selectPoints, vWeights, centerP, radius);
		Mat colorImg;
		cvtColor(src, colorImg, CV_GRAY2BGR);
		circle(colorImg, centerP, radius, Scalar(0, 0, 255), 1);
		for (size_t i = 0; i < selectPoints.size(); i = i + 3)
		{
			circle(colorImg, selectPoints[i], 2 , Scalar(255, 255, 0), CV_FILLED);
		}
	}
}

void fitCircle(const vector<Point>& selectPoint, vector<double> vWeights, Point& centerP, int& radius)
{
	double x1, y1,x2,y2,x3,y3;
	double x1y1, x1y2, x2y1;
	double n = 0;
    
	x1 = y1 = x2 = y2 = x3 = y3 = 0;
	x1y1 = x1y2 = x2y1 = 0;

	for (size_t i = 0; i < selectPoint.size();i++)
	{
		double x = selectPoint[i].x;
		double y = selectPoint[i].y;
		double weight = vWeights[i];

		x1 += weight*x;
		y1 += weight*y;
		x2 += weight*x*x;
		y2 += weight*y*y;
		x3 += weight*x*x*x;
		y3 += weight*y*y*y;
		x1y1 += weight*x*y;
		x1y2 += weight*x*y*y;
		x2y1 += weight*x*x*y;
		n += weight;
	}

	Mat A = (Mat_<float>(3, 3) << x2, x1y1, x1, x1y1, y2, y1, x1, y1, n);
	Vec3f b = { -x3 - x1y2, -x2y1 - y3, -x2 - y2 };
	Vec3f res;
	solve(A, b, res, cv::DECOMP_LU);
	
	double centerX = -res[0] / 2;
	double centerY = -res[1] / 2;

	radius = sqrt(centerY*centerY + centerX*centerX - res[2]);
	centerP = Point(centerX, centerY);
}

5. 拟合效果

第1次迭代：

第2次迭代：

第3次迭代：

第4次迭代：

第5次迭代：

从拟合结果可以看出，在第5次迭代时，可以相对较好的得到圆拟合结果。