blog
工业测量拟合
工业测量中的圆拟合
在工业拟合中，有些设备是平面二次曲线。圆形作为二次曲线的一种这里进行拟合方法阐述。 圆形是工业产品中常见的形状，如管子、圆盘等。一般质量控制工程师会对生产线上的产品是否符合工业标准进行测试。 使用传感器可以记录工业产品圆周上点的坐标，进而利用坐标进行拟合。对于一般测量系统如果是非线性问题，则可以进行线性化处理。在同济大学王解先教授的《工业测量拟合》中对圆进行线性化处理。 因此需要微分改正，并进行迭代处理。
这里对圆拟合方程进行简单变换，可以使得非线性问题化为线性问题处理。因此只需要求解超定线性方程即可，无需迭代即可得到数值解精确结果。下面简述方法。
为拟合一个圆 \[{\left( {x - {c_1}} \right)^2} + {\left( {y - {c_2}} \right)^2} = {r^2}\] 对于n个样本坐标，我们需要确定圆形和半径。改写以上方程得到 \[2x{c_1} + 2y{c_2} + \left( {{r^2} - c_1^2 - c_2^2} \right) = {x^2} + {y^2}\]
令 \[{c_3} = {r^2} - c_1^2 - c_2^2\] 则方程可以写为 \[2x{c_1} + 2y{c_2} + {c_3} = {x^2} + {y^2}\] 将数据代入方程得到超定方程组 \[\left[ {\begin{array}{*{20}{c}} {2{x_1}}&{2{y_1}}&1 \\ {2{x_1}}&{2{y_2}}&1 \\ \vdots & \vdots & \vdots \\ {2{x_n}}&{{2_n}}&1 \end{array}} \right]\left[ \begin{gathered} {c_1} \hfill \\ {c_2} \hfill \\ {c_3} \hfill \\ \end{gathered} \right] = \left[ \begin{gathered} x_1^2 + y_1^2 \hfill \\ x_2^2 + y_2^2 \hfill \\ \vdots \hfill \\ x_n^2 + y_n^2 \hfill \\ \end{gathered} \right]\] 求的最小二乘解后，则最小二乘意义下最优圆心为\[\left[ {{c_1},{c_2}} \right]\]，半径为 \[r = \sqrt {{c_3} + c_1^2 + c_2^2} \]
go语言代码为
package main
import (
"bufio"
"fmt"
"io"
"math"
"os"
"strconv"
"strings"
)
func Vec(N int) (V []float64) {
/*------------------------------------------------------
! Author : Song Yezhi
! verison : 2020-5-24 17:50
!
!
! -----------------------------------------------------
! Input Parameters :
!
! Output Parameters :
!
------------------------------------------------------*/
V = make([]float64, N)
for i := 0; i < N; i++ {
V[i] = 0.0
}
return
}
func Mat(M int, N int) (A [][]float64) {
/*------------------------------------------------------
! Author : Song Yezhi
! verison : 2020-5-23 23:43
!
! creat a two dimention slices (matrix)
! -----------------------------------------------------
! Input Parameters :
!
! Output Parameters :
!
------------------------------------------------------*/
A = make([][]float64, M)
for i := 0; i < M; i++ {
A[i] = make([]float64, N)
}
for i := 0; i < M; i++ {
for j := 0; j < N; j++ {
A[i][j] = 0.0
}
}
return
}
func MatDim(A [][]float64) (M int, N int) {
/*------------------------------------------------------
! Author : Song Yezhi
! verison : 2020-5-24 12:45
------------------------------------------------------*/
M = len(A)
N = len(A[M-1])
return
}
func VecOutput(V []float64) {
N := len(V)
for i := 0; i < N; i++ {
fmt.Printf("%18.6f\n", V[i])
}
}
func VecDot(x1 []float64, x2 []float64) (dot float64) {
/*------------------------------------------------------
! Author : Song Yezhi
! verison : 2020-5-19 22:53
!
! dot product
------------------------------------------------------*/
N := len(x1)
var i int
dot = 0.0
for i = 0; i < N; i++ {
dot = dot + x1[i]*x2[i]
}
return
}
func MatVec(A [][]float64, x []float64) (b []float64) {
/*------------------------------------------------------
! Author : Song Yezhi
! verison : 2020-5-19 22:25
! b = Ax
!
! -----------------------------------------------------
! Input Parameters :
! A[M][N] ----MATRIX
! Output Parameters :
! b[M]-----vector
------------------------------------------------------*/
M := len(A)
N := len(A[0])
//the size of mat
//b = make([]float64, M)
b = Vec(M)
var sum float64
for i := 0; i < M; i++ {
sum = 0.0
for j := 0; j < N; j++ {
sum = sum + A[i][j]*x[j]
}
b[i] = sum
}
return
}
func MatMul(A [][]float64, B [][]float64) (C [][]float64) {
/*------------------------------------------------------
! Author : Song Yezhi
! verison : 2020-5-23 23:27
! C = A*B
!
! -----------------------------------------------------
! Input Parameters :
! A[M][N]
! B[N][P]
! Output Parameters :
! C[M][P]
------------------------------------------------------*/
M := len(A)
N := len(A[M-1])
P := len(B[N-1])
var i, j, k int
C = Mat(M, P)
var sum float64
for i = 0; i < M; i++ {
for j = 0; j < P; j++ {
sum = 0.0
for k = 0; k < N; k++ {
sum = sum + A[i][k]*B[k][j]
C[i][j] = sum
}
}
}
return
}
func MatOutput(A [][]float64) {
/*------------------------------------------------------
! Author : Song Yezhi
! verison : 2020-5-24 16:54
!
------------------------------------------------------*/
M, N := MatDim(A)
for i := 0; i < M; i++ {
for j := 0; j < N; j++ {
fmt.Printf("%18.6f ", A[i][j])
}
fmt.Printf("\n")
}
}
func LDL(A [][]float64) (L [][]float64, D []float64) {
/*------------------------------------------------------
! Author : Song Yezhi
! verison : 2020-5-24 17:24
!
! A=LDL'
! -----------------------------------------------------
! Input Parameters :
! A
! Output Parameters :
! L
! D
------------------------------------------------------*/
M, N := MatDim(A)
if M != N {
fmt.Printf("the rows and cols is not equal")
}
G := Mat(N, N)
L = Mat(N, N)
D = Vec(N)
var i, j, k int
D[0] = A[0][0]
var tmp1 float64
for i = 1; i < N; i++ {
for j = 0; j <= i-1; j++ {
tmp1 = 0.0
for k = 0; k <= j-1; k++ {
tmp1 = tmp1 + G[i][k]*L[j][k]
}
G[i][j] = A[i][j] - tmp1
}
for j = 0; j <= i-1; j++ {
L[i][j] = G[i][j] / D[j]
}
//comput D[i]
tmp1 = 0.0
for k = 0; k <= i-1; k++ {
tmp1 = tmp1 + G[i][k]*L[i][k]
}
D[i] = A[i][i] - tmp1
}
//matOutput(L)
//set the diag elements
for i = 0; i < N; i++ {
L[i][i] = 1.0
}
return
}
func AddNeq(ATA [][]float64, ATb []float64, H []float64, oc float64) {
/*------------------------------------------------------
Created : Song Yezhi 2022-5-21 14:15
add a new observation to normal equations
(ATA+ H'H) x = ATb + H'oc
ATA = ATA + H'H
ATb = ATb + H'oc
--------------------------------------------------------
Input Parameters :
ATA
ATb
H
oc
Output Parameters :
ATA --- updated
ATb --- updated
--------------------------------------------------------
Email : song.yz@foxmail.com
Copyrigt (C) : Chinese Academy of Sciences
All rights reserved, 2022
-------------------------------------------------------*/
N := len(ATA)
var i, j int
for i = 0; i < N; i++ {
for j = 0; j < N; j++ {
ATA[i][j] = ATA[i][j] + H[i]*H[j]
}
ATb[i] = ATb[i] + H[i]*oc
}
}
func LeastSquareEquation(ATA [][]float64, ATb []float64) (X []float64) {
/*------------------------------------------------------
Created : Song Yezhi 2022-5-21 20:04
solve Least Square Equation by LDL'
--------------------------------------------------------
Input Parameters :
Output Parameters :
--------------------------------------------------------
Email : song.yz@foxmail.com
Copyrigt (C) : Chinese Academy of Sciences
All rights reserved, 2022
-------------------------------------------------------*/
N := len(ATA)
L := Mat(N, N)
D := Vec(N)
L, D = LDL(ATA)
X = Vec(N)
var tmp1 float64
var i, k int
y := Vec(N)
y[0] = ATb[0]
for i = 1; i < N; i++ {
tmp1 = 0.0
for k = 0; k <= i-1; k++ {
tmp1 = tmp1 + L[i][k]*y[k]
}
y[i] = ATb[i] - tmp1
}
X[N-1] = y[N-1] / D[N-1]
//-------------------------
for i=N-1; i>=0 ;i--{
tmp1 = 0.0
for k = i + 1; k < N; k++ {
tmp1 = tmp1 + L[k][i]*X[k]
}
X[i] = y[i]/D[i] - tmp1
}
return
}
func main() {
fin, _ := os.Open("obs.txt")
defer fin.Close()
ATA := Mat(3, 3)
ATb := Vec(3)
H := Vec(3)
br := bufio.NewReader(fin)
var word []string
var x, y, oc float64
for {
var line string
line, ioerr := br.ReadString('\n')
if ioerr == io.EOF {
break
}
if strings.TrimSpace(line) == "" {
continue
}
if line[0] == '#' {
continue
}
word = strings.Fields(line)
x, _ = strconv.ParseFloat(word[0], 64)
y, _ = strconv.ParseFloat(word[1], 64)
H[0] = 2.0 * x
H[1] = 2.0 * y
H[2] = 1.0
oc = x*x + y*y
AddNeq(ATA, ATb, H, oc)
}
X := LeastSquareEquation(ATA, ATb)
fmt.Printf("the center of circle is %16.6f %16.6f \n", X[0], X[1])
fmt.Printf("the radius of circle is %16.6f \n", math.Sqrt(X[2]+X[0]*X[0]+X[1]*X[1]))
}
187.6604 29.391
24.9298 57.083
6.8860 47.580
-49.0342 -124.730
120.7845 -213.135
215.0836 -136.364
输出结果为
the center of circle is 85.794936 -73.195089 the radius of circle is 144.186642
fortran代码为
subroutine add_neq(ATA,ATb,oc,H,N)
!---------------------------------subroutine comment
! Purpose : 2018-09-05 13:33 (Created)
! add a new observation to normal equations
! (ATA+ H'H) x = ATb + H'oc
! Changes :
! *.
!-----------------------------------------------------
! Input Parameters :
! ATA ---- initial normal matrix
! Atb ---- initial right vector
! oc ----- o-c for liner equation
! H ------ obs vector
! N ------- the dimention of the matrix
! Output Parameters :
! ATA ------ updated normal matrix
! ATb ------- updated right vector
!-----------------------------------------------------
! Author : Song Yezhi
! Copyrigt (C) : Shanghai Astronomical Observatory,CAS
! (All rights reserved, 2018)
!----------------------------------------------------
implicit real*8(a-h,o-z)
real*8:: ATA(N,N),ATb(N) ,H(N)
do i = 1,N
if (H(i)==0d0) cycle
do j =1 ,N
ATA(i,j) = ATA(i,j)+ H(i)*H(j)
end do
ATb(i)= ATb(i)+H(i)*oc
end do
end subroutine add_neq
subroutine chol_eq(A,b,x,P,N)
!---------------------------------subroutine comment
! Version : V1.0
! Coded by : song.yz
! Date : 2019.04.15
!-----------------------------------------------------
! Purpose : 对称正定方程解算（如法方程）
!
! Post Script :
! 1. 修正的不开平方Cholesky分解法
! 2.
! 3.
!-----------------------------------------------------
! Input parameters :
! 1. A(N,N)对称正定系数矩阵
! 2. b(N)右向量
! 3. N--方程维数
! 4.
! Output parameters :
! 1. x-计算结果
! 2. P ----协方差 （由于A已经为对称正定矩阵<法矩阵>，因此P为A的逆）
! 3.
! 4.
!-----------------------------------------------------
! Copyrigt :
! Center for Astro-geodynamics
! Shanghai Astronomical Observatory
! Chinese Academy of Sciences
!----------------------------------------------------
implicit real*8(a-z)
integer::N
real*8::A(N,N),b(n),x(n),P(N,N)
!---------------------------------subroutine variable
real*8::L(N,N),d(n)
integer::i,k
real*8::y(N)
call chol_rf(A,L,d,P,n)
!以上已经得到Cholesky分解
y(1)=b(1)
do i=2,n
tmp1=0d0
do k=1,i-1
tmp1=tmp1+L(i,k)*y(k)
end do
y(i)=b(i)-tmp1
end do
x(n)=y(n)/d(n)
do i=n-1,1,-1
tmp1=0d0
do k=i+1,n
tmp1=tmp1+L(k,i)*x(k)
end do
x(i)=y(i)/d(i)-tmp1
end do
end subroutine chol_eq
subroutine chol_rf(A,L,d,P,N)
!---------------------------------subroutine comment
! Version : V1.0
! Coded by : syz
! Date : 2019.04.15
!-----------------------------------------------------
! Purpose :
!
! Post Script :
! 1. 不用开平方的Cholesky分解
! 2.
! 3.
!-----------------------------------------------------
! Input parameters :
! 1. A(N,N)--输入矩阵
! 2. N----矩阵维书
! 3.
! 4.
! Output parameters :
! 1. L矩阵
! 2. d---对角矩阵（用向量存储）
! 3.
! 4. P ---协方差矩阵（由于A已经为对称正定矩阵<法矩阵>，因此P为A的逆）
!-----------------------------------------------------
! Copyrigt :
! Center for Astro-geodynamics
! Shanghai Astronomical Observatory
! Chinese Academy of Sciences
!----------------------------------------------------
implicit real*8(a-z)
integer::N
real*8::A(n,n),L(n,n),d(n),P(N,N)
!---------------------------------subroutine variable
integer::i,j
real*8:: g(n,n)
!设置初值
L=0d0
d(1)=a(1,1)
do i=2,n
do j=1,i-1
!此层循环算g(i,j)
tmp1=0d0
do k=1,j-1
tmp1=tmp1+g(i,k)*L(j,k)
end do
g(i,j)=a(i,j)-tmp1
end do
do j=1,i-1
!此层循环算L(i,j)
L(i,j)=g(i,j)/d(j)
end do
!以下算d(i)
tmp1=0d0
do k=1,i-1
tmp1=tmp1+g(i,k)*L(i,k)
end do
d(i)=a(i,i)-tmp1
end do
!设置对角线元素
do i=1,N
L(i,i)=1d0
end do
call covariance(L,D,P,N)
end subroutine chol_rf
subroutine covariance(L,D,P,N)
!-----------------------------------------------------
! Purpose : 2019-04-15 13:08 (Created)
! 根据LDL'分解计算协方差矩阵
!
! Changes :
!
!-----------------------------------------------------
! Input Parameters :
! L -----
! D ---- 对角线元素
! N ---- 矩阵维数
! Output Parameters :
!
!
!-----------------------------------------------------
! Author : Song Yezhi
! Copyrigt (C) : Shanghai Astronomical Observatory,CAS
! (All rights reserved, 2019)
!----------------------------------------------------
implicit none
integer :: N
real*8 :: L(N,N),D(N),P(N,N)
!-----------------------------------------------------
integer :: i
real*8 :: invL(N,N),invLT(N,N)
!-----------------------------------------------------
call inv_dtri(L,invL,N)
invLT = transpose(invL)
do i =1,N
invLT(:,i)=invLT(:,i)/D(i)
end do
P = matmul(invLT,invL)
end subroutine covariance
subroutine inv_dtri(L,invL,N)
!-----------------------------------------------------
! Purpose : 2019-04-15 10:45 (Created)
! 计算下三角矩阵的逆矩阵，回带过程,无开平方运算
!
! Changes :
!
!-----------------------------------------------------
! Input Parameters :
! L ----- 下三角矩阵
! N ----- 矩阵维数
!
! Output Parameters :
! invL ------ 下三角的逆矩阵
!
!-----------------------------------------------------
! Author : Song Yezhi
! Copyrigt (C) : Shanghai Astronomical Observatory,CAS
! (All rights reserved, 2019)
!----------------------------------------------------
implicit none
integer :: N
real*8 :: L(N,N),invL(N,N)
!----------------------------------------------------
integer :: i , j ,k
real*8 :: xsum
!-----------------------------------------------------
invL = 0d0
invL(1,1) = 1/L(1,1)
do i = 2, N
invL(i,i)= 1/L(i,i)
do j = 1,i-1
xsum = 0d0
do k = 1,i
xsum = xsum + L(i,k)*invL(k,j)
end do
invL(i,j)= - xsum / L(i,i)
end do
end do
end subroutine inv_dtri
program main
!-----------------------------------------------------
! Created : Song Yezhi 2022-5-21 21:34
! Purpose :
!
! Changes :
!
! Paramters :
!
!-----------------------------------------------------
! Email : song.yz@foxmail.com
! Copyrigt (C) : Chinese Academy of Sciences
! All rights reserved, 2022
!-----------------------------------------------------
implicit none
integer i,j ,ioerr
integer,parameter :: N =3
real*8 :: ATA(N,N) ,ATb(N) ,XX(N) ,H(N) ,oc ,P(N,N)
real*8 :: x,y
open(unit=11,file ='obs.txt')
ATA =0D0
ATb =0d0
do
read(11,*,iostat=ioerr) x,y
if(ioerr/=0) exit
H(1) = 2d0*x
H(2) = 2d0*y
H(3) = 1d0
oc = x*x + y* y
call add_neq(ATA,ATb,oc,H,N)
end do
call chol_eq(ATA,ATb,XX,P,N)
write(*,*) XX
end program main