diff --git a/cmd/proc.go b/cmd/proc.go index a0c97af..d5b72ef 100644 --- a/cmd/proc.go +++ b/cmd/proc.go @@ -48,8 +48,10 @@ var procCmd = &cobra.Command{ if err := reg.LoadAuxData(); err != nil { logrus.Fatal(err) } + // reg.AuxPrint() + if err := reg.LoadMssRaw(); err != nil { logrus.Fatal(err) } diff --git a/pkg/auxilary/att.go b/pkg/auxilary/att.go new file mode 100644 index 0000000..63aeda5 --- /dev/null +++ b/pkg/auxilary/att.go @@ -0,0 +1,126 @@ +package auxilary + +import ( + "fmt" + "math" + "os" +) + +type Attitudes struct { + Atts []*Attitude +} + +func (atts Attitudes) Save(attFile string) error { + f, err := os.Create(attFile) + if err != nil { + return err + } + defer f.Close() + + for _, att := range atts.Atts { + content := fmt.Sprintf("%.8f %.8f %.8f %.8f %.8f\n", att.UTCTimestampSec, att.Q0, att.Q1, att.Q2, att.Q3) + f.WriteString(content) + } + + return nil +} + +// 四元数球面线性插值 +func (atts Attitudes) Slerp(t float64) *Attitude { + // 取 t 前后的四元数 + var q0, q1 *Attitude + + for i, att := range atts.Atts { + if att.UTCTimestampSec >= t { + if i == 0 { + q0 = att + q1 = atts.Atts[i+1] + } else { + q0 = atts.Atts[i-1] + q1 = att + } + break + } + } + + if q0 == nil || q1 == nil { + return atts.Atts[len(atts.Atts)-1] + } + + var w0, x0, y0, z0, w1, x1, y1, z1 float64 + w0, x0, y0, z0 = q0.Q0, q0.Q1, q0.Q2, q0.Q3 + w1, x1, y1, z1 = q1.Q0, q1.Q1, q1.Q2, q1.Q3 + + // 用点乘计算两个四元数夹角的cos值 + cosOmega := w0*w1 + x0*x1 + y0*y1 + z0*z1 + + // 如果点乘为负，则反转一个四元数以取得短的4D弧 + if cosOmega < 0.0 { + w1 = -w1 + x1 = -x1 + y1 = -y1 + z1 = -z1 + cosOmega = -cosOmega + } + + var k0, k1 float64 + t0, t1 := q0.UTCTimestampSec, q1.UTCTimestampSec + if cosOmega > 0.99999999 { // cos=1的时候就是夹角为0，重合 + k0 = (t1 - t) / (t1 - t0) + k1 = (t - t0) / (t1 - t0) + } else { + // 用三角公式sin^2+cos^2=1计算sin值 + sinOmega := math.Sqrt(1.0 - cosOmega*cosOmega) + // 通过sin和cos计算角度 + omega := math.Atan2(sinOmega, cosOmega) // 计算点(cosOmega,sinOmega)与x轴正向的夹角 + // 计算分母的倒数，这样就只需要一次除法 + oneOverSinOmega := 1.0 / sinOmega + //计算插值变量 + k0 = math.Sin((t1-t)/(t1-t0)*omega) * oneOverSinOmega + k1 = math.Sin((t-t0)/(t1-t0)*omega) * oneOverSinOmega + } + // 插值 + w := w0*k0 + w1*k1 + x := x0*k0 + x1*k1 + y := y0*k0 + y1*k1 + z := z0*k0 + z1*k1 + + return &Attitude{ + UTCTimestampSec: t, + Q0: w, + Q1: x, + Q2: y, + Q3: z, + } +} + +type Attitude struct { + UTCTimestampSec float64 + Q0, Q1, Q2, Q3 float64 +} + +func StoreAtt(aps []*AuxPlatform, attFile string) (*Attitudes, error) { + atts := ExtractAttitude(aps) + return atts, atts.Save(attFile) +} + +func ExtractAttitude(aps []*AuxPlatform) *Attitudes { + var atts Attitudes + var sec, microsec uint32 + for _, ap := range aps { + if ap.UTCTimeSec != sec || ap.Microsecond != microsec { + sec, microsec = ap.UTCTimeSec, ap.Microsecond + att := Attitude{ + UTCTimestampSec: float64(sec) + float64(ReferenceTime2000) + + float64(transfromGPSandAttMicrosec(microsec))/1e6, + Q0: ap.QuatAttstarQ0, + Q1: ap.QuatAttstarQ1, + Q2: ap.QuatAttstarQ2, + Q3: ap.QuatAttstarQ3, + } + atts.Atts = append(atts.Atts, &att) + } + } + + return &atts +} diff --git a/pkg/auxilary/aux.go b/pkg/auxilary/aux.go index 85b727b..08c575e 100644 --- a/pkg/auxilary/aux.go +++ b/pkg/auxilary/aux.go @@ -44,3 +44,12 @@ func ExtractAux(auxfile string) ([]*AuxFrameHead, []*AuxFocalBox, []*AuxPlatform return afh, afb, aps, err } + +// 长光卫星姿态和GPS数据更新频率为 4 次/秒 +func transfromGPSandAttMicrosec(microsec uint32) uint32 { + unit := uint32(250000) + + microsec = (microsec / unit) * unit + + return microsec +} diff --git a/pkg/auxilary/gps.go b/pkg/auxilary/gps.go new file mode 100644 index 0000000..0be4a97 --- /dev/null +++ b/pkg/auxilary/gps.go @@ -0,0 +1,126 @@ +package auxilary + +import ( + "fmt" + "math" + "os" + + "starwiz.cn/sjy01/image-proc/pkg/utils" +) + +type GPSs struct { + GPSs []*GPS +} + +func (g GPSs) Save(gpsFile string) error { + f, err := os.Create(gpsFile) + if err != nil { + return err + } + defer f.Close() + + for _, gps := range g.GPSs { + content := fmt.Sprintf("%.8f %.8f %.8f %.8f %.8f %.8f %.8f\n", + gps.UTCTimestampSec, + gps.X84, gps.Y84, gps.Z84, + gps.Vx84, gps.Vy84, gps.Vz84) + f.WriteString(content) + } + + return nil +} + +// 采用拉格朗日内插方法获得gps位置 +// 输入：t为UTC时间戳，单位为秒 +func (g GPSs) Lagrange(t float64) *GPS { + // 找到扫描行成像时刻前后四个时间点的位置矢量 + var gps_sample []*GPS + var idx int + + if t < g.GPSs[0].UTCTimestampSec { + return g.GPSs[0] + } else if t > g.GPSs[len(g.GPSs)-1].UTCTimestampSec { + return g.GPSs[len(g.GPSs)-1] + } + + for i, g := range g.GPSs { + if g.UTCTimestampSec <= t { + idx = i + } + } + + idx0 := int(math.Max(0, float64(idx-3))) + for i := idx0; i <= idx; i++ { + gps_sample = append(gps_sample, g.GPSs[i]) + } + + idx1 := int(math.Min(float64(len(g.GPSs))-1, float64(idx+1))) + idx2 := int(math.Min(float64(len(g.GPSs))-1, float64(idx+4))) + for i := idx1; i <= idx2; i++ { + gps_sample = append(gps_sample, g.GPSs[i]) + } + + var x, y, z, tt []float64 + for i := 0; i < len(gps_sample); i++ { + tt = append(tt, gps_sample[i].UTCTimestampSec) + x = append(x, gps_sample[i].X84) + y = append(y, gps_sample[i].Y84) + z = append(z, gps_sample[i].Z84) + } + + px := utils.InterpLagrange(tt, x, t) + py := utils.InterpLagrange(tt, y, t) + pz := utils.InterpLagrange(tt, z, t) + + return &GPS{ + UTCTimestampSec: t, + X84: px, + Y84: py, + Z84: pz, + Vx84: g.GPSs[idx].Vx84, + Vy84: g.GPSs[idx].Vy84, + Vz84: g.GPSs[idx].Vz84, + } +} + +type GPS struct { + UTCTimestampSec float64 + X84, Y84, Z84 float64 + Vx84, Vy84, Vz84 float64 +} + +func StoreGPS(aps []*AuxPlatform, gpsFile string) (*GPSs, error) { + gpss := ExtractGPS(aps) + var interGPS GPSs + for _, gps := range gpss.GPSs { + interGPS.GPSs = append(interGPS.GPSs, gps) + t := gps.UTCTimestampSec + 0.098678123 + interGPS.GPSs = append(interGPS.GPSs, gpss.Lagrange(t)) + } + + return gpss, interGPS.Save(gpsFile) +} + +func ExtractGPS(aps []*AuxPlatform) *GPSs { + var gpss GPSs + var sec, microsec uint32 + for _, ap := range aps { + if ap.UTCTimeSec != sec || ap.Microsecond != microsec { + sec, microsec = ap.UTCTimeSec, ap.Microsecond + gps := GPS{ + UTCTimestampSec: float64(sec) + float64(ReferenceTime2000) + + float64(transfromGPSandAttMicrosec(microsec))/1e6, + X84: ap.W84PosX, + Y84: ap.W84PosY, + Z84: ap.W84PosZ, + Vx84: ap.W84VelX, + Vy84: ap.W84VelY, + Vz84: ap.W84VelZ, + } + + gpss.GPSs = append(gpss.GPSs, &gps) + } + } + + return &gpss +} diff --git a/pkg/auxilary/image_time.go b/pkg/auxilary/image_time.go new file mode 100644 index 0000000..26b98aa --- /dev/null +++ b/pkg/auxilary/image_time.go @@ -0,0 +1,62 @@ +package auxilary + +import ( + "fmt" + "os" +) + +// PAN 16 行 / MSS 4 行 图像对应 1 行辅助数据记录，需要单独计算图像行时间 +type ImageTime struct { + auxT []*auxT +} + +type auxT struct { + Tutc float64 + Row int +} + +func NewImageTime() *ImageTime { + return &ImageTime{ + auxT: make([]*auxT, 0), + } +} + +func (it *ImageTime) Extract(aps []*AuxPlatform) { + var sec, microsec uint32 + for i, ap := range aps { + if ap.UTCTimeSec != sec || ap.Microsecond != microsec { + sec, microsec = ap.UTCTimeSec, ap.Microsecond + t := float64(ap.UTCTimeSec) + float64(ReferenceTime2000) + float64(ap.Microsecond)/1e6 + it.auxT = append(it.auxT, &auxT{Tutc: t, Row: i}) + } + } +} + +// Interp 内插出成像时刻 +// 参数: imgrow 图像行号 cross 图像跨度 aps 辅助平台列表 +// 返回值: 成像时刻(UTC seconds) +func (it *ImageTime) Interp(imgrow int, cross int) (float64, float64) { + // 内插出成像时刻 + u := int(imgrow / cross) + var u1, u2 *auxT + for i, a := range it.auxT { + if a.Row > u { + u1 = it.auxT[i-1] + u2 = a + break + } + } + dt := (u2.Tutc - u1.Tutc) / float64(cross*(u2.Row-u1.Row)) + t := u1.Tutc + dt*float64(imgrow-u1.Row*cross) + return t, dt +} + +func (it *ImageTime) Print(n int, cross int) { + f, _ := os.Create("log/image_time.txt") + defer f.Close() + for i := 0; i < n; i++ { + t, dt := it.Interp(i, cross) + s := fmt.Sprintf("%d %f %f\n", i, t, dt) + f.WriteString(s) + } +} diff --git a/pkg/calculator/intersection.go b/pkg/calculator/intersection.go index 088f26d..7fcef1c 100644 --- a/pkg/calculator/intersection.go +++ b/pkg/calculator/intersection.go @@ -15,7 +15,7 @@ type IntersectionPoint struct { H float64 } -func IntersectionAttitude(q Quaternion, satPosECI, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) { +func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) { // alpha := FOV * math.Pi / 180.0 // alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels)) // direction := []float64{0, math.Tan(alpha), -1.3} @@ -33,18 +33,15 @@ func IntersectionAttitude(q Quaternion, satPosECI, satPos84 []float64, satTime t dECI := result.RawVector().Data // -------- 转到ECEF坐标系 -------- - // x, y, z := ECItoECEF(dECI[0], dECI[1], dECI[2], satTime) - // dECEF := []float64{x, y, z} + x, y, z := ECItoECEF(dECI[0], dECI[1], dECI[2], satTime) + dECEF := []float64{x, y, z} // -------- 计算与地球表面的交点 -------- - intersection, err := intersectWithEllipsoid(satPosECI, dECI) + intersection, err := intersectWithEllipsoid(satPos84, dECEF) if err != nil { return IntersectionPoint{}, err } - x, y, z := ECItoECEF(intersection[0], intersection[1], intersection[2], satTime) - intersection = []float64{x, y, z} - lat, lon, h := ECEFGeocentricToGeodetic(intersection[0], intersection[1], intersection[2]) return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil } diff --git a/pkg/calculator/wgs84.go b/pkg/calculator/wgs84.go index dcaa793..a8ee09a 100644 --- a/pkg/calculator/wgs84.go +++ b/pkg/calculator/wgs84.go @@ -3,7 +3,7 @@ package calculator import "math" func WGS84XYZtoLatLngH(X, Y, Z float64) (float64, float64, float64) { - return ECEFToGeodetic(X, Y, Z) + return ECEFGeocentricToGeodetic(X, Y, Z) } // Function to convert ECEF (ITRS) coordinates to geodetic coordinates (latitude, longitude, height) diff --git a/pkg/producer/aux.go b/pkg/producer/aux.go index c6fe683..65056e7 100644 --- a/pkg/producer/aux.go +++ b/pkg/producer/aux.go @@ -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}) diff --git a/pkg/producer/image_registration.go b/pkg/producer/image_registration.go index 08101ef..b88a2a1 100644 --- a/pkg/producer/image_registration.go +++ b/pkg/producer/image_registration.go @@ -11,8 +11,6 @@ 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" "starwiz.cn/sjy01/image-proc/pkg/payload" ) @@ -60,15 +58,12 @@ type Registrator struct { resampleMethod ResampleMethod - auxHeads []*auxilary.AuxFrameHead - auxBoxes []*auxilary.AuxFocalBox - AuxPlatforms []*auxilary.AuxPlatform - w84Positions []r3.Vec - w84PositionX []float64 - w84PositionY []float64 - w84PositionZ []float64 - w84PositionTime []float64 - w84FitPre [3]interp.FittablePredictor + auxHeads []*auxilary.AuxFrameHead + auxBoxes []*auxilary.AuxFocalBox + AuxPlatforms []*auxilary.AuxPlatform + + GPSs *auxilary.GPSs + AttQuaternion *auxilary.Attitudes report Report } diff --git a/pkg/producer/rpc.go b/pkg/producer/rpc.go index 65d322a..fc33920 100644 --- a/pkg/producer/rpc.go +++ b/pkg/producer/rpc.go @@ -207,14 +207,9 @@ func (rpc *RPC) calculateLatLonH(row, col int) GroundPoint { as := rpc.registrator.AuxPlatforms[auxIdx] t := time.Unix(int64(auxilary.ReferenceTime2000)+int64(as.UTCTimeSec), int64(as.Microsecond)*1000).UTC() p84 := []float64{as.W84PosX, as.W84PosY, as.W84PosZ} - pECI := []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, - } Qsat2eci := calculator.Quaternion{W: as.QuatAttstarQ0, X: as.QuatAttstarQ1, Y: as.QuatAttstarQ2, Z: as.QuatAttstarQ3} - groudPoint84, _ := calculator.IntersectionAttitude(Qsat2eci, pECI, p84, t, col) + groudPoint84, _ := calculator.IntersectionAttitude(Qsat2eci, p84, t, col) elv := dem.Dem1KmLT.Elevation(groudPoint84.Lon, groudPoint84.Lat) if elv < 0.0 {