song.yz@foxmail.com wechat: math-box

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