辅助数据拟合

2024-08-21 15:52:23 +08:00
parent 239d2787e3
commit ca3e91b1d8
10 changed files with 360 additions and 113 deletions
--- a/pkg/auxilary/att.go
+++ 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
+}
--- 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
+}
--- a/pkg/auxilary/gps.go
+++ 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
+}
--- a/pkg/auxilary/image_time.go
+++ 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)
+	}
+}