Halcon最小二乘法拟合二次函数、指数函数、圆

二次函数

输入XY坐标，输出方程系数 A B C

* 现在将生成的点进行二次曲线拟合：f(x) = a*x^2 + b*x + c
*转化为： Ax = B        A=[X^2, X, 1],        x = [a, b, c]        B=f(x)
create_matrix (|X|, 3, 1.0, MatrixA)    //* 创建|x|行3列，值为1.0的矩阵
create_matrix (|X|, 1, X, MatrixACol1)   //* 创建|x|行1列，值为x的矩阵,x== [25:50:|Y|]
create_matrix (|X|, 1, X * X, MatrixACol0) // 创建|x|行1列，值为X * X的矩阵
set_sub_matrix (MatrixA, MatrixACol1, 0, 1)//*用矩阵MatrixACol1来替换矩阵MatrixA中的0行1列中的值
set_sub_matrix (MatrixA, MatrixACol0, 0, 0)//*用矩阵MatrixACol0来替换矩阵MatrixA中的0行0列中的值（这里MatrixA是经过上一步变换之后的矩阵）
create_matrix (|X|, 1, Y, MatrixB) 
solve_matrix (MatrixA, 'general', 0, MatrixB, MatrixX)  //* 解方程即可得到a,b,c的系数，记为矩阵MatrixX
*曲线拟合
Sequence := [0:|Y|]
*该函数是得到矩阵MatrixX中0行0列的值
get_value_matrix (MatrixX, 0, 0, A)
get_value_matrix (MatrixX, 1, 0, B)
get_value_matrix (MatrixX, 2, 0, C)
Distances := A * Sequence * Sequence + B * Sequence + C   //*得到多项式的值
Function := 'f(x) = ' + A$'.2' + '*x^2 + ' + B$'.2' + '*x + ' + C$'.2' + ''

coefficient:=[A,B,C]

return ()

指数函数

输入XY坐标，输出方程系数 A B

* 现在将生成的点进行二次曲线拟合：f(x) = a*e^bx 
*转化为： AX = B        A=[n, sum(x), sum(x), sum(x^2)],        X = [lna, b]        B=[sum(y),sum(x*lny)]
ValueA:=[|X|,sum(X),sum(X),sum(pow(X,2))]
create_matrix (2, 2, ValueA, Matrix_A)    

ValueB:=[sum(log(Y)),sum(X*log(Y))]

create_matrix (2, 1, ValueB, Matrix_B) 

solve_matrix (Matrix_A, 'general', 0, Matrix_B, MatrixX)  //* 解方程即可得到[lna, b]的系数，记为矩阵MatrixX
*曲线拟合
*该函数是得到矩阵MatrixX中0行0列的值
get_value_matrix (MatrixX, 0, 0, lnA)
get_value_matrix (MatrixX, 1, 0, B)

coefficient:=[exp(lnA),B]

return ()

圆

输出圆心、半径、起始角度、终止角度、旋转方向

* 现在将生成的点进行圆拟合： (x-h)^2 + (y-k)^2 =r^2  => -2hx-2ky+D = -(x^2+y^2), D=h^2+k^2-r^2
* 转化为：AX=B    A = (-2x,-2y,1)     X = [h,k,D]    B = (-(x^2+y^2))
* 确定点的数量
N := |Columns|
create_matrix (N, 3, 1.0, MatrixA) 
create_matrix (N, 1, -2* Columns, MatrixACol0)   // A的第一列，创建|x|行1列，值为-2x的矩阵
create_matrix (N, 1, -2* Rows, MatrixACol1) //A的第二列，创建|x|行1列，值为-2y的矩阵
set_sub_matrix (MatrixA, MatrixACol0, 0, 0) //设置A的第一列
set_sub_matrix (MatrixA, MatrixACol1, 0, 1) //设置A的第二列
create_matrix (N, 1, -(Columns*Columns + Rows*Rows), MatrixB) 

solve_matrix (MatrixA, 'general', 0, MatrixB, MatrixX)
get_value_matrix (MatrixX, 0, 0, CenterColumn)
get_value_matrix (MatrixX, 1, 0, CenterRow)
get_value_matrix (MatrixX, 2, 0, D)

Radius := sqrt(pow(CenterColumn,2)+pow(CenterRow,2)-D)
* 计算每个点的极角
Phis := []
for i := 0 to N-1 by 1
    phi := -atan2(Rows[i]-CenterRow, Columns[i]-CenterColumn)
    Phis := [Phis, phi]
endfor

* 对角度进行排序
SortedPhis := sort(Phis)
* 找到起始角度和终止角度
StartPhi := SortedPhis[0]
EndPhi := SortedPhis[N-1]

* 判断旋转方向
if ((SortedPhis[1] - SortedPhis[0]) > 0)
    PointOrder := 'positive'
else
    PointOrder := 'negative'
endif
return ()