From abe5ecbdecb4e620ed55732d4e1c490be6866048 Mon Sep 17 00:00:00 2001 From: nuknal Date: Wed, 4 Sep 2024 15:15:25 +0800 Subject: [PATCH] =?UTF-8?q?=E7=9B=B8=E6=9C=BA=E5=81=8F=E7=BD=AE=E7=9F=A9?= =?UTF-8?q?=E9=98=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- pkg/calculator/camera.go | 17 +++++++--- pkg/calculator/intersection.go | 2 +- pkg/producer/rpc.go | 4 +-- pkg/producer/rpc_helper.go | 58 ++++++++++++++++++++-------------- pkg/utils/tiff.go | 2 +- 5 files changed, 51 insertions(+), 32 deletions(-) diff --git a/pkg/calculator/camera.go b/pkg/calculator/camera.go index 24a162e..371962b 100644 --- a/pkg/calculator/camera.go +++ b/pkg/calculator/camera.go @@ -14,9 +14,10 @@ const ( PANPixels = float64(payload.PAN_PIXEL_WIDTH) PANCellSize = 3.2 // µm MSSPixels = float64(payload.MSS_PIXEL_WIDTH) - MSSCellSize = 12.8 // µm - CameraRoll = 0.0 // 相机与卫星本体X轴的安装角度, degree FIXME: 安装矩阵应该由卫星方提供 - CameraPitch = 0.0 // 相机与卫星本体Y轴的安装角度, degree + MSSCellSize = 12.8 // µm + CameraRoll = -0.010 // 相机与卫星本体X轴的安装角度, degree FIXME: 安装矩阵应该由卫星方提供 + CameraPitch = 0.183 // 相机与卫星本体Y轴的安装角度, degree + CameraYaw = 0.012 // 相机与卫星本体Z轴的安装角度, degree ) // 计算过程使用PAN分辨率 @@ -80,9 +81,17 @@ func CameraRotMatrix(phi, theta, psi float64) *mat.Dense { -math.Sin(theta), 0, math.Cos(theta), }) + Rz := mat.NewDense(3, 3, []float64{ + math.Cos(psi), -math.Sin(psi), 0, + math.Sin(psi), math.Cos(psi), 0, + 0, 0, 1, + }) + // R = Rz * Ry * Rx RyRx := mat.NewDense(3, 3, nil) RyRx.Mul(Ry, Rx) + Rzxy := mat.NewDense(3, 3, nil) + Rzxy.Mul(Rz, RyRx) - return RyRx + return Rzxy } diff --git a/pkg/calculator/intersection.go b/pkg/calculator/intersection.go index 8e0ed7e..2fbcd37 100644 --- a/pkg/calculator/intersection.go +++ b/pkg/calculator/intersection.go @@ -23,7 +23,7 @@ func Camera2GroundPoint(q Quaternion, satPos84 []float64, satTime time.Time, uca direction := CameraDirectionVec(0, float64(ucam)) // -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 -------- - Rcam := CameraRotMatrix(CameraRoll*math.Pi/180.0, CameraPitch*math.Pi/180.0, 0) + Rcam := CameraRotMatrix(CameraRoll*math.Pi/180.0, CameraPitch*math.Pi/180.0, CameraYaw*math.Pi/180.0) var dCam mat.VecDense dCam.MulVec(Rcam, mat.NewVecDense(3, direction)) diff --git a/pkg/producer/rpc.go b/pkg/producer/rpc.go index a435a4d..2e617de 100644 --- a/pkg/producer/rpc.go +++ b/pkg/producer/rpc.go @@ -157,7 +157,7 @@ func (rpc *RPC) RPC() error { // B := setupSystemOfEquations(rowVec, latVec, lonVec, heightVec) // J, err := SolveNormalEquation(B, rowVec) log.Println("solving row coefficients") - J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec) + J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec, SolveMethodNelderMead) if err != nil { return err } @@ -166,7 +166,7 @@ func (rpc *RPC) RPC() error { // D := setupSystemOfEquations(colVec, latVec, lonVec, heightVec) // K, err := SolveNormalEquation(D, colVec) log.Println("solving col coefficients") - K, err := solveCoefficients(colVec, latVec, lonVec, heightVec) + K, err := solveCoefficients(colVec, latVec, lonVec, heightVec, SolveMethodNelderMead) if err != nil { return err } diff --git a/pkg/producer/rpc_helper.go b/pkg/producer/rpc_helper.go index aaa3f4a..213b126 100644 --- a/pkg/producer/rpc_helper.go +++ b/pkg/producer/rpc_helper.go @@ -14,6 +14,14 @@ type VX struct { x mat.VecDense } +type SolveMethod int + +const ( + SolveMethodOptimize SolveMethod = iota + SolveMethodNelderMead + SolveMethodLeastSquare +) + const epsilon = 1e-4 func normalize(v *mat.VecDense) (*mat.VecDense, float64, float64) { @@ -35,7 +43,7 @@ func normalize2(v *mat.VecDense, vOff, vScale float64) *mat.VecDense { return v } -func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, error) { +func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense, method SolveMethod) ([]float64, error) { M := setupSystemOfEquations(f, latVec, lonVec, heightVec) n := f.Len() weights := mat.NewDiagDense(n, nil) @@ -61,27 +69,29 @@ func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, e 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)) + if method == SolveMethodNelderMead { + 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 } - // 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 @@ -91,7 +101,7 @@ func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, e v0 := 0.0 iterations := 0 maxIterations := 10 - + denominator := mat.NewVecDense(20, nil) for iterations < maxIterations { denominator.SetVec(0, 1.0) for idx := 1; idx < 20; idx++ { @@ -243,9 +253,9 @@ func invertRPCMatrix(At *mat.Dense) (*mat.Dense, error) { if err != nil { // 岭估计方法调整法方程状态，使得矩阵非奇异，最小二乘平差可以收敛 r, c := At.Dims() - log.Infof("cannot inverse matrix(%d*%d): %v", r, c, err) + log.Debugf("cannot inverse matrix(%d*%d): %v", r, c, err) k := 0.0000001 // [0.00000005, 0.000005] - log.Infof("try to adjust matrix with +kI, k=%.8f", k) + log.Debugf("try to adjust matrix with +kI, k=%.8f", k) I := mat.NewDiagDense(r, nil) for i := 0; i < r; i++ { I.SetDiag(i, k) @@ -256,7 +266,7 @@ func invertRPCMatrix(At *mat.Dense) (*mat.Dense, error) { } if err != nil { - log.Infof("cannot inverse matrix: %v, try SVD method", err) + log.Debugf("cannot inverse matrix: %v, try SVD method", err) // 计算矩阵的 SVD 分解 var svd mat.SVD ok := svd.Factorize(At, mat.SVDThin) diff --git a/pkg/utils/tiff.go b/pkg/utils/tiff.go index e0c3de0..76a9bb0 100644 --- a/pkg/utils/tiff.go +++ b/pkg/utils/tiff.go @@ -87,7 +87,7 @@ func SaveBGRToGDALGTiff(bgr gocv.Mat, defer ds.Close() // ds.SetMetadata("NBITS", "16") - setGeoTransform(ds, topLeftX, topLeftY, resolution) + // setGeoTransform(ds, topLeftX, topLeftY, resolution) for b := 0; b < bands; b++ { band := ds.Bands()[b]