blog
工业测量拟合
工业测量中的球面拟合
类似于圆拟合，球面拟合亦可以化为线性形式。 为拟合一个球面 \[{\left( {x - {c_1}} \right)^2} + {\left( {y - {c_2}} \right)^2} + {\left( {z - {c_3}} \right)^2}= {r^2}\] 对于n个样本坐标，我们需要确定圆形和半径。改写以上方程得到 \[2x{c_1} + 2y{c_2} + 2z{c_3} + \left( {{r^2} - c_1^2 - c_2^2 - c_3^2} \right) = {x^2} + {y^2} + {z^2}\]令 \[{c_3} = {r^2} - c_1^2 - c_2^2 -c_3^2\] 则方程可以写为 \[2x{c_1} + 2y{c_2} + 2z{c_3} + {c_4} = {x^2} + {y^2} + {z^2}\] 将数据代入方程得到超定方程组 \[\left[ {\begin{array}{*{20}{c}} {2{x_1}}&{2{y_1}}&{2{z_1}}&1 \\ {2{x_2}}&{2{y_2}}&{2{z_2}}&1 \\ \vdots & \vdots & \vdots & \vdots \\ {2{x_n}}&{2{y_n}}&{2{z_n}}&1 \end{array}} \right]\left[ \begin{gathered} {c_1} \hfill \\ {c_2} \hfill \\ \vdots \hfill \\ {c_3} \hfill \\ \end{gathered} \right] = \left[ \begin{gathered} x_1^2 + y_1^2 + z_1^2 \hfill \\ x_2^2 + y_2^2 + z_2^2 \hfill \\ \vdots \hfill \\ x_n^2 + y_n^2 + z_n^2 \hfill \\ \end{gathered} \right]\] 求的最小二乘解后，则最小二乘意义下最优圆心为$$\left[ {{c_1},{c_2},{c_3}} \right] $$，半径为 \[r = \sqrt {{c_4} + c_1^2 + c_2^2+ c_3^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]
//-------------------------
//i = 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
/*----------------------
if i == 0 {
break
}
i = i-1
*/
}
return
}
func main() {
fin, _ := os.Open("obsSphere.txt")
defer fin.Close()
ATA := Mat(4, 4)
ATb := Vec(4)
H := Vec(4)
br := bufio.NewReader(fin)
var word []string
var x, y,z, 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)
z, _ = strconv.ParseFloat(word[2], 64)
H[0] = 2.0 * x
H[1] = 2.0 * y
H[2] = 2.0 * z
H[3] = 1.0
oc = x*x + y*y + z*z
AddNeq(ATA, ATb, H, oc)
}
X := LeastSquareEquation(ATA, ATb)
fmt.Printf("the center of sphere is %16.6f %16.6f %16.6f\n", X[0], X[1],X[2])
fmt.Printf("the radius of sphere is %16.6f \n", math.Sqrt(X[3]+X[0]*X[0]+X[1]*X[1]+X[2]*X[2]))
}
153.62 3.989 932.46 0.0869 89.915 -5.267 969.386 -0.2558 180.01 -59.28 903.142 -0.0607 204.783 42.755 970.217 0.0285 79.993 16.983 1054.974 0.0723 89.62 -123.551 949.755 0.1623 285.391 10.158 989.71 -0.0437 143.244 25.543 1133.817 -0.2845 158.474 -196.14 968.185 0.0528 319.764 -23.489 1046.427 0.0292 228.021 -17.403 1168.727 -0.0227 85.004 -115.646 1139.649 0.2243 241.736 -209.393 1025.232 -0.2170 284.235 -33.134 1141.093 0.1261 193.791 -97.284 1187.479 -0.0832 169.299 -199.153 1120.8 -0.1152 267.82 -180.76 1105.22 -0.1155 262.578 -99.679 1165.977 0.2943输出结果为
the center of sphere is 187.005955 -78.374358 1045.329632 the radius of sphere is 143.593040