上一篇博客介紹了最小二乘法擬合圓的方法咕缎。這種方法對(duì)誤差符合正態(tài)分布的數(shù)據(jù)點(diǎn)很有效滞欠。但是在機(jī)器視覺(jué)應(yīng)用中經(jīng)常會(huì)碰到一些干擾點(diǎn)残家。這些干擾點(diǎn)多數(shù)時(shí)候是偏向某一個(gè)方向的。這時(shí)要是用最小二乘法擬合莉撇，擬合出的圓會(huì)偏很多觉义。因此测摔，有必要研究更有效的擬合算法蛮瞄。
這里介紹一個(gè)我常用的擬合算法，根據(jù)數(shù)據(jù)點(diǎn)到圓的距離絕對(duì)值的和來(lái)確定圓的參數(shù)谆扎，也就是下面這個(gè)式子：
f=∑∣∣(xi?xc)2+(yi?yc)2??????????????????√?R∣∣f=∑|(xi?xc)2+(yi?yc)2?R|
使得ff取得最小值的xcxc挂捅、ycyc和RR就是最佳擬合參數(shù)。
用這個(gè)式子沒(méi)有解析解堂湖，只能靠數(shù)值算法闲先。這里我采用了 GSL （GNU Scientific Library ）里的多維函數(shù)求極值的功能。關(guān)于如何安裝无蜂、使用 GSL 等問(wèn)題我有其他的博客介紹了伺糠，這里就不多說(shuō)了。下面給代碼斥季。
建立了一個(gè)類训桶，叫做 “CircleFitSolver”累驮。把所有的計(jì)算都封裝到這個(gè)類中。
#ifndef CIRCLEFITSOLVER_H#define CIRCLEFITSOLVER_H#include #include #include #include using namespace std;typedef complex POINT;bool circleLeastFit(const std::vector &points, double ￠er_x, double ￠er_y, double &radius);class CircleFitSolver
{public:
? ? CircleFitSolver();
? ? ~CircleFitSolver();
? ? void setMaxIter(int iter) {m_max_iter = iter;}
? ? /**
? ? * @brief circleFitL1? 擬合圓舵揭，擬合判據(jù)為數(shù)據(jù)點(diǎn)到擬合圓的距離絕對(duì)值之和最小谤专。
? ? * @param points 輸入?yún)?shù)，存儲(chǔ)各個(gè)數(shù)據(jù)點(diǎn)午绳。
? ? * @param center_x radius > 0 時(shí)作為迭代算法的初始值置侍。計(jì)算完成后返回?cái)M合圓的圓心 X 坐標(biāo)
? ? * @param center_y radius > 0 時(shí)作為迭代算法的初始值。計(jì)算完成后返回?cái)M合圓的圓心 Y 坐標(biāo)
? ? * @param radius? radius < 0 時(shí)拦焚，用最小二乘擬合的結(jié)果作為迭代算法的初始值蜡坊。計(jì)算完成后返回?cái)M合圓的半徑。
? ? * @return true 表示擬合成功赎败，否則擬合失敗秕衙。
? ? */? ? bool circleFitL1(const vector &points, double ￠er_x, double ￠er_y, double &radius);private:
? ? gsl_multimin_function m_function;
? ? gsl_multimin_fminimizer * m_fminimizer;
? ? int m_max_iter; // 迭代算法的最大迭代次數(shù)? ? gsl_vector *m_start_point; // 迭代算法的初始值? ? gsl_vector *m_step_size; // 迭代算法的初始步長(zhǎng)? ? void setStartPoint(double center_x, double center_y, double radius);
? ? static double L1_distance(const gsl_vector * v, void * params);
};#endif// CIRCLEFITSOLVER_H
具體的實(shí)現(xiàn)代碼如下：
#include "circlefitsolver.h"#include using namespace std;/**
* 最小二乘法擬合圓
* 擬合出的圓以圓心坐標(biāo)和半徑的形式表示
* 此代碼改編自 newsmth.net 的 jingxing 在 Graphics 版貼出的代碼。
* 版權(quán)歸 jingxing螟够， 我只是搬運(yùn)工外加一些簡(jiǎn)單的修改工作灾梦。
*/bool circleLeastFit(const std::vector &points, double ￠er_x, double ￠er_y, double &radius)
{
? ? center_x = 0.0f;
? ? center_y = 0.0f;
? ? radius = 0.0f;
? ? if (points.size() < 3)
? ? {
? ? ? ? return false;
? ? }
? ? double sum_x = 0.0f, sum_y = 0.0f;
? ? double sum_x2 = 0.0f, sum_y2 = 0.0f;
? ? double sum_x3 = 0.0f, sum_y3 = 0.0f;
? ? double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;
? ? int N = points.size();
? ? for (int i = 0; i < N; i++)
? ? {
? ? ? ? double x = points[i].real();
? ? ? ? double y = points[i].imag();
? ? ? ? double x2 = x * x;
? ? ? ? double y2 = y * y;
? ? ? ? sum_x += x;
? ? ? ? sum_y += y;
? ? ? ? sum_x2 += x2;
? ? ? ? sum_y2 += y2;
? ? ? ? sum_x3 += x2 * x;
? ? ? ? sum_y3 += y2 * y;
? ? ? ? sum_xy += x * y;
? ? ? ? sum_x1y2 += x * y2;
? ? ? ? sum_x2y1 += x2 * y;
? ? }
? ? double C, D, E, G, H;
? ? double a, b, c;
? ? C = N * sum_x2 - sum_x * sum_x;
? ? D = N * sum_xy - sum_x * sum_y;
? ? E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
? ? G = N * sum_y2 - sum_y * sum_y;
? ? H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
? ? a = (H * D - E * G) / (C * G - D * D);
? ? b = (H * C - E * D) / (D * D - G * C);
? ? c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;
? ? center_x = a / (-2);
? ? center_y = b / (-2);
? ? radius = sqrt(a * a + b * b - 4 * c) / 2;
? ? return true;
}double CircleFitSolver::L1_distance(const gsl_vector * v, void * params)
{
? ? vector *vect = (vector *)params;
? ? int N? = vect->size();
? ? double a, b, r;
? ? a = gsl_vector_get(v, 0);
? ? b = gsl_vector_get(v, 1);
? ? r = gsl_vector_get(v, 2);
? ? double sum = 0;
? ? for(int i = 0; i < N; i++)
? ? {
? ? ? ? const POINT p = vect->at(i);
? ? ? ? double xi = p.real() - a;
? ? ? ? double yi = p.imag() - b;
? ? ? ? double dist = sqrt(xi * xi + yi * yi) - r;
? ? ? ? sum += fabs(dist);
? ? }
? ? return sum;
}inline void CircleFitSolver::setStartPoint(double center_x, double center_y, double radius)
{
? ? gsl_vector_set (m_start_point, 0, center_x);
? ? gsl_vector_set (m_start_point, 1, center_y);
? ? gsl_vector_set (m_start_point, 2, radius);
}bool CircleFitSolver::circleFitL1(const vector &points, double ￠er_x, double ￠er_y, double &radius)
{
? ? m_function.params = (void *)&points;
? ? if( radius < 0 )
? ? {
? ? ? ? // 用最小二乘擬合的結(jié)果作為初始值? ? ? ? if( !circleLeastFit(points, center_x, center_y, radius) )
? ? ? ? {
? ? ? ? ? ? return false;
? ? ? ? }
? ? }
? ? setStartPoint(center_x, center_y, radius);
? ? /* 經(jīng)驗(yàn)值，初始步長(zhǎng)設(shè)置為半徑的十分之一 */? ? gsl_vector_set (m_step_size, 0, radius / 10.0);
? ? gsl_vector_set (m_step_size, 1, radius / 10.0);
? ? gsl_vector_set (m_step_size, 2, radius / 10.0);
? ? gsl_multimin_fminimizer_set(m_fminimizer, &m_function, m_start_point, m_step_size);
? ? int iter = 0;
? ? int status;
? ? do? ? {
? ? ? ? iter++;
? ? ? ? status = gsl_multimin_fminimizer_iterate(m_fminimizer);
? ? ? ? if (status == GSL_ENOPROG ) // 表示無(wú)法找到更好的解了? ? ? ? {
? ? ? ? ? ? break;
? ? ? ? }
? ? ? ? double size = gsl_multimin_fminimizer_size (m_fminimizer);
? ? ? ? status = gsl_multimin_test_size (size, 1e-2);
? ? }
? ? while (status == GSL_CONTINUE && iter < m_max_iter);
? ? gsl_vector * out = gsl_multimin_fminimizer_x(m_fminimizer);
? ? center_x = gsl_vector_get(out, 0);
? ? center_y = gsl_vector_get(out, 1);
? ? radius = gsl_vector_get(out, 2);
? ? return true;
}
CircleFitSolver::CircleFitSolver()
{
? ? m_max_iter = 100; // 默認(rèn)最大迭代 100 步? ? m_function.n = 3;
? ? m_function.f = L1_distance;
? ? m_start_point = gsl_vector_alloc (m_function.n);
? ? m_step_size = gsl_vector_alloc (m_function.n);
? ? m_fminimizer = gsl_multimin_fminimizer_alloc(gsl_multimin_fminimizer_nmsimplex, 3);
}
CircleFitSolver::~CircleFitSolver()
{
? ? gsl_vector_free(m_start_point);
? ? gsl_vector_free(m_step_size);
? ? gsl_multimin_fminimizer_free(m_fminimizer);
}
這個(gè)代碼基本上就是中規(guī)中矩的計(jì)算妓笙，沒(méi)太多可說(shuō)的若河。唯一一個(gè)小技巧就是用最小二乘法的結(jié)果作為迭代的初始值。這樣很快就能收斂寞宫。
下面給個(gè)圖萧福，說(shuō)說(shuō)它的效果吧。
