拟合GPS位置

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)

config/config.yaml

@@ -11,8 +11,8 @@ coregistration:
   fus_band_order: "RGB"
 
 radiation:
-  pan_remove_hf_noise: true
+  pan_remove_hf_noise: false
-  mss_remove_hf_noise: true
+  mss_remove_hf_noise: false
   hf_radius_ratio: 0.49
   min_hist_level: 0.3
   max_hist_level: 0.6

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

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=

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}

pkg/producer/geometric.go

@@ -0,0 +1,6 @@
+package producer
+
+// 卫星GPS位置需要经过插值得到准确的图像数据行对应位置
+
+func GeoFit() {
+}

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"
 )
 
@@ -56,6 +58,12 @@ type Registrator struct {
 	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
 }

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()

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
+}

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))
+}

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)
+}