From 31887a6bfe6f01f3ce6ce88b3c47aae0bbd81996 Mon Sep 17 00:00:00 2001 From: nuknal Date: Wed, 3 Jul 2024 09:50:14 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8B=9F=E5=90=88GPS=E4=BD=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- cmd/proc.go | 2 +- config/config.yaml | 4 +- go.mod | 1 + go.sum | 2 + pkg/producer/aux.go | 63 +++++++++++++++ pkg/producer/geometric.go | 6 ++ pkg/producer/image_registration.go | 14 +++- pkg/rrc/hf_filter.go | 2 - pkg/utils/interp_lagrange.go | 120 +++++++++++++++++++++++++++++ pkg/utils/interp_lagrange_test.go | 29 +++++++ pkg/utils/interp_polynomial.go | 58 ++++++++++++++ 11 files changed, 293 insertions(+), 8 deletions(-) create mode 100644 pkg/producer/geometric.go create mode 100644 pkg/utils/interp_lagrange.go create mode 100644 pkg/utils/interp_lagrange_test.go create mode 100644 pkg/utils/interp_polynomial.go diff --git a/cmd/proc.go b/cmd/proc.go index 5e613ac..ff2c8d1 100644 --- a/cmd/proc.go +++ b/cmd/proc.go @@ -35,7 +35,7 @@ var procCmd = &cobra.Command{ if err := reg.LoadAuxData(); err != nil { logrus.Fatal(err) } - reg.AuxPrint() + // reg.AuxPrint() if err := reg.LoadMssRaw(); err != nil { logrus.Fatal(err) diff --git a/config/config.yaml b/config/config.yaml index 46b451f..4f1f6a2 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -11,8 +11,8 @@ coregistration: fus_band_order: "RGB" radiation: - pan_remove_hf_noise: true - mss_remove_hf_noise: true + pan_remove_hf_noise: false + mss_remove_hf_noise: false hf_radius_ratio: 0.49 min_hist_level: 0.3 max_hist_level: 0.6 diff --git a/go.mod b/go.mod index 75941c0..c112892 100644 --- a/go.mod +++ b/go.mod @@ -18,6 +18,7 @@ require ( git.sr.ht/~sbinet/gg v0.5.0 // indirect github.com/ajstarks/svgo v0.0.0-20211024235047-1546f124cd8b // indirect github.com/campoy/embedmd v1.0.0 // indirect + github.com/chfenger/goNum v0.0.0-20191211064013-a00d841c1e7b // indirect github.com/dustin/go-humanize v1.0.1 // indirect github.com/go-fonts/liberation v0.3.2 // indirect github.com/go-latex/latex v0.0.0-20231108140139-5c1ce85aa4ea // indirect diff --git a/go.sum b/go.sum index 665fe0f..6800939 100644 --- a/go.sum +++ b/go.sum @@ -820,6 +820,8 @@ github.com/census-instrumentation/opencensus-proto v0.4.1/go.mod h1:4T9NM4+4Vw91 github.com/cespare/xxhash v1.1.0/go.mod h1:XrSqR1VqqWfGrhpAt58auRo0WTKS1nRRg3ghfAqPWnc= github.com/cespare/xxhash/v2 v2.1.1/go.mod h1:VGX0DQ3Q6kWi7AoAeZDth3/j3BFtOZR5XLFGgcrjCOs= github.com/cespare/xxhash/v2 v2.2.0/go.mod h1:VGX0DQ3Q6kWi7AoAeZDth3/j3BFtOZR5XLFGgcrjCOs= +github.com/chfenger/goNum v0.0.0-20191211064013-a00d841c1e7b h1:reWb4G/B0Z6zoXHxxJnp+RiWDmQ5TKDhlcVwxTybcqY= +github.com/chfenger/goNum v0.0.0-20191211064013-a00d841c1e7b/go.mod h1:bquY2/umuyWXh59fKDMFlbPf/jdR31Md1PZXbgde9EM= github.com/chzyer/logex v1.1.10/go.mod h1:+Ywpsq7O8HXn0nuIou7OrIPyXbp3wmkHB+jjWRnGsAI= github.com/chzyer/readline v0.0.0-20180603132655-2972be24d48e/go.mod h1:nSuG5e5PlCu98SY8svDHJxuZscDgtXS6KTTbou5AhLI= github.com/chzyer/test v0.0.0-20180213035817-a1ea475d72b1/go.mod h1:Q3SI9o4m/ZMnBNeIyt5eFwwo7qiLfzFZmjNmxjkiQlU= diff --git a/pkg/producer/aux.go b/pkg/producer/aux.go index 6300c3f..f5fab99 100644 --- a/pkg/producer/aux.go +++ b/pkg/producer/aux.go @@ -8,6 +8,7 @@ import ( "time" log "github.com/sirupsen/logrus" + "gonum.org/v1/gonum/spatial/r3" "github.com/duke-git/lancet/v2/mathutil" "github.com/paulmach/orb" @@ -16,14 +17,45 @@ import ( "github.com/paulmach/orb/planar" "starwiz.cn/sjy01/image-proc/pkg/auxilary" "starwiz.cn/sjy01/image-proc/pkg/calculator" + "starwiz.cn/sjy01/image-proc/pkg/utils" ) func (r *Registrator) LoadAuxData() error { var err error r.auxHeads, r.auxBoxes, r.AuxPlatforms, err = auxilary.ExtractAux(r.Params.AuxRawFile) + r.setW84Positions() return err } +// GPS 点按秒更新，从辅助数据按秒提取 +func (r *Registrator) setW84Positions() { + sec := uint32(0) + var x, y, z, t []float64 + for _, p := range r.AuxPlatforms { + if p.UTCTimeSec != sec { + r.w84Positions = append(r.w84Positions, r3.Vec{X: p.W84PosX, Y: p.W84PosY, Z: p.W84PosZ}) + x = append(x, p.W84PosX) + y = append(y, p.W84PosY) + z = append(z, p.W84PosZ) + sec = p.UTCTimeSec + t = append(t, float64(p.UTCTimeSec)) + } + } + + r.w84PositionTime = t + r.w84PositionX = x + r.w84PositionY = y + r.w84PositionZ = z + r.w84FitPre[0] = &utils.PolynomialInterpolator{} + r.w84FitPre[1] = &utils.PolynomialInterpolator{} + r.w84FitPre[2] = &utils.PolynomialInterpolator{} + r.w84FitPre[0].Fit(t, x) + r.w84FitPre[1].Fit(t, y) + r.w84FitPre[2].Fit(t, z) + + log.Println("set w84 positions:", len(r.w84Positions), "points") +} + // 数据校验和测试 func (r *Registrator) AuxPrint() { var fcPos84 geojson.FeatureCollection @@ -36,6 +68,22 @@ func (r *Registrator) AuxPrint() { f, _ := os.Create(fmt.Sprintf("log/%s_aux_pos_84.geojson", r.Params.DataId)) defer f.Close() f.Write(data) + + var fcPos84Interp geojson.FeatureCollection + + for _, p := range r.auxHeads { + tp := float64(p.TimeSec) + float64(p.TimeSecFrac)/10e6 + X := utils.InterpPolynomial(r.w84PositionTime, r.w84PositionX, tp) + Y := utils.InterpPolynomial(r.w84PositionTime, r.w84PositionY, tp) + Z := utils.InterpPolynomial(r.w84PositionTime, r.w84PositionZ, tp) + lat, lon, _ := calculator.WGS84XYZtoLatLngH(X, Y, Z) + point := orb.Point{lon, lat} + fcPos84Interp.Features = append(fcPos84Interp.Features, geojson.NewFeature(point)) + } + data, _ = json.Marshal(fcPos84Interp) + f, _ = os.Create(fmt.Sprintf("log/%s_aux_pos_84_interp.geojson", r.Params.DataId)) + defer f.Close() + f.Write(data) } func (r *Registrator) SceneImageTime(scene *Scene) (start, center, end time.Time) { @@ -65,6 +113,21 @@ func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.P startPos84 := []float64{as.W84PosX, as.W84PosY, as.W84PosZ} endPos84 := []float64{ae.W84PosX, ae.W84PosY, ae.W84PosZ} + // FIXME: GPS 拟合效果不佳 + // x0 := float64(r.auxHeads[startPosInAux].TimeSec) + float64(r.auxHeads[startPosInAux].TimeSecFrac)/10e6 + // x1 := float64(r.auxHeads[endPosInAux].TimeSec) + float64(r.auxHeads[endPosInAux].TimeSecFrac)/10e6 + // startPos84 := []float64{r.w84FitPre[0].Predict(x0), r.w84FitPre[1].Predict(x0), r.w84FitPre[2].Predict(x0)} + // endPos84 := []float64{r.w84FitPre[0].Predict(x1), r.w84FitPre[1].Predict(x1), r.w84FitPre[2].Predict(x1)} + // startPos84 := []float64{ + // utils.InterpPolynomial(r.w84PositionTime, r.w84PositionX, x0), + // utils.InterpPolynomial(r.w84PositionTime, r.w84PositionY, x0), + // utils.InterpPolynomial(r.w84PositionTime, r.w84PositionZ, x0), + // } + // endPos84 := []float64{ + // utils.InterpPolynomial(r.w84PositionTime, r.w84PositionX, x1), + // utils.InterpPolynomial(r.w84PositionTime, r.w84PositionY, x1), + // utils.InterpPolynomial(r.w84PositionTime, r.w84PositionZ, x1), + // } // ------------------ 使用定姿态四元数计算图像边界 ------------------ log.Info("using attitude quaternion to calculate image boundary...") Qsat2eci := calculator.Quaternion{W: as.QuatAttstarQ0, X: as.QuatAttstarQ1, Y: as.QuatAttstarQ2, Z: as.QuatAttstarQ3} diff --git a/pkg/producer/geometric.go b/pkg/producer/geometric.go new file mode 100644 index 0000000..cb8dd38 --- /dev/null +++ b/pkg/producer/geometric.go @@ -0,0 +1,6 @@ +package producer + +// 卫星GPS位置需要经过插值得到准确的图像数据行对应位置 + +func GeoFit() { +} diff --git a/pkg/producer/image_registration.go b/pkg/producer/image_registration.go index 02e3dc6..0b78c16 100644 --- a/pkg/producer/image_registration.go +++ b/pkg/producer/image_registration.go @@ -11,6 +11,8 @@ import ( "github.com/airbusgeo/godal" log "github.com/sirupsen/logrus" "gocv.io/x/gocv" + "gonum.org/v1/gonum/interp" + "gonum.org/v1/gonum/spatial/r3" "starwiz.cn/sjy01/image-proc/pkg/auxilary" ) @@ -53,9 +55,15 @@ type Registrator struct { resampleMethod ResampleMethod - auxHeads []*auxilary.AuxFrameHead - auxBoxes []*auxilary.AuxFocalBox - AuxPlatforms []*auxilary.AuxPlatform + auxHeads []*auxilary.AuxFrameHead + auxBoxes []*auxilary.AuxFocalBox + AuxPlatforms []*auxilary.AuxPlatform + w84Positions []r3.Vec + w84PositionX []float64 + w84PositionY []float64 + w84PositionZ []float64 + w84PositionTime []float64 + w84FitPre [3]interp.FittablePredictor report Report } diff --git a/pkg/rrc/hf_filter.go b/pkg/rrc/hf_filter.go index 9cfbfe9..1ac74a7 100644 --- a/pkg/rrc/hf_filter.go +++ b/pkg/rrc/hf_filter.go @@ -4,7 +4,6 @@ import ( "image" "math" - log "github.com/sirupsen/logrus" "gocv.io/x/gocv" "starwiz.cn/sjy01/image-proc/pkg/config" ) @@ -12,7 +11,6 @@ import ( // filter high frequence noise func HFNoiseFilter(img gocv.Mat, radius float64) gocv.Mat { minGrayVal, maxGrayVal, _, _ := gocv.MinMaxLoc(img) - log.Println("minGrayVal:", minGrayVal, "maxGrayVal:", maxGrayVal) // 将图像转换为32位浮点型 img32f := gocv.NewMat() diff --git a/pkg/utils/interp_lagrange.go b/pkg/utils/interp_lagrange.go new file mode 100644 index 0000000..8e172a6 --- /dev/null +++ b/pkg/utils/interp_lagrange.go @@ -0,0 +1,120 @@ +package utils + +import ( + "fmt" + "sort" + + "github.com/chfenger/goNum" +) + +type LagrangeInterpolator struct { + coeffs []float64 + n int +} + +func (li *LagrangeInterpolator) Fit(x []float64, y []float64) error { + li.n = len(x) - 1 + if li.n < 0 || len(y) != li.n+1 { + return fmt.Errorf("invalid input data") + } + + if li.n > 9 { + li.n = 9 // 限制最大阶数为9 + } + + n := li.n + 1 + + // 初始化系数数组 + li.coeffs = make([]float64, n) + for i := range li.coeffs { + li.coeffs[i] = 0 + } + + // 计算拉格朗日插值多项式的系数 + for i := 0; i < n; i++ { + li_coeff := make([]float64, n) + li_coeff[0] = 1 + for j := 0; j < n; j++ { + if i != j { + for k := n - 1; k >= 0; k-- { + li_coeff[k] *= -x[j] + if k > 0 { + li_coeff[k] += li_coeff[k-1] + } + } + for k := 0; k < n; k++ { + li_coeff[k] /= (x[i] - x[j]) + } + } + } + for k := 0; k < n; k++ { + li.coeffs[k] += y[i] * li_coeff[k] + } + } + + return nil +} + +func (li LagrangeInterpolator) Predict(x float64) float64 { + n := len(li.coeffs) + y := 0.0 + for i := 0; i < n; i++ { + term := li.coeffs[i] + for j := 0; j < i; j++ { + term *= x + } + y += term + } + return y +} + +func (li LagrangeInterpolator) N() int { + return li.n +} + +// InterpLagrange 利用拉格朗日插值法计算函数值 +// 尽量9阶采用内插值 +const STEP_N = 7 + +func InterpLagrange(x []float64, y []float64, xq float64) float64 { + if len(x) != len(y) { + return 0.0 + } + + // 限制阶数为9 + var data []float64 + start, end := FindClosestSubset(x, xq, STEP_N) + for i := start; i <= end; i++ { + data = append(data, x[i]) + data = append(data, y[i]) + } + + A := goNum.NewMatrix(len(data)/2, 2, data) + yq, _ := goNum.InterpLagrange(A, xq) + return yq +} + +// FindClosestSubset 找到包含xq的最近的n个元素的子数组 +func FindClosestSubset(x []float64, xq float64, n int) (int, int) { + if len(x) <= n { + return 0, len(x) - 1 // 如果元素数量少于等于n，直接返回整个数组 + } + + // 找到xq在数组中的插入点 + idx := sort.Search(len(x), func(i int) bool { return x[i] >= xq }) + + // 计算子数组的起始和结束位置 + start := idx - n/2 // 尽量让xq在中间，4是因为9个元素的中间位置是4 + end := idx + n/2 + + // 调整边界 + if start < 0 { + start = 0 + end = n + } else if end >= n { + end = n - 1 + start = end - n + 1 + } + + return start, end +} diff --git a/pkg/utils/interp_lagrange_test.go b/pkg/utils/interp_lagrange_test.go new file mode 100644 index 0000000..e35e720 --- /dev/null +++ b/pkg/utils/interp_lagrange_test.go @@ -0,0 +1,29 @@ +package utils + +import ( + "fmt" + "testing" +) + +func TestInterpLagrange(t *testing.T) { + x := []float64{0, 1, 2, 3, 4} + y := []float64{0, 1, 4, 9, 16} + + interp := &LagrangeInterpolator{} + if err := interp.Fit(x, y); err != nil { + t.Error(err) + } + + fmt.Println("x = 2.5, y =", interp.Predict(2.5)) + fmt.Println("x = 5.5, y =", interp.Predict(5.5)) + fmt.Println("x = 2, y =", interp.Predict(2.0)) + + p := &PolynomialInterpolator{} + if err := p.Fit(x, y); err != nil { + t.Error(err) + } + + fmt.Println("x = 2.5, y =", p.Predict(2.5)) + fmt.Println("x = 5.5, y =", p.Predict(5.5)) + fmt.Println("x = 2, y =", p.Predict(2.0)) +} diff --git a/pkg/utils/interp_polynomial.go b/pkg/utils/interp_polynomial.go new file mode 100644 index 0000000..1c4878b --- /dev/null +++ b/pkg/utils/interp_polynomial.go @@ -0,0 +1,58 @@ +package utils + +import ( + "math" + + "gonum.org/v1/gonum/mat" +) + +type PolynomialInterpolator struct { + Degree int + Coeffs []float64 +} + +func (p *PolynomialInterpolator) Fit(x, y []float64) error { + if p.Degree == 0 { + p.Degree = len(x) - 1 + } + + degree := p.Degree + n := len(x) + // Create the Vandermonde matrix + vander := mat.NewDense(n, degree+1, nil) + for i := 0; i < n; i++ { + for j := 0; j <= degree; j++ { + vander.Set(i, j, math.Pow(x[i], float64(j))) + } + } + + // Create the right-hand side vector + yVec := mat.NewVecDense(n, y) + + // Solve the least squares problem + var qr mat.QR + qr.Factorize(vander) + coeffs := mat.NewDense(degree+1, 1, nil) + err := qr.SolveTo(coeffs, false, yVec) + p.Coeffs = coeffs.RawMatrix().Data + return err +} + +func (p PolynomialInterpolator) Predict(x float64) float64 { + var y float64 + for i, coeff := range p.Coeffs { + y += coeff * math.Pow(x, float64(i)) + } + return y +} + +func InterpPolynomial(x []float64, y []float64, xq float64) float64 { + if len(x) != len(y) { + return 0.0 + } + + start, end := FindClosestSubset(x, xq, 4) + interp := &PolynomialInterpolator{Degree: 3} + interp.Fit(x[start:end+1], y[start:end+1]) + return interp.Predict(xq) +}