253 lines
11 KiB
Go
253 lines
11 KiB
Go
package producer
|
|
|
|
import (
|
|
"encoding/json"
|
|
"fmt"
|
|
"math"
|
|
"os"
|
|
"time"
|
|
|
|
log "github.com/sirupsen/logrus"
|
|
"gonum.org/v1/gonum/spatial/r3"
|
|
|
|
"github.com/duke-git/lancet/v2/mathutil"
|
|
"github.com/paulmach/orb"
|
|
"github.com/paulmach/orb/geo"
|
|
"github.com/paulmach/orb/geojson"
|
|
"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
|
|
for _, p := range r.AuxPlatforms {
|
|
lat, lon, _ := calculator.WGS84XYZtoLatLngH(p.W84PosX, p.W84PosY, p.W84PosZ)
|
|
point := orb.Point{lon, lat}
|
|
fcPos84.Features = append(fcPos84.Features, geojson.NewFeature(point))
|
|
}
|
|
data, _ := json.Marshal(fcPos84)
|
|
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, 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(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) {
|
|
startPosInAux, endPosInAux := r.SceneInAuxIndex(scene)
|
|
centerPosInAux := (startPosInAux + endPosInAux) / 2
|
|
|
|
start = time.Unix(int64(auxilary.ReferenceTime2000)+int64(r.AuxPlatforms[startPosInAux].UTCTimeSec),
|
|
int64(r.AuxPlatforms[startPosInAux].Microsecond)*1000)
|
|
center = time.Unix(int64(auxilary.ReferenceTime2000)+int64(r.AuxPlatforms[centerPosInAux].UTCTimeSec),
|
|
int64(r.AuxPlatforms[centerPosInAux].Microsecond)*1000)
|
|
end = time.Unix(int64(auxilary.ReferenceTime2000)+int64(r.AuxPlatforms[endPosInAux].UTCTimeSec),
|
|
int64(r.AuxPlatforms[endPosInAux].Microsecond)*1000)
|
|
|
|
return
|
|
}
|
|
|
|
// FIXME: This function is not accurate enough.
|
|
func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.Point) {
|
|
startPosInAux, endPosInAux := r.SceneInAuxIndex(scene)
|
|
|
|
as := r.AuxPlatforms[startPosInAux]
|
|
ae := r.AuxPlatforms[endPosInAux]
|
|
|
|
startTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(as.UTCTimeSec), int64(as.Microsecond)*1000).UTC()
|
|
endTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(ae.UTCTimeSec), int64(ae.Microsecond)*1000).UTC()
|
|
|
|
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)}
|
|
// stepN := 2
|
|
// startPos84 = []float64{
|
|
// utils.InterpPolynomial(r.w84PositionTime, r.w84PositionX, x0, stepN),
|
|
// utils.InterpPolynomial(r.w84PositionTime, r.w84PositionY, x0, stepN),
|
|
// utils.InterpPolynomial(r.w84PositionTime, r.w84PositionZ, x0, stepN),
|
|
// }
|
|
// endPos84 = []float64{
|
|
// utils.InterpPolynomial(r.w84PositionTime, r.w84PositionX, x1, stepN),
|
|
// utils.InterpPolynomial(r.w84PositionTime, r.w84PositionY, x1, stepN),
|
|
// utils.InterpPolynomial(r.w84PositionTime, r.w84PositionZ, x1, stepN),
|
|
// }
|
|
// ------------------ 使用定姿态四元数计算图像边界 ------------------
|
|
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, startPos84, startTime, 0)
|
|
line0End, _ := calculator.IntersectionAttitude(Qsat2eci, startPos84, startTime, 9344)
|
|
|
|
Qsat2eci = calculator.Quaternion{W: ae.QuatAttstarQ0, X: ae.QuatAttstarQ1, Y: ae.QuatAttstarQ2, Z: ae.QuatAttstarQ3}
|
|
lineNStart, _ := calculator.IntersectionAttitude(Qsat2eci, endPos84, endTime, 0)
|
|
lineNEnd, _ := calculator.IntersectionAttitude(Qsat2eci, endPos84, endTime, 9344)
|
|
|
|
// ------------------ 使用本体和轨道四元数计算图像边界 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, 9344)
|
|
|
|
// 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, 9344)
|
|
|
|
// ------------------ 使用本体和轨道四元数计算图像边界 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, 9344)
|
|
|
|
// 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, 9344)
|
|
|
|
// ------------------ 计算图像边界距离和分辨率 ------------------
|
|
W0 := geo.Distance(orb.Point{line0Start.Lon, line0Start.Lat}, orb.Point{line0End.Lon, line0End.Lat})
|
|
// WN := geo.Distance(orb.Point{lineNStart.Lon, lineNStart.Lat}, orb.Point{lineNEnd.Lon, lineNEnd.Lat})
|
|
H0 := geo.Distance(orb.Point{line0Start.Lon, line0Start.Lat}, orb.Point{lineNStart.Lon, lineNStart.Lat})
|
|
// HN := geo.Distance(orb.Point{line0End.Lon, line0End.Lat}, orb.Point{lineNEnd.Lon, lineNEnd.Lat})
|
|
xResolution := W0 / float64(scene.Width)
|
|
yResolution := H0 / float64(scene.Height)
|
|
scene.Meta.Gsd = math.Min(xResolution, yResolution)
|
|
|
|
// FIXME: 临时设置分辨率
|
|
if scene.Meta.Gsd < 2 {
|
|
scene.Meta.Gsd = 1.3
|
|
} else {
|
|
scene.Meta.Gsd = 5.2
|
|
}
|
|
log.Debug("resolution x: ", xResolution)
|
|
log.Debug("resolution y: ", yResolution)
|
|
|
|
// 求外接矩形
|
|
latMin := mathutil.Min(line0Start.Lat, line0End.Lat, lineNStart.Lat, lineNEnd.Lat)
|
|
lngMin := mathutil.Min(line0Start.Lon, line0End.Lon, lineNStart.Lon, lineNEnd.Lon)
|
|
latMax := mathutil.Max(line0Start.Lat, line0End.Lat, lineNStart.Lat, lineNEnd.Lat)
|
|
lngMax := mathutil.Max(line0Start.Lon, line0End.Lon, lineNStart.Lon, lineNEnd.Lon)
|
|
|
|
poly := orb.Polygon{
|
|
{
|
|
{lngMin, latMin},
|
|
{lngMax, latMin},
|
|
{lngMax, latMax},
|
|
{lngMin, latMax},
|
|
{lngMin, latMin},
|
|
},
|
|
}
|
|
|
|
centroid, _ := planar.CentroidArea(poly)
|
|
scene.Meta.CentreLocation.Latitude = centroid.Y()
|
|
scene.Meta.CentreLocation.Longitude = centroid.X()
|
|
|
|
// 暂定存储四角点
|
|
scene.Meta.Corners.UpperLeft.Latitude = line0Start.Lat
|
|
scene.Meta.Corners.UpperLeft.Longitude = line0Start.Lon
|
|
scene.Meta.Corners.UpperRight.Latitude = line0End.Lat
|
|
scene.Meta.Corners.UpperRight.Longitude = line0End.Lon
|
|
scene.Meta.Corners.LowerLeft.Latitude = lineNStart.Lat
|
|
scene.Meta.Corners.LowerLeft.Longitude = lineNStart.Lon
|
|
scene.Meta.Corners.LowerRight.Latitude = lineNEnd.Lat
|
|
scene.Meta.Corners.LowerRight.Longitude = lineNEnd.Lon
|
|
|
|
scene.Meta.SatPosX = startPos84[0]
|
|
scene.Meta.SatPosY = startPos84[1]
|
|
scene.Meta.SatPosZ = startPos84[2]
|
|
scene.Meta.Yaw = ae.Eular3 * 180 / math.Pi
|
|
scene.Meta.Pitch = ae.Eular2 * 180 / math.Pi
|
|
scene.Meta.Roll = ae.Eular1 * 180 / math.Pi
|
|
|
|
return
|
|
}
|
|
|
|
func (r *Registrator) SceneInAuxIndex(scene *Scene) (int, int) {
|
|
var auxForImageRow int
|
|
switch scene.Type {
|
|
case "MSS":
|
|
auxForImageRow = 4
|
|
case "PAN":
|
|
auxForImageRow = 16
|
|
case "FUS":
|
|
auxForImageRow = 16
|
|
}
|
|
|
|
startPosInAux := scene.Y / auxForImageRow
|
|
if startPosInAux >= len(r.AuxPlatforms) {
|
|
startPosInAux = len(r.AuxPlatforms) - 1
|
|
}
|
|
|
|
endPosInAux := (scene.Y + scene.Height) / auxForImageRow
|
|
if endPosInAux >= len(r.AuxPlatforms) {
|
|
endPosInAux = len(r.AuxPlatforms) - 1
|
|
}
|
|
|
|
return startPosInAux, endPosInAux
|
|
}
|