辅助数据拟合
This commit is contained in:
nuknal
@@ -10,7 +10,6 @@ import (
|
||||
"time"
|
||||
|
||||
log "github.com/sirupsen/logrus"
|
||||
"gonum.org/v1/gonum/spatial/r3"
|
||||
|
||||
"github.com/duke-git/lancet/v2/mathutil"
|
||||
"github.com/paulmach/orb"
|
||||
@@ -21,69 +20,45 @@ import (
|
||||
"starwiz.cn/sjy01/image-proc/pkg/calculator"
|
||||
"starwiz.cn/sjy01/image-proc/pkg/config"
|
||||
"starwiz.cn/sjy01/image-proc/pkg/payload"
|
||||
"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()
|
||||
|
||||
attFile := strings.Replace(r.Params.AuxRawFile, ".AUX", ".att.txt", 1)
|
||||
gpsFile := strings.Replace(r.Params.AuxRawFile, ".AUX", ".gps.txt", 1)
|
||||
r.AttQuaternion, _ = auxilary.StoreAtt(r.AuxPlatforms, attFile)
|
||||
r.GPSs, _ = auxilary.StoreGPS(r.AuxPlatforms, gpsFile)
|
||||
|
||||
imgtime := auxilary.NewImageTime()
|
||||
imgtime.Extract(r.AuxPlatforms)
|
||||
imgtime.Print(100, 16)
|
||||
|
||||
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
|
||||
var fcPos84Interp geojson.FeatureCollection
|
||||
for _, p := range r.AuxPlatforms {
|
||||
lat, lon, _ := calculator.WGS84XYZtoLatLngH(p.W84PosX, p.W84PosY, p.W84PosZ)
|
||||
lat, lon, _ := calculator.ECEFGeocentricToGeodetic(p.W84PosX, p.W84PosY, p.W84PosZ)
|
||||
point := orb.Point{lon, lat}
|
||||
fcPos84.Features = append(fcPos84.Features, geojson.NewFeature(point))
|
||||
|
||||
tp := float64(p.UTCTimeSec) + float64(auxilary.ReferenceTime2000) + float64(p.Microsecond)/1e6
|
||||
interp := r.GPSs.Lagrange(tp)
|
||||
lat, lon, _ = calculator.ECEFGeocentricToGeodetic(interp.X84, interp.Y84, interp.Z84)
|
||||
point = orb.Point{lon, lat}
|
||||
fcPos84Interp.Features = append(fcPos84Interp.Features, geojson.NewFeature(point))
|
||||
}
|
||||
|
||||
data, _ := json.Marshal(fcPos84)
|
||||
f, _ := os.Create(filepath.Join(config.GCONFIG.Log.LogDir, fmt.Sprintf("%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, 2)
|
||||
Y := utils.InterpPolynomial(r.w84PositionTime, r.w84PositionY, tp, 2)
|
||||
Z := utils.InterpPolynomial(r.w84PositionTime, r.w84PositionZ, tp, 2)
|
||||
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(filepath.Join(config.GCONFIG.Log.LogDir, fmt.Sprintf("%s_aux_pos_84_interp.geojson", r.Params.DataId)))
|
||||
defer f.Close()
|
||||
@@ -115,17 +90,7 @@ func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.P
|
||||
endTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(ae.UTCTimeSec), int64(ae.Microsecond)*1000).UTC()
|
||||
|
||||
startPos84 := []float64{as.W84PosX, as.W84PosY, as.W84PosZ}
|
||||
startPosECI := []float64{
|
||||
as.J2000PosX + as.J2000VelX*float64(as.Microsecond)/10e6,
|
||||
as.J2000PosY + as.J2000VelY*float64(as.Microsecond)/10e6,
|
||||
as.J2000PosZ + as.J2000VelZ*float64(as.Microsecond)/10e6,
|
||||
}
|
||||
endPos84 := []float64{ae.W84PosX, ae.W84PosY, ae.W84PosZ}
|
||||
endPosECI := []float64{
|
||||
ae.J2000PosX + ae.J2000VelX*float64(ae.Microsecond)/10e6,
|
||||
ae.J2000PosY + ae.J2000VelY*float64(ae.Microsecond)/10e6,
|
||||
ae.J2000PosZ + ae.J2000VelZ*float64(ae.Microsecond)/10e6,
|
||||
}
|
||||
|
||||
// FIXME: GPS 拟合效果不佳
|
||||
// x0 := float64(r.auxHeads[startPosInAux].TimeSec) + float64(r.auxHeads[startPosInAux].TimeSecFrac)/10e6
|
||||
@@ -146,42 +111,12 @@ func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.P
|
||||
// ------------------ 使用定姿态四元数计算图像边界 ------------------
|
||||
log.Info("using attitude quaternion to calculate image boundary...")
|
||||
Qsat2eci := calculator.Quaternion{W: as.QuatAttstarQ0, X: as.QuatAttstarQ1, Y: as.QuatAttstarQ2, Z: as.QuatAttstarQ3}
|
||||
line0Start, _ := calculator.IntersectionAttitude(Qsat2eci, startPosECI, startPos84, startTime, 0)
|
||||
line0End, _ := calculator.IntersectionAttitude(Qsat2eci, startPosECI, startPos84, startTime, payload.PAN_PIXEL_WIDTH)
|
||||
line0Start, _ := calculator.IntersectionAttitude(Qsat2eci, startPos84, startTime, 0)
|
||||
line0End, _ := calculator.IntersectionAttitude(Qsat2eci, startPos84, startTime, payload.PAN_PIXEL_WIDTH)
|
||||
|
||||
Qsat2eci = calculator.Quaternion{W: ae.QuatAttstarQ0, X: ae.QuatAttstarQ1, Y: ae.QuatAttstarQ2, Z: ae.QuatAttstarQ3}
|
||||
lineNStart, _ := calculator.IntersectionAttitude(Qsat2eci, endPosECI, endPos84, endTime, 0)
|
||||
lineNEnd, _ := calculator.IntersectionAttitude(Qsat2eci, endPosECI, endPos84, endTime, payload.PAN_PIXEL_WIDTH)
|
||||
|
||||
// ------------------ 使用本体和轨道四元数计算图像边界 ECI------------------
|
||||
// log.Info("using orbit and body quaternion to calculate image boundary...")
|
||||
// Qsat2orbit := calculator.Quaternion{X: as.QuatOrbitQ1, Y: as.QuatOrbitQ2, Z: as.QuatOrbitQ3}
|
||||
// Qsat2orbit.W = math.Sqrt(1 - Qsat2orbit.X*Qsat2orbit.X - Qsat2orbit.Y*Qsat2orbit.Y - Qsat2orbit.Z*Qsat2orbit.Z)
|
||||
// Qorbit2eci := calculator.Quaternion{X: as.QuatOrbJQ1, Y: as.QuatOrbJQ2, Z: as.QuatOrbJQ3}
|
||||
// Qorbit2eci.W = math.Sqrt(1 - Qorbit2eci.X*Qorbit2eci.X - Qorbit2eci.Y*Qorbit2eci.Y - Qorbit2eci.Z*Qorbit2eci.Z)
|
||||
// line0Start,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, startPos84, startTime, 0)
|
||||
// line0End,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, startPos84, startTime, payload.PAN_PIXEL_WIDTH)
|
||||
|
||||
// Qsat2orbit = calculator.Quaternion{X: ae.QuatOrbitQ1, Y: ae.QuatOrbitQ2, Z: ae.QuatOrbitQ3}
|
||||
// Qsat2orbit.W = math.Sqrt(1 - Qsat2orbit.X*Qsat2orbit.X - Qsat2orbit.Y*Qsat2orbit.Y - Qsat2orbit.Z*Qsat2orbit.Z)
|
||||
// Qorbit2eci = calculator.Quaternion{X: ae.QuatOrbJQ1, Y: ae.QuatOrbJQ2, Z: ae.QuatOrbJQ3}
|
||||
// Qorbit2eci.W = math.Sqrt(1 - Qorbit2eci.X*Qorbit2eci.X - Qorbit2eci.Y*Qorbit2eci.Y - Qorbit2eci.Z*Qorbit2eci.Z)
|
||||
// lineNStart,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, endPos84, endTime, 0)
|
||||
// lineNEnd,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, endPos84, endTime, payload.PAN_PIXEL_WIDTH)
|
||||
|
||||
// ------------------ 使用本体和轨道四元数计算图像边界 ECEF------------------
|
||||
// log.Info("using orbit and body quaternion to calculate image boundary...")
|
||||
// Qsat2orbit := calculator.Quaternion{X: as.QuatOrbitQ1, Y: as.QuatOrbitQ2, Z: as.QuatOrbitQ3}
|
||||
// Qsat2orbit.W = math.Sqrt(1 - Qsat2orbit.X*Qsat2orbit.X - Qsat2orbit.Y*Qsat2orbit.Y - Qsat2orbit.Z*Qsat2orbit.Z)
|
||||
// vec84 := []float64{as.W84VelX, as.W84VelY, as.W84VelZ}
|
||||
// line0Start, _ := calculator.IntersectionECEF(Qsat2orbit, startPos84, vec84, 0)
|
||||
// line0End, _ := calculator.IntersectionECEF(Qsat2orbit, startPos84, vec84, payload.PAN_PIXEL_WIDTH)
|
||||
|
||||
// Qsat2orbit = calculator.Quaternion{X: ae.QuatOrbitQ1, Y: ae.QuatOrbitQ2, Z: ae.QuatOrbitQ3}
|
||||
// Qsat2orbit.W = math.Sqrt(1 - Qsat2orbit.X*Qsat2orbit.X - Qsat2orbit.Y*Qsat2orbit.Y - Qsat2orbit.Z*Qsat2orbit.Z)
|
||||
// vec84 = []float64{ae.W84VelX, ae.W84VelY, ae.W84VelZ}
|
||||
// lineNStart, _ := calculator.IntersectionECEF(Qsat2orbit, endPos84, vec84, 0)
|
||||
// lineNEnd, _ := calculator.IntersectionECEF(Qsat2orbit, endPos84, vec84, payload.PAN_PIXEL_WIDTH)
|
||||
lineNStart, _ := calculator.IntersectionAttitude(Qsat2eci, endPos84, endTime, 0)
|
||||
lineNEnd, _ := calculator.IntersectionAttitude(Qsat2eci, endPos84, endTime, payload.PAN_PIXEL_WIDTH)
|
||||
|
||||
// ------------------ 计算图像边界距离和分辨率 ------------------
|
||||
W0 := geo.Distance(orb.Point{line0Start.Lon, line0Start.Lat}, orb.Point{line0End.Lon, line0End.Lat})
|
||||
|
||||
Reference in New Issue
Block a user