使用Ceres进行2D圆拟合
1 构建最小二乘问题
2D 圆的一般方程为：
(
x
−
a
)
2
+
(
y
−
b
)
2
=
r
2
(1)
(x-a)^2 + (y - b)^2 = r^2 \tag{1}
(x−a)2+(y−b)2=r2(1)
式中，
(
a
,
b
)
(a,b)
(a,b)为圆心坐标，
r
r
r为半径。
假设有 k 个 2D 观测点数据
(
x
i
,
y
i
)
,
i
=
1
,
2
,
.
.
.
,
k
(x_i,y_i),i = 1,2,...,k
(xi,yi),i=1,2,...,k ，根据观测点数据拟合圆，并获取拟合圆的圆心
(
a
,
b
)
(a,b)
(a,b) 和半径
r
r
r ，可以先构建如下非线性最小二乘问题：
m
i
n
1
2
∑
i
=
1
k
∣
∣
r
2
−
(
x
i
−
a
)
2
−
(
y
i
−
b
)
2
∣
∣
2
(2)
min\frac{1}{2}\sum_{i = 1}^k||r^2 - (x_i - a)^2 - (y_i - b)^2||^2 \tag{2}
min21i=1∑k∣∣r2−(xi−a)2−(yi−b)2∣∣2(2)
式中，将代价函数
f
(
a
,
b
,
r
)
f(a,b,r)
f(a,b,r) 定义为：
f
(
a
,
b
,
r
)
=
r
2
−
[
(
x
i
−
a
)
2
+
(
y
i
−
b
)
2
]
(3)
f(a,b,r) = r^2 - [(x_i - a)^2 + (y_i - b)^2] \tag{3}
f(a,b,r)=r2−[(xi−a)2+(yi−b)2](3)
而不是：
f
(
a
,
b
,
r
)
=
r
−
(
x
i
−
a
)
2
+
(
y
i
−
b
)
2
(4)
f(a,b,r) = r - \sqrt{(x_i - a)^2 + (y_i - b)^2} \tag{4}
f(a,b,r)=r−(xi−a)2+(yi−b)2(4)
式（4）表示观测点到圆心的距离，看似比较直观、合理，但其中的根号运算让代价函数更加非线性；
式（3）虽然严格意义上不能表示点到圆心的距离，但通常更加鲁棒（尤其是存在外点时），这是因为代价函数更符合凸函数，更容易找到期望的最小值。
但由此也会带来一个问题，估计出来的半径 r r r 可能是负值，与实际不符，需要增加约束，重新构建非线性最小二乘问题，有如下两种方案：
1.令
r
=
m
2
r = m^2
r=m2 ，那么带估计的参数将变成
(
a
,
b
,
m
)
(a,b,m)
(a,b,m) ，代价函数如下：
f
(
a
,
b
,
m
)
=
m
4
−
[
(
x
i
−
a
)
2
+
(
y
i
−
b
)
2
]
(5)
f(a,b,m) = m^4 - [(x_i - a)^2 + (y_i - b)^2] \tag{5}
f(a,b,m)=m4−[(xi−a)2+(yi−b)2](5)
那么，即使估计出的 m 值为负，
r
=
m
2
r =m^2
r=m2 仍然为正值。
2.令
r
2
=
r
2
r_2 = r^2
r2=r2 待估计的参数将变成
(
a
,
b
,
r
2
)
(a,b,r_2)
(a,b,r2)， 代价函数如下：
f
(
a
,
b
,
r
2
)
=
r
2
−
[
(
x
i
−
a
)
2
+
(
y
i
−
b
)
2
]
(6)
f(a,b,r_2) = r_2 - [(x_i - a)^2 + (y_i - b)^2] \tag{6}
f(a,b,r2)=r2−[(xi−a)2+(yi−b)2](6)
那么，估计出的
r
2
r_2
r2 肯定是正值，
r
=
r
2
r = \sqrt{r_2}
r=r2 也是正值。
下面将对（5）（6）两式分别采用自动微分法和解析解法来实现。
2 生成数据
以圆心坐标
(
a
,
b
)
=
(
300
,
200
)
(a,b) = (300,200)
(a,b)=(300,200) ，半径
r
=
100
r = 100
r=100 为标准圆，加入标准差
s
i
g
m
a
=
1.0
sigma =1.0
sigma=1.0 的零均值高斯分布噪声，生成
k
=
100
k= 100
k=100 个点，生成方程如下：
{
x
i
=
a
+
r
∗
c
o
s
i
∗
2
π
k
+
N
(
0
,
σ
2
)
y
i
=
b
+
r
∗
s
i
n
i
∗
2
π
k
+
N
(
0
,
σ
2
)
(7)
\left\{\begin{matrix} x_i=a+ r * cos\frac{i * 2 \pi}{k} + N(0, \sigma^2) \\ y_i = b + r* sin\frac{i * 2\pi}{k} + N(0, \sigma^2) \end{matrix}\right. \tag{7}
{xi=a+r∗coski∗2π+N(0,σ2)yi=b+r∗sinki∗2π+N(0,σ2)(7)
3 代码实践
3.1 circlefit_m_autodiff
完整代码如下：
#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "opencv2/core.hpp"
#include "easyx.h"
// 用户自定义残差计算模型
struct DistanceFromCircleCost
{
// 样本数据观测值通过构造函数传入
DistanceFromCircleCost(double x, double y) : x_(x), y_(y) {}
template <typename T>
bool operator()(const T* const a, const T* const b, const T* const m, T* residual) const
{
// 输出残差维度为1
// 输入3个参数块，每个参数块的维度为1
T xp = x_ - a[0];
T yp = y_ - b[0];
residual[0] = m[0] * m[0] * m[0] * m[0] - xp * xp - yp * yp;
return true;
}
private:
const double x_;
const double y_;
};
int main (int argc, char** argv)
{
google::InitGoogleLogging(argv[0]);
// 生成数据
const int kNumObservations = 100; // 生成100个点
double sigma = 1.0; // 标准差
cv::RNG rng; // OpenCV随机数产生器
double data[2 * kNumObservations];
double a_ideal = 300.0;
double b_ideal = 200.0;
double r_ideal = 100.0;
for (int i = 0; i < kNumObservations; ++i)
{
double x = a_ideal + r_ideal * std::cos(i * CV_2PI / kNumObservations);
double y = b_ideal + r_ideal * std::sin(i * CV_2PI / kNumObservations);
double noise_x = rng.gaussian(sigma);
double noise_y = rng.gaussian(sigma);
data[2 * i] = x + noise_x;
data[2 * i + 1] = y + noise_y;
}
// 随机加入异常点
data[20] = 300.0, data[21] = 200.0;
data[22] = 100.0, data[23] = 150.0;
data[50] = 500.0, data[51] = 100.0;
// 设置参数初始值
// 输入3个参数块，每个参数块的维度为1
const double initial_a = 0;
const double initial_b = 0;
const double initial_m = 1;
double a = initial_a;
double b = initial_b;
double m = initial_m;
// 构建非线性最小二乘问题
ceres::Problem problem;
for (int i = 0; i < kNumObservations; ++i)
{
// 添加残差块，需要依次指定代价函数、损失函数、参数块
// 本例中损失函数为Huber函数
problem.AddResidualBlock(
// 输出残差维度为1，输出参数块有3个，每个参数块维度都为1
new ceres::AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>(new DistanceFromCircleCost(data[2 * i], data[2 * i + 1])),
// Huber损失函数
new ceres::HuberLoss(sigma),
// 第1个参数块
&a,
// 第2个参数块
&b,
// 第3个参数块
&m);
}
// 配置求解器参数
ceres::Solver::Options options;
// 设置最大迭代次数
options.max_num_iterations = 500;
// 指定线性求解器来求解问题
options.linear_solver_type = ceres::DENSE_QR;
// 输出每次迭代的信息
options.minimizer_progress_to_stdout = true;
// 输出日志内容
ceres::Solver::Summary summary;
// 开始优化求解
ceres::Solve(options, &problem, &summary);
double r = m * m;
// 输出优化过程及结果
std::cout << summary.FullReport() << "\n";
std::cout << "initial a: " << initial_a << "->" << a << "\n";
std::cout << "initial b: " << initial_b << "->" << b << "\n";
std::cout << "initial r: " << initial_m << "->" << r << "\n";
// 创建绘图窗口，大小为 640x480 像素
initgraph(640, 480);
// 设置背景颜色为白色
setbkcolor(WHITE);
cleardevice();
// 绘制观测点、十字线、黑色
setlinecolor(BLACK);
for(int i = 0; i < kNumObservations; ++i)
{
int size = 3;
double center_x = data[2 * i];
double center_y = data[2 * i + 1];
line(center_x - size, center_y, center_x + size, center_y);
line(center_x, center_y - size, center_x, center_y + size);
}
// 绘制理想圆，蓝色，圆心（300，200），半径 100
setlinestyle(PS_SOLID, 2);
setlinecolor(BLUE);
circle(a_ideal, b_ideal, r_ideal);
// 绘制鲁棒拟合圆，绿色
setlinecolor(GREEN);
circle(a, b, r);
std::system("pause");
closegraph();
return 0;
}
迭代结果如下：
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.382256e+07 0.00e+00 5.97e+04 0.00e+00 0.00e+00 1.00e+04 0 6.99e-03 7.54e-03
1 5.407934e+19 -5.41e+19 5.97e+04 2.71e+04 -7.84e+12 5.00e+03 1 1.31e-03 9.40e-03
2 5.308636e+19 -5.31e+19 5.97e+04 2.70e+04 -7.70e+12 1.25e+03 1 6.97e-04 1.02e-02
3 4.753642e+19 -4.75e+19 5.97e+04 2.63e+04 -6.90e+12 1.56e+02 1 6.16e-04 1.09e-02
4 1.790201e+19 -1.79e+19 5.97e+04 2.06e+04 -2.60e+12 9.77e+00 1 6.28e-04 1.16e-02
5 9.529089e+14 -9.53e+14 5.97e+04 1.76e+03 -1.45e+08 3.05e-01 1 5.90e-04 1.23e-02
6 4.368009e+16 -4.37e+16 5.97e+04 4.57e+03 -9.53e+09 4.77e-03 1 5.82e-04 1.29e-02
7 3.575734e+10 -3.57e+10 5.97e+04 1.37e+02 -2.08e+05 3.73e-05 1 5.88e-04 1.36e-02
8 1.381984e+07 2.71e+03 5.97e+04 1.08e+00 1.98e+00 1.12e-04 1 3.69e-03 1.73e-02
9 1.381536e+07 4.48e+03 5.97e+04 3.62e-01 1.09e+00 3.35e-04 1 3.87e-03 2.12e-02
10 1.380116e+07 1.42e+04 5.97e+04 6.80e-01 1.16e+00 1.01e-03 1 4.07e-03 2.54e-02
11 1.375778e+07 4.34e+04 5.96e+04 1.04e+00 1.18e+00 3.02e-03 1 4.79e-03 3.03e-02
12 1.362882e+07 1.29e+05 6.13e+04 1.70e+00 1.19e+00 9.05e-03 1 4.03e-03 3.44e-02
13 1.325494e+07 3.74e+05 1.37e+05 3.67e+00 1.19e+00 2.72e-02 1 3.85e-03 3.84e-02
14 1.223981e+07 1.02e+06 2.93e+05 9.40e+00 1.20e+00 8.15e-02 1 4.21e-03 4.27e-02
15 9.928699e+06 2.31e+06 5.63e+05 2.27e+01 1.22e+00 2.44e-01 1 4.19e-03 4.70e-02
16 6.583481e+06 3.35e+06 8.25e+05 4.03e+01 1.26e+00 7.33e-01 1 3.90e-03 5.10e-02
17 4.203227e+06 2.38e+06 5.50e+05 4.44e+01 1.25e+00 2.20e+00 1 4.07e-03 5.52e-02
18 3.586101e+06 6.17e+05 4.31e+05 2.44e+01 1.35e+00 6.60e+00 1 4.30e-03 5.96e-02
19 2.943270e+06 6.43e+05 3.15e+05 3.29e+01 1.24e+00 1.98e+01 1 4.07e-03 6.39e-02
20 1.664384e+06 1.28e+06 1.47e+05 8.29e+01 1.30e+00 5.94e+01 1 3.67e-03 6.77e-02
21 9.567219e+05 7.08e+05 1.11e+05 5.36e+01 1.38e+00 1.78e+02 1 3.60e-03 7.14e-02
22 4.339197e+05 5.23e+05 2.81e+05 5.60e+01 1.27e+00 5.35e+02 1 4.97e-03 7.66e-02
23 1.577583e+05 2.76e+05 4.07e+05 4.03e+00 1.57e+00 1.60e+03 1 3.98e-03 8.07e-02
24 9.851760e+04 5.92e+04 1.61e+05 3.88e-01 1.72e+00 4.81e+03 1 3.64e-03 8.45e-02
25 9.756555e+04 9.52e+02 1.21e+05 1.02e-01 1.71e+00 1.44e+04 1 4.22e-03 8.88e-02
26 9.706839e+04 4.97e+02 1.04e+05 4.18e-02 1.83e+00 4.33e+04 1 4.69e-03 9.39e-02
27 9.669641e+04 3.72e+02 8.03e+04 2.64e-02 1.82e+00 1.30e+05 1 3.83e-03 9.78e-02
28 9.656395e+04 1.32e+02 4.81e+04 1.01e-02 1.68e+00 3.90e+05 1 3.68e-03 1.02e-01
29 9.653603e+04 2.79e+01 3.28e+04 4.65e-03 1.63e+00 1.17e+06 1 4.04e-03 1.06e-01
30 9.651426e+04 2.18e+01 3.21e+04 6.67e-03 1.98e+00 3.51e+06 1 5.25e-03 1.11e-01
31 9.648512e+04 2.91e+01 3.21e+04 7.54e-03 2.00e+00 1.05e+07 1 4.21e-03 1.16e-01
32 9.645172e+04 3.34e+01 3.21e+04 7.12e-03 2.00e+00 3.16e+07 1 4.01e-03 1.20e-01
33 9.641834e+04 3.34e+01 3.20e+04 6.36e-03 2.00e+00 9.47e+07 1 3.96e-03 1.24e-01
34 9.639493e+04 2.34e+01 1.83e+04 5.35e-03 1.77e+00 2.84e+08 1 4.29e-03 1.29e-01
35 9.638998e+04 4.95e+00 1.60e+04 1.70e-03 1.80e+00 8.52e+08 1 3.84e-03 1.33e-01
36 9.638467e+04 5.31e+00 1.60e+04 1.77e-03 2.00e+00 2.56e+09 1 4.00e-03 1.37e-01
37 9.637927e+04 5.39e+00 1.37e+04 2.44e-03 1.94e+00 7.67e+09 1 3.75e-03 1.41e-01
38 9.637505e+04 4.23e+00 1.12e+04 4.00e-03 1.91e+00 2.30e+10 1 4.04e-03 1.45e-01
39 9.637144e+04 3.60e+00 9.68e+03 4.83e-03 1.97e+00 6.90e+10 1 4.47e-03 1.49e-01
40 9.636863e+04 2.81e+00 6.51e+03 5.05e-03 1.86e+00 2.07e+11 1 4.09e-03 1.54e-01
41 9.636634e+04 2.29e+00 5.26e+03 5.23e-03 1.98e+00 6.21e+11 1 3.76e-03 1.57e-01
42 9.636431e+04 2.04e+00 4.81e+03 5.02e-03 2.00e+00 1.86e+12 1 3.97e-03 1.61e-01
43 9.636257e+04 1.73e+00 4.74e+03 4.42e-03 2.00e+00 5.59e+12 1 4.28e-03 1.66e-01
44 9.636156e+04 1.01e+00 2.04e+03 3.39e-03 1.59e+00 1.68e+13 1 3.83e-03 1.70e-01
45 9.636126e+04 2.99e-01 1.25e+03 2.01e-03 1.59e+00 5.03e+13 1 3.90e-03 1.74e-01
46 9.636115e+04 1.13e-01 1.04e+03 1.24e-03 1.66e+00 1.51e+14 1 5.35e-03 1.79e-01
Solver Summary (v 2.0.0-eigen-(3.3.8)-no_lapack-eigensparse-no_openmp)
Original Reduced
Parameter blocks 3 3
Parameters 3 3
Residual blocks 100 100
Residuals 100 100
Minimizer TRUST_REGION
Dense linear algebra library EIGEN
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver DENSE_QR DENSE_QR
Threads 1 1
Linear solver ordering AUTOMATIC 3
Cost:
Initial 1.382256e+07
Final 9.636115e+04
Change 1.372620e+07
Minimizer iterations 47
Successful steps 40
Unsuccessful steps 7
Time (in seconds):
Preprocessor 0.000552
Residual only evaluation 0.010958 (47)
Jacobian & residual evaluation 0.134731 (40)
Linear solver 0.018897 (47)
Minimizer 0.181130
Postprocessor 0.000083
Total 0.181765
Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 5.805692e-07 <= 1.000000e-06)
initial a: 0->300.266
initial b: 0->199.758
initial r: 1->100.023
最终拟合得到的圆的圆心 ( a , b ) = ( 300.266 , 199.758 ) (a,b) = (300.266,199.758) (a,b)=(300.266,199.758) ，半径 r = 100.023 r = 100.023 r=100.023。
收敛后的残差值仍然很大，这是因为，代价函数并非是直接表示半径
r
r
r，观测点与圆心距离
d
d
d 这两者间的偏差，即并非是
r
−
d
r-d
r−d ，而是定义成了
r
2
−
d
2
=
(
r
−
d
)
∗
(
r
+
d
)
r^2 - d^2 = (r -d)*(r+d)
r2−d2=(r−d)∗(r+d)，偏差相当于被放大了
(
r
+
d
)
(r+d)
(r+d) 倍，加上残差又是代价函数的平方，也就是还要再被放大一次，因此最终收敛后的残差值很大。
图中，+ 表示观测点，蓝色的圆是理想圆，绿色的圆是鲁棒拟合圆，两个圆基本重合。
由于加入了异常点，因此使用了 Huber 损失函数降低异常点对拟合的影响。
如果去掉损失函数，即 LossFunction = NULL，仅使用一般的最小二乘拟合圆，结果如下：
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.092554e+12 0.00e+00 8.92e+09 0.00e+00 0.00e+00 1.00e+04 0 6.96e-03 7.56e-03
1 2.738082e+37 -2.74e+37 8.92e+09 2.93e+04 -2.51e+25 5.00e+03 1 1.35e-03 9.51e-03
2 2.615438e+37 -2.62e+37 8.92e+09 2.92e+04 -2.40e+25 1.25e+03 1 7.22e-04 1.04e-02
3 1.990936e+37 -1.99e+37 8.92e+09 2.82e+04 -1.82e+25 1.56e+02 1 6.62e-04 1.12e-02
4 1.799492e+36 -1.80e+36 8.92e+09 2.09e+04 -1.65e+24 9.77e+00 1 6.66e-04 1.21e-02
5 3.165118e+30 -3.17e+30 8.92e+09 3.99e+03 -3.01e+18 3.05e-01 1 1.04e-03 1.32e-02
6 4.039797e+31 -4.04e+31 8.92e+09 5.47e+03 -5.47e+19 4.77e-03 1 8.75e-04 1.43e-02
7 2.550254e+19 -2.55e+19 8.92e+09 1.62e+02 -9.20e+08 3.73e-05 1 6.61e-04 1.51e-02
8 1.092040e+12 5.14e+08 8.92e+09 1.29e+00 2.33e+00 1.12e-04 1 3.29e-03 1.86e-02
9 1.091327e+12 7.12e+08 8.91e+09 3.26e-01 1.07e+00 3.35e-04 1 3.52e-03 2.22e-02
10 1.089062e+12 2.27e+09 8.90e+09 6.65e-01 1.14e+00 1.01e-03 1 3.71e-03 2.61e-02
11 1.082074e+12 6.99e+09 8.87e+09 1.08e+00 1.18e+00 3.02e-03 1 4.46e-03 3.06e-02
12 1.061380e+12 2.07e+10 9.50e+09 1.80e+00 1.18e+00 9.05e-03 1 4.34e-03 3.51e-02
13 1.002511e+12 5.89e+10 2.06e+10 3.95e+00 1.17e+00 2.72e-02 1 3.53e-03 3.88e-02
14 8.511883e+11 1.51e+11 4.04e+10 1.02e+01 1.15e+00 8.15e-02 1 3.51e-03 4.25e-02
15 5.528670e+11 2.98e+11 6.14e+10 2.49e+01 1.10e+00 2.44e-01 1 3.94e-03 4.65e-02
16 2.220276e+11 3.31e+11 5.47e+10 4.65e+01 1.03e+00 7.33e-01 1 3.56e-03 5.03e-02
17 7.378387e+10 1.48e+11 2.09e+10 5.50e+01 9.82e-01 2.20e+00 1 3.33e-03 5.38e-02
18 4.094784e+10 3.28e+10 9.08e+08 4.70e+01 1.00e+00 6.60e+00 1 3.35e-03 5.74e-02
19 1.786394e+10 2.31e+10 1.82e+09 6.84e+01 1.01e+00 1.98e+01 1 3.50e-03 6.12e-02
20 2.255007e+09 1.56e+10 4.34e+08 9.32e+01 1.00e+00 5.94e+01 1 4.00e-03 6.54e-02
21 1.355118e+09 9.00e+08 2.95e+08 2.74e+01 9.70e-01 1.78e+02 1 3.39e-03 6.89e-02
22 1.327190e+09 2.79e+07 6.98e+06 5.59e-01 1.00e+00 5.35e+02 1 3.44e-03 7.25e-02
23 1.327176e+09 1.39e+04 1.71e+04 5.44e-03 1.00e+00 1.60e+03 1 3.45e-03 7.61e-02
Solver Summary (v 2.0.0-eigen-(3.3.8)-no_lapack-eigensparse-no_openmp)
Original Reduced
Parameter blocks 3 3
Parameters 3 3
Residual blocks 100 100
Residuals 100 100
Minimizer TRUST_REGION
Dense linear algebra library EIGEN
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver DENSE_QR DENSE_QR
Threads 1 1
Linear solver ordering AUTOMATIC 3
Cost:
Initial 1.092554e+12
Final 1.327176e+09
Change 1.091227e+12
Minimizer iterations 24
Successful steps 17
Unsuccessful steps 7
Time (in seconds):
Preprocessor 0.000602
Residual only evaluation 0.005418 (24)
Jacobian & residual evaluation 0.052757 (17)
Linear solver 0.009628 (24)
Minimizer 0.076465
Postprocessor 0.000057
Total 0.077125
Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 6.254251e-11 <= 1.000000e-06)
initial a: 0->301.317
initial b: 0->194.53
initial r: 1->103.131
两圆明显不重合。
3.2 circlefit_r2_autodiff
完整代码如下：
#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "opencv2/core.hpp"
#include "easyx.h"
// 用户自定义残差计算模型
struct DistanceFromCircleCost
{
// 样本数据观测值通过构造函数传入
DistanceFromCircleCost(double x, double y) : x_(x), y_(y) {}
template <typename T>
bool operator()(const T* const a, const T* const b, const T* const r2, T* residual) const
{
// 输出残差维度为1
// 输入3个参数块，每个参数块的维度为1
T xp = x_ - a[0];
T yp = y_ - b[0];
residual[0] = r2[0] - xp * xp - yp * yp;
return true;
}
private:
const double x_;
const double y_;
};
int main(int argc, char** argv)
{
google::InitGoogleLogging(argv[0]);
// 生成数据
const int kNumObservations = 100; // 生成100个点
double sigma = 1.0; // 标准差
cv::RNG rng; // OpenCV随机数产生器
double data[2 * kNumObservations];
double a_ideal = 300.0;
double b_ideal = 200.0;
double r_ideal = 100.0;
for (int i = 0; i < kNumObservations; ++i)
{
double x = a_ideal + r_ideal * std::cos(i * CV_2PI / kNumObservations);
double y = b_ideal + r_ideal * std::sin(i * CV_2PI / kNumObservations);
double noise_x = rng.gaussian(sigma);
double noise_y = rng.gaussian(sigma);
data[2 * i] = x + noise_x;
data[2 * i + 1] = y + noise_y;
}
// 随机加入异常点
data[20] = 300.0, data[21] = 200.0;
data[22] = 100.0, data[23] = 150.0;
data[50] = 500.0, data[51] = 100.0;
// 设置参数初始值
// 输入3个参数块，每个参数块的维度为1
const double initial_a = 0;
const double initial_b = 0;
const double initial_r = 0;
double a = initial_a;
double b = initial_b;
double r2 = initial_r * initial_r;
// 构建非线性最小二乘问题
ceres::Problem problem;
for (int i = 0; i < kNumObservations; ++i)
{
// 添加残差块，需要依次指定代价函数、损失函数、参数块
// 本例中损失函数为Huber函数
problem.AddResidualBlock(
// 输出残差维度为1，输出参数块有3个，每个参数块维度都为1
new ceres::AutoDiffCostFunction<DistanceFromCircleCost, 1, 1, 1, 1>(new DistanceFromCircleCost(data[2 * i], data[2 * i + 1])),
// Huber损失函数
new ceres::HuberLoss(sigma)/*NULL*/,
// 第1个参数块
&a,
// 第2个参数块
&b,
// 第3个参数块
&r2);
}
// 配置求解器参数
ceres::Solver::Options options;
// 设置最大迭代次数
options.max_num_iterations = 500;
// 指定线性求解器来求解问题
options.linear_solver_type = ceres::DENSE_QR;
// 输出每次迭代的信息
options.minimizer_progress_to_stdout = true;
// 输出日志内容
ceres::Solver::Summary summary;
// 开始优化求解
ceres::Solve(options, &problem, &summary);
double r = std::sqrt(r2);
// 输出优化过程及结果
std::cout << summary.FullReport() << "\n";
std::cout << "initial a: " << initial_a << "->" << a << "\n";
std::cout << "initial b: " << initial_b << "->" << b << "\n";
std::cout << "initial r: " << initial_r << "->" << r << "\n";
// 创建绘图窗口，大小为 640x480 像素
initgraph(640, 480);
// 设置背景颜色为白色
setbkcolor(WHITE);
cleardevice();
// 绘制观测点、十字线、黑色
setlinecolor(BLACK);
for (int i = 0; i < kNumObservations; ++i)
{
int size = 3;
double center_x = data[2 * i];
double center_y = data[2 * i + 1];
line(center_x - size, center_y, center_x + size, center_y);
line(center_x, center_y - size, center_x, center_y + size);
}
// 绘制理想圆，蓝色，圆心（300，200），半径 100
setlinestyle(PS_SOLID, 2);
setlinecolor(BLUE);
circle(a_ideal, b_ideal, r_ideal);
// 绘制鲁棒拟合圆，绿色
setlinecolor(GREEN);
circle(a, b, r);
std::system("pause");
closegraph();
return 0;
}
迭代结果与前向代码结果相同。
3.3 circlefit_m_analytic
对于代价函数
f
(
a
,
b
,
m
)
=
m
4
−
(
x
−
a
)
2
−
(
y
−
b
)
2
f(a,b,m) = m^4 - (x-a)^2 - (y - b)^2
f(a,b,m)=m4−(x−a)2−(y−b)2 ，偏导数如下：
{
∂
f
∂
a
=
2
∗
(
x
i
−
a
)
∂
f
∂
b
=
2
∗
(
y
i
−
b
)
∂
f
∂
m
=
4
∗
m
3
\left\{\begin{matrix} \frac{\partial f}{\partial a} = 2 * (x_i - a) \\ \frac{\partial f}{\partial b} = 2 * (y_i - b) \\ \frac{\partial f}{\partial m} = 4* m ^3 \end{matrix}\right.
⎩
⎨
⎧∂a∂f=2∗(xi−a)∂b∂f=2∗(yi−b)∂m∂f=4∗m3
完整代码如下：
#include "ceres/ceres.h"
#include "glog/logging.h"
#include "gflags/gflags.h"
#include "easyx.h"
#include "opencv2/core.hpp"
class DIstanceFromCircleCost
: public ceres::SizedCostFunction<1,1,1,1>
{
public:
DIstanceFromCircleCost(double x, double y): x_(x), y_(y) {}
// 用户自定义残差计算方法
virtual bool Evaluate(double const* const* x, double* residuals, double** jacobians) const
{
// 本例中有3个输入参数块，每个参数块中有1个参数
double a = x[0][0];
double b = x[1][0];
double m = x[2][0];
// 本例中输出残差维度为1
double xp = x_ - a;
double yp = y_ - b;
residuals[0] = m * m * m * m - xp * xp - yp * yp;
if(jacobians == NULL)
{
return true;
}
// 残差对第1个参数块中的参数依次求偏导，即对a求偏导
if(jacobians[0] != NULL)
{
jacobians[0][0] = 2 * xp;
}
// 残差对第2个参数块中的参数依次求偏导，即对b求偏导
if(jacobians[1] != NULL)
{
jacobians[1][0] = 2 * yp;
}
// 残差对第3个参数块中的参数依次求偏导，即对m求偏导
if(jacobians[2] != NULL)
{
jacobians[2][0] = 4 * m * m * m;
}
return true;
}
private:
const double x_;
const double y_;
};
int main(int argc, char** argv)
{
google::InitGoogleLogging(argv[0]);
// 生成数据
const int kNumObservations = 100; // 生成100个点
double sigma = 1.0; // 标准差
cv::RNG rng; // OpenCV随机数产生器
double data[2 * kNumObservations];
double a_ideal = 300.0;
double b_ideal = 200.0;
double r_ideal = 100.0;
for (int i = 0; i < kNumObservations; ++i)
{
double x = a_ideal + r_ideal * std::cos(i * CV_2PI / kNumObservations);
double y = b_ideal + r_ideal * std::sin(i * CV_2PI / kNumObservations);
double noise_x = rng.gaussian(sigma);
double noise_y = rng.gaussian(sigma);
data[2 * i] = x + noise_x;
data[2 * i + 1] = y + noise_y;
}
// 随机加入异常点
data[20] = 300.0, data[21] = 200.0;
data[22] = 100.0, data[23] = 150.0;
data[50] = 500.0, data[51] = 100.0;
// 设置参数初始值
// 输入3个参数块，每个参数块的维度为1
const double initial_a = 0;
const double initial_b = 0;
const double initial_m = 0.1;
double a = initial_a;
double b = initial_b;
double m = initial_m;
// 构建非线性最小二乘问题
ceres::Problem problem;
for (int i = 0; i < kNumObservations; ++i)
{
// 添加残差块，需要依次指定代价函数、损失函数、参数块
// 本例中损失函数为Huber函数
problem.AddResidualBlock(
// 输出残差维度为1，输出参数块有3个，每个参数块维度都为1
new DIstanceFromCircleCost(data[2 * i], data[2 * i + 1]),
// Huber损失函数
new ceres::HuberLoss(sigma)/*NULL*/,
// 第1个参数块
&a,
// 第2个参数块
&b,
// 第3个参数块
&m);
}
// 配置求解器参数
ceres::Solver::Options options;
// 设置最大迭代次数
options.max_num_iterations = 500;
// 指定线性求解器来求解问题
options.linear_solver_type = ceres::DENSE_QR;
// 输出每次迭代的信息
options.minimizer_progress_to_stdout = true;
// 输出日志内容
ceres::Solver::Summary summary;
// 开始优化求解
ceres::Solve(options, &problem, &summary);
double r = m * m;
// 输出优化过程及结果
std::cout << summary.FullReport() << "\n";
std::cout << "initial a: " << initial_a << "->" << a << "\n";
std::cout << "initial b: " << initial_b << "->" << b << "\n";
std::cout << "initial r: " << initial_m << "->" << r << "\n";
// 创建绘图窗口，大小为 640x480 像素
initgraph(640, 480);
// 设置背景颜色为白色
setbkcolor(WHITE);
cleardevice();
// 绘制观测点、十字线、黑色
setlinecolor(BLACK);
for (int i = 0; i < kNumObservations; ++i)
{
int size = 3;
double center_x = data[2 * i];
double center_y = data[2 * i + 1];
line(center_x - size, center_y, center_x + size, center_y);
line(center_x, center_y - size, center_x, center_y + size);
}
// 绘制理想圆，蓝色，圆心（300，200），半径 100
setlinestyle(PS_SOLID, 2);
setlinecolor(BLUE);
circle(a_ideal, b_ideal, r_ideal);
// 绘制鲁棒拟合圆，绿色
setlinecolor(GREEN);
circle(a, b, r);
std::system("pause");
closegraph();
return 0;
}
结果与自动微分法一致。
3.4 circle_r2_analytic
对于代价函数
f
(
a
,
b
,
r
2
)
=
r
2
−
[
(
x
i
−
a
)
2
+
(
y
i
−
b
)
2
]
f(a,b,r2) = r2 - [(x_i - a)^2 + (y_i - b)^2]
f(a,b,r2)=r2−[(xi−a)2+(yi−b)2] ，偏导函数如下：
{
∂
f
∂
a
=
2
∗
(
x
i
−
a
)
∂
f
∂
b
=
2
∗
(
y
i
−
b
)
∂
f
∂
r
2
=
1
\left\{\begin{matrix} \frac{\partial f}{\partial a} = 2 * (x_i - a) \\ \frac{\partial f}{\partial b} = 2 * (y_i - b) \\ \frac{\partial f}{\partial r2} = 1 \end{matrix}\right.
⎩
⎨
⎧∂a∂f=2∗(xi−a)∂b∂f=2∗(yi−b)∂r2∂f=1
完整代码如下：
#include "ceres/ceres.h"
#include "glog/logging.h"
#include "gflags/gflags.h"
#include "easyx.h"
#include "opencv2/core.hpp"
class DIstanceFromCircleCost
: public ceres::SizedCostFunction<1, 1, 1, 1>
{
public:
DIstanceFromCircleCost(double x, double y) : x_(x), y_(y) {}
// 用户自定义残差计算方法
virtual bool Evaluate(double const* const* x, double* residuals, double** jacobians) const
{
// 本例中有3个输入参数块，每个参数块中有1个参数
double a = x[0][0];
double b = x[1][0];
double r2 = x[2][0];
// 本例中输出残差维度为1
double xp = x_ - a;
double yp = y_ - b;
residuals[0] = r2 - xp * xp - yp * yp;
if (jacobians == NULL)
{
return true;
}
// 残差对第1个参数块中的参数依次求偏导，即对a求偏导
if (jacobians[0] != NULL)
{
jacobians[0][0] = 2 * xp;
}
// 残差对第2个参数块中的参数依次求偏导，即对b求偏导
if (jacobians[1] != NULL)
{
jacobians[1][0] = 2 * yp;
}
// 残差对第3个参数块中的参数依次求偏导，即对m求偏导
if (jacobians[2] != NULL)
{
jacobians[2][0] = 1;
}
return true;
}
private:
const double x_;
const double y_;
};
int main(int argc, char** argv)
{
google::InitGoogleLogging(argv[0]);
// 生成数据
const int kNumObservations = 100; // 生成100个点
double sigma = 1.0; // 标准差
cv::RNG rng; // OpenCV随机数产生器
double data[2 * kNumObservations];
double a_ideal = 300.0;
double b_ideal = 200.0;
double r_ideal = 100.0;
for (int i = 0; i < kNumObservations; ++i)
{
double x = a_ideal + r_ideal * std::cos(i * CV_2PI / kNumObservations);
double y = b_ideal + r_ideal * std::sin(i * CV_2PI / kNumObservations);
double noise_x = rng.gaussian(sigma);
double noise_y = rng.gaussian(sigma);
data[2 * i] = x + noise_x;
data[2 * i + 1] = y + noise_y;
}
// 随机加入异常点
data[20] = 300.0, data[21] = 200.0;
data[22] = 100.0, data[23] = 150.0;
data[50] = 500.0, data[51] = 100.0;
// 设置参数初始值
// 输入3个参数块，每个参数块的维度为1
const double initial_a = 0;
const double initial_b = 0;
const double initial_r2 = 0.1;
double a = initial_a;
double b = initial_b;
double r2 = initial_r2;
// 构建非线性最小二乘问题
ceres::Problem problem;
for (int i = 0; i < kNumObservations; ++i)
{
// 添加残差块，需要依次指定代价函数、损失函数、参数块
// 本例中损失函数为Huber函数
problem.AddResidualBlock(
// 输出残差维度为1，输出参数块有3个，每个参数块维度都为1
new DIstanceFromCircleCost(data[2 * i], data[2 * i + 1]),
// Huber损失函数
new ceres::HuberLoss(sigma)/*NULL*/,
// 第1个参数块
&a,
// 第2个参数块
&b,
// 第3个参数块
&r2);
}
// 配置求解器参数
ceres::Solver::Options options;
// 设置最大迭代次数
options.max_num_iterations = 500;
// 指定线性求解器来求解问题
options.linear_solver_type = ceres::DENSE_QR;
// 输出每次迭代的信息
options.minimizer_progress_to_stdout = true;
// 输出日志内容
ceres::Solver::Summary summary;
// 开始优化求解
ceres::Solve(options, &problem, &summary);
double r = std::sqrt(r2);
// 输出优化过程及结果
std::cout << summary.FullReport() << "\n";
std::cout << "initial a: " << initial_a << "->" << a << "\n";
std::cout << "initial b: " << initial_b << "->" << b << "\n";
std::cout << "initial r: " << initial_r2 << "->" << r << "\n";
// 创建绘图窗口，大小为 640x480 像素
initgraph(640, 480);
// 设置背景颜色为白色
setbkcolor(WHITE);
cleardevice();
// 绘制观测点、十字线、黑色
setlinecolor(BLACK);
for (int i = 0; i < kNumObservations; ++i)
{
int size = 3;
double center_x = data[2 * i];
double center_y = data[2 * i + 1];
line(center_x - size, center_y, center_x + size, center_y);
line(center_x, center_y - size, center_x, center_y + size);
}
// 绘制理想圆，蓝色，圆心（300，200），半径 100
setlinestyle(PS_SOLID, 2);
setlinecolor(BLUE);
circle(a_ideal, b_ideal, r_ideal);
// 绘制鲁棒拟合圆，绿色
setlinecolor(GREEN);
circle(a, b, r);
std::system("pause");
closegraph();
return 0;
}
结果与自动微分法一致。
1064
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
