rpc 非线性方法

go.mod

@@ -63,6 +63,7 @@ require (
 	golang.org/x/oauth2 v0.16.0 // indirect
 	golang.org/x/term v0.20.0 // indirect
 	golang.org/x/text v0.15.0 // indirect
+	golang.org/x/tools v0.20.0 // indirect
 	google.golang.org/api v0.155.0 // indirect
 	google.golang.org/genproto v0.0.0-20240123012728-ef4313101c80 // indirect
 	google.golang.org/genproto/googleapis/rpc v0.0.0-20240311173647-c811ad7063a7 // indirect

go.sum

@@ -1716,6 +1716,8 @@ golang.org/x/tools v0.7.0/go.mod h1:4pg6aUX35JBAogB10C9AtvVL+qowtN4pT3CGSQex14s=
 golang.org/x/tools v0.8.0/go.mod h1:JxBZ99ISMI5ViVkT1tr6tdNmXeTrcpVSD3vZ1RsRdN4=
 golang.org/x/tools v0.9.1/go.mod h1:owI94Op576fPu3cIGQeHs3joujW/2Oc6MtlxbF5dfNc=
 golang.org/x/tools v0.10.0/go.mod h1:UJwyiVBsOA2uwvK/e5OY3GTpDUJriEd+/YlqAwLPmyM=
+golang.org/x/tools v0.20.0 h1:hz/CVckiOxybQvFw6h7b/q80NTr9IUQb4s1IIzW7KNY=
+golang.org/x/tools v0.20.0/go.mod h1:WvitBU7JJf6A4jOdg4S1tviW9bhUxkgeCui/0JHctQg=
 golang.org/x/xerrors v0.0.0-20190717185122-a985d3407aa7/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0=
 golang.org/x/xerrors v0.0.0-20191011141410-1b5146add898/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0=
 golang.org/x/xerrors v0.0.0-20191204190536-9bdfabe68543/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0=

pkg/calculator/camera.go

@@ -42,6 +42,10 @@ func CameraDirectionVec(u, v float64) []float64 {
 	directionVec[1] = (v - PANPixels/2) * PANCellSize / 1000.0 / 1000.0 // y方向, mm
 	directionVec[2] = -FocalLength / 1000.0                             // z方向, mm
 
+	// deltaFOV := (FOV * math.Pi / 180) / PANPixels
+	// directionVec[1] = math.Tan((v - PANPixels/2) * deltaFOV)
+	// directionVec[2] = -1
+
 	// 归一化
 	// fmt.Printf("Direction Vector: (%.6f, %.6f, %.6f) \n", directionVec[0], directionVec[1], directionVec[2])
 	// directionVec = normalizeVec(directionVec)

pkg/producer/grid_img.go

@@ -22,7 +22,7 @@ func gridImage2(m, n, height, width, k, hmin, hmax int) []*GridPoint {
 	hmin = hmin - 500
 	dh := (hmax - hmin) / (k)
 	var h []int
-	for i := 0; i <= k; i++ {
+	for i := 1; i <= k; i++ {
 		h = append(h, hmin+dh*i)
 	}
 

pkg/producer/rpc.go

@@ -56,8 +56,8 @@ type RPCModel struct {
 // rational polynomial coeffients
 func NewRPC(r *Registrator, scene *Scene, rpb string) *RPC {
 	rpc := RPC{
-		elevationLayer: 5,
-		gridsize:       5,
+		elevationLayer: 4,
+		gridsize:       19,
 		registrator:    r,
 		scene:          scene,
 		rpb:            rpb,

pkg/producer/rpc_helper.go

@@ -3,11 +3,19 @@ package producer
 import (
 	"fmt"
 	"math"
+	"sort"
 
 	log "github.com/sirupsen/logrus"
 	"gonum.org/v1/gonum/mat"
 )
 
+type VX struct {
+	v float64
+	x mat.VecDense
+}
+
+const epsilon = 1e-4
+
 func normalize(v *mat.VecDense) (*mat.VecDense, float64, float64) {
 	var vOff, vScale float64
 	vOff = mat.Sum(v) / float64(v.Len())
@@ -34,102 +42,102 @@ func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, e
 	for i := 0; i < n; i++ {
 		weights.SetDiag(i, 1.0)
 	}
+
+	// 计算初始值
+	var x0 mat.VecDense
 	w2 := mat.NewDiagDense(n, nil)
+	for i := 0; i < n; i++ {
+		w2.SetDiag(i, weights.At(i, i)*weights.At(i, i))
+	}
+	var MtW2 mat.Dense
+	MtW2.Mul(M.T(), w2)
+	var MtW2M mat.Dense
+	MtW2M.Mul(&MtW2, M)
+	invMtW2M, err := invertRPCMatrix(&MtW2M)
+	if err != nil {
+		return nil, err
+	}
+	var MtW2F mat.VecDense
+	MtW2F.MulVec(&MtW2, f)
+	x0.MulVec(invMtW2M, &MtW2F)
 
+	numerator := mat.NewVecDense(20, nil)
+	denominator := mat.NewVecDense(20, nil)
+	denominator.SetVec(0, 1.0)
+	numerator.SetVec(0, x0.AtVec(0))
+	for i := 1; i < 20; i++ {
+		numerator.SetVec(i, x0.AtVec(i))
+		denominator.SetVec(i, x0.AtVec(i+19))
+	}
+
+	num, den, err := solveNelderMead(numerator, denominator, f, latVec, lonVec, heightVec)
+	if err != nil {
+		return nil, err
+	}
+
+	var coeffs []float64
+	coeffs = append(coeffs, num.RawVector().Data...)
+	for i := 1; i < 20; i++ {
+		coeffs = append(coeffs, den.AtVec(i))
+	}
+	return coeffs, nil
+
+	// 迭代
+	var wm mat.Dense
+	var wmx, wf mat.VecDense
+	var x1 mat.VecDense
+
+	var vx []*VX
+	v0 := 0.0
 	iterations := 0
-	var x mat.VecDense
-	// var e0 float64
+	maxIterations := 10
 
-	for iterations < 20 {
-		iterations++
-
-		// w2 = weights^2
+	for iterations < maxIterations {
+		denominator.SetVec(0, 1.0)
+		for idx := 1; idx < 20; idx++ {
+			denominator.SetVec(idx, x0.AtVec(idx+19))
+		}
+		weights = setupWeightMatrix(denominator, latVec, lonVec, heightVec)
 		for i := 0; i < n; i++ {
 			w2.SetDiag(i, weights.At(i, i)*weights.At(i, i))
 		}
-
-		// x = (M^T * w2 * M)^-1 * M^T * w2 * R
-		var MtW2 mat.Dense
 		MtW2.Mul(M.T(), w2)
-		var MtW2M mat.Dense
 		MtW2M.Mul(&MtW2, M)
-
 		invMtW2M, err := invertRPCMatrix(&MtW2M)
 		if err != nil {
 			return nil, err
 		}
-		var MtW2F mat.VecDense
 		MtW2F.MulVec(&MtW2, f)
+		x1.MulVec(invMtW2M, &MtW2F)
 
-		x.MulVec(invMtW2M, &MtW2F)
+		wm.Mul(weights, M)
+		wmx.MulVec(&wm, &x1)
+		wf.MulVec(weights, f)
+		wmx.SubVec(&wmx, &wf)
+		wmx.MulElemVec(&wmx, &wmx)
+		v := math.Sqrt(mat.Max(&wmx))
+		log.Println("iteration:", iterations, "v-err:", v)
 
-		numerator := mat.NewVecDense(20, nil)
-		denominator := mat.NewVecDense(20, nil)
-		numerator.SetVec(0, x.AtVec(0))
-		denominator.SetVec(0, 1.0)
-		for idx := 1; idx < 20; idx++ {
-			numerator.SetVec(idx, x.AtVec(idx))
-			denominator.SetVec(idx, x.AtVec(idx+19))
-		}
-
-		weights = setupWeightMatrix(denominator, latVec, lonVec, heightVec)
-
-		// residual = m.t()*w2*(m*tempCoeff-r);
-		// var temp0, temp1, residual mat.VecDense
-		// temp0.MulVec(M, &x)
-		// temp1.SubVec(&temp0, f)
-		// residual.MulVec(&MtW2, &temp1)
-		// var square mat.Dense
-		// square.Mul(residual.T(), &residual)
-		// residualValue := math.Sqrt(square.At(0, 0))
-		// if residualValue < 0.0001 {
-		// 	break
-		// }
-
-		// fmt.Printf("residual value: %.16f\n", residualValue)
-		// fmt.Printf("iterations: %d\n", iterations)
-
-		e := 0.0
-		for i := 0; i < n; i++ {
-			Rcal := project(numerator, denominator, latVec.AtVec(i), lonVec.AtVec(i), heightVec.AtVec(i))
-			e += math.Pow(Rcal-f.AtVec(i), 2)
-		}
-		e = math.Sqrt(e / float64(n))
-		fmt.Println("iterations:", iterations, "r error:", e)
-
-		if e < 1e-5 {
+		vx = append(vx, &VX{v: v, x: x1})
+		if math.Abs(v-v0) < epsilon {
 			break
 		}
 
-		// dnum := mat.NewVecDense(20, nil)
-		// b0, b1 := true, true
-		// dden := mat.NewVecDense(19, nil)
-		// for i := 0; i < n; i++ {
-		// 	dnum.SetVec(i, math.Abs(numerator.AtVec(i)-numerator0.AtVec(i)))
-		// 	numerator0.SetVec(i, numerator.AtVec(i))
-		// 	fmt.Println("dnum:", i, dnum.AtVec(i))
-		// 	if dnum.AtVec(i) > 0.000001 {
-		// 		b0 = false
-		// 		break
-		// 	}
-		// }
-		// for i := 1; i < n; i++ {
-		// 	dden.SetVec(i, math.Abs(denominator.AtVec(i)-denominator0.AtVec(i)))
-		// 	denominator0.SetVec(i, denominator.AtVec(i))
-		// 	if dden.AtVec(i) > 0.000001 {
-		// 		b1 = false
-		// 		break
-		// 	}
-		// }
-
-		// if b0 && b1 {
-		// 	break
-		// }
+		x0 = x1
+		v0 = v
+		iterations++
 	}
 
 	log.Println("iterations:", iterations)
+	if iterations == maxIterations {
+		sort.Slice(vx, func(i, j int) bool {
+			return vx[i].v < vx[j].v
+		})
+		x0 = vx[0].x
+		log.Printf("select min v-err: %.8f", vx[0].v)
+	}
 
-	return mat.Col(nil, 0, &x), nil
+	return mat.Col(nil, 0, &x0), nil
 }
 
 func setupSystemOfEquations(Rn, latVec, lonVec, heightVec *mat.VecDense) *mat.Dense {
@@ -152,7 +160,7 @@ func setupSystemOfEquations(Rn, latVec, lonVec, heightVec *mat.VecDense) *mat.De
 		B.Set(i, 7, L*L)
 		B.Set(i, 8, P*P)
 		B.Set(i, 9, H*H)
-		B.Set(i, 10, P*L*H)
+		B.Set(i, 10, L*P*H)
 		B.Set(i, 11, L*L*L)
 		B.Set(i, 12, L*P*P)
 		B.Set(i, 13, L*H*H)
@@ -171,7 +179,7 @@ func setupSystemOfEquations(Rn, latVec, lonVec, heightVec *mat.VecDense) *mat.De
 		B.Set(i, 26, -L*L*r)
 		B.Set(i, 27, -P*P*r)
 		B.Set(i, 28, -H*H*r)
-		B.Set(i, 29, -P*L*H*r)
+		B.Set(i, 29, -L*P*H*r)
 		B.Set(i, 30, -L*L*L*r)
 		B.Set(i, 31, -L*P*P*r)
 		B.Set(i, 32, -L*H*H*r)
@@ -206,7 +214,7 @@ func setupWeightMatrix(coeffs, latVec, lonVec, heightVec *mat.VecDense) *mat.Dia
 		row.Set(i, 7, L*L)
 		row.Set(i, 8, P*P)
 		row.Set(i, 9, H*H)
-		row.Set(i, 10, P*L*H)
+		row.Set(i, 10, L*P*H)
 		row.Set(i, 11, L*L*L)
 		row.Set(i, 12, L*P*P)
 		row.Set(i, 13, L*H*H)

pkg/producer/rpc_optimize.go

@@ -0,0 +1,64 @@
+package producer
+
+import (
+	"fmt"
+
+	log "github.com/sirupsen/logrus"
+	"gonum.org/v1/gonum/mat"
+	"gonum.org/v1/gonum/optimize"
+)
+
+type NormElements struct {
+	LineOff, SampOff, LatOff, LonOff, HeightOff           float64
+	LineScale, SampScale, LatScale, LonScale, HeightScale float64
+}
+
+func objectiveFunc(numerator, denominator, f, latVec, lonVec, heightVec *mat.VecDense) float64 {
+	errorSquared := 0.0
+	n := f.Len()
+	for i := 0; i < n; i++ {
+		predictedV := project(numerator, denominator, latVec.AtVec(i), lonVec.AtVec(i), heightVec.AtVec(i))
+		errorV := predictedV - f.AtVec(i)
+		errorSquared += errorV * errorV
+	}
+	return errorSquared / float64(n)
+}
+
+func solveNelderMead(num, den, f, latVec, lonVec, heightVec *mat.VecDense) (*mat.VecDense, *mat.VecDense, error) {
+	var initCoeffs []float64 // a0-a19,b1-b19
+	initCoeffs = append(initCoeffs, num.RawVector().Data...)
+	for i := 1; i < 20; i++ {
+		initCoeffs = append(initCoeffs, den.AtVec(i))
+	}
+	problem := optimize.Problem{
+		Func: func(coeffs []float64) float64 {
+			numerator := mat.NewVecDense(20, nil)
+			denominator := mat.NewVecDense(20, nil)
+			denominator.SetVec(0, 1.0)
+			numerator.SetVec(0, coeffs[0])
+			for i := 1; i < 20; i++ {
+				numerator.SetVec(i, coeffs[i])
+				denominator.SetVec(i, coeffs[i+19])
+			}
+			return objectiveFunc(numerator, denominator, f, latVec, lonVec, heightVec)
+		},
+	}
+
+	result, err := optimize.Minimize(problem, initCoeffs, &optimize.Settings{}, &optimize.NelderMead{})
+	if err != nil {
+		log.Error("Error in RPC optimization: ", err)
+		return nil, nil, err
+	}
+
+	numerator := mat.NewVecDense(20, nil)
+	denominator := mat.NewVecDense(20, nil)
+	denominator.SetVec(0, 1.0)
+	numerator.SetVec(0, result.X[0])
+	fmt.Println("len of result.X: ", len(result.X))
+	for i := 1; i < 20; i++ {
+		numerator.SetVec(i, result.X[i])
+		denominator.SetVec(i, result.X[i+19])
+	}
+
+	return numerator, denominator, nil
+}