From 19522db7c8166a4e25a81c739af6de6d7ad54890 Mon Sep 17 00:00:00 2001 From: nuknal Date: Fri, 30 Aug 2024 17:01:33 +0800 Subject: [PATCH] =?UTF-8?q?rpc=20=E9=9D=9E=E7=BA=BF=E6=80=A7=E6=96=B9?= =?UTF-8?q?=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- go.mod | 1 + go.sum | 2 + pkg/calculator/camera.go | 4 + pkg/producer/grid_img.go | 2 +- pkg/producer/rpc.go | 4 +- pkg/producer/rpc_helper.go | 160 ++++++++++++++++++----------------- pkg/producer/rpc_optimize.go | 64 ++++++++++++++ 7 files changed, 158 insertions(+), 79 deletions(-) create mode 100644 pkg/producer/rpc_optimize.go diff --git a/go.mod b/go.mod index ec63ff2..02c7106 100644 --- a/go.mod +++ b/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 diff --git a/go.sum b/go.sum index 720676e..cd59a41 100644 --- a/go.sum +++ b/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= diff --git a/pkg/calculator/camera.go b/pkg/calculator/camera.go index b20babd..24a162e 100644 --- a/pkg/calculator/camera.go +++ b/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) diff --git a/pkg/producer/grid_img.go b/pkg/producer/grid_img.go index 4383fce..07e8153 100644 --- a/pkg/producer/grid_img.go +++ b/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) } diff --git a/pkg/producer/rpc.go b/pkg/producer/rpc.go index d0ff38d..7d6a062 100644 --- a/pkg/producer/rpc.go +++ b/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, diff --git a/pkg/producer/rpc_helper.go b/pkg/producer/rpc_helper.go index 82e0222..e14cde6 100644 --- a/pkg/producer/rpc_helper.go +++ b/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) diff --git a/pkg/producer/rpc_optimize.go b/pkg/producer/rpc_optimize.go new file mode 100644 index 0000000..e54575e --- /dev/null +++ b/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 +}