lankun/sjy01-image-proc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

拟合GPS位置

Browse Source
This commit is contained in:
nuknal
2024-07-03 09:50:14 +08:00
parent d99b8a9740
commit 31887a6bfe
11 changed files with 293 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
pkg/producer/aux.go
View File

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