gps image-time attitude 拟合精度

pkg/auxilary/att.go

@@ -18,7 +18,7 @@ func (atts Attitudes) Save(attFile string) error {
 	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)
+		content := fmt.Sprintf("%.6f %.8f %.8f %.8f %.8f\n", att.UTCTimestampSec, att.Q0, att.Q1, att.Q2, att.Q3)
 		f.WriteString(content)
 	}
 
@@ -112,7 +112,7 @@ func ExtractAttitude(aps []*AuxPlatform) *Attitudes {
 			sec, microsec = ap.UTCTimeSec, ap.Microsecond
 			att := Attitude{
 				UTCTimestampSec: float64(sec) + float64(ReferenceTime2000) +
-					float64((microsec))/1e6,
+					float64(transfromGPSandAttMicrosec(microsec))/1e6,
 				Q0: ap.QuatAttstarQ0,
 				Q1: ap.QuatAttstarQ1,
 				Q2: ap.QuatAttstarQ2,

pkg/auxilary/aux.go

@@ -48,6 +48,7 @@ func ExtractAux(auxfile string) ([]*AuxFrameHead, []*AuxFocalBox, []*AuxPlatform
 // 长光卫星姿态和GPS数据更新频率为 4 次/秒
 func transfromGPSandAttMicrosec(microsec uint32) uint32 {
 	unit := uint32(250000)
+	// return microsec
 
 	microsec = (microsec / unit) * unit
 

pkg/auxilary/gps.go

@@ -20,7 +20,7 @@ func (g GPSs) Save(gpsFile string) error {
 	defer f.Close()
 
 	for _, gps := range g.GPSs {
-		content := fmt.Sprintf("%.8f %.8f %.8f %.8f %.8f %.8f %.8f\n",
+		content := fmt.Sprintf("%.6f %.8f %.8f %.8f %.8f %.8f %.8f\n",
 			gps.UTCTimestampSec,
 			gps.X84, gps.Y84, gps.Z84,
 			gps.Vx84, gps.Vy84, gps.Vz84)
@@ -37,9 +37,9 @@ func (g GPSs) Lagrange(t float64) *GPS {
 	var gps_sample []*GPS
 	var idx int
 
-	if t < g.GPSs[0].UTCTimestampSec {
+	if t-g.GPSs[0].UTCTimestampSec < 1e-7 {
 		return g.GPSs[0]
-	} else if t > g.GPSs[len(g.GPSs)-1].UTCTimestampSec {
+	} else if t-g.GPSs[len(g.GPSs)-1].UTCTimestampSec > -1e-7 {
 		return g.GPSs[len(g.GPSs)-1]
 	}
 
@@ -98,7 +98,7 @@ func StoreGPS(aps []*AuxPlatform, gpsFile string) (*GPSs, error) {
 		interGPS.GPSs = append(interGPS.GPSs, gpss.Lagrange(t))
 	}
 
-	return gpss, interGPS.Save(gpsFile)
+	return gpss, gpss.Save(gpsFile)
 }
 
 func ExtractGPS(aps []*AuxPlatform) *GPSs {

pkg/auxilary/image_time.go

@@ -53,12 +53,15 @@ func (it *ImageTime) Interp(imgrow int, cross int) (float64, float64) {
 	return t, dt
 }
 
-func (it *ImageTime) Print(n int, cross int) {
+func (it *ImageTime) Store(imgtimeFile string) error {
-	f, _ := os.Create("log/image_time.txt")
+	f, err := os.Create(imgtimeFile)
+	if err != nil {
+		return err
+	}
 	defer f.Close()
-	for i := 0; i < n; i++ {
+	for _, a := range it.auxT {
-		t, dt := it.Interp(i, cross)
+		s := fmt.Sprintf("%.6f\t%d\n", a.Tutc, a.Row)
-		s := fmt.Sprintf("%d %f %f\n", i, t, dt)
 		f.WriteString(s)
 	}
+	return nil
 }

pkg/calculator/sofa.go

@@ -19,7 +19,7 @@ func ECItoECEF(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64,
 		utc.Day(),
 		utc.Hour(),
 		utc.Minute(),
-		float64(utc.Second()),
+		float64(utc.Second()+utc.Nanosecond()/1e9),
 		&tt1, &tt2)
 
 	var djm0, mjd float64
@@ -59,7 +59,7 @@ func ECEFtoECI(X84, Y84, Z84 float64, utc time.Time) (float64, float64, float64)
 		utc.Day(),
 		utc.Hour(),
 		utc.Minute(),
-		float64(utc.Second()),
+		float64(utc.Second()+utc.Nanosecond()/1e9),
 		&tt1, &tt2)
 
 	var djm0, mjd float64

pkg/producer/aux.go

@@ -27,9 +27,11 @@ func (r *Registrator) LoadAuxData() error {
 
 	attFile := strings.Replace(r.Params.AuxRawFile, ".AUX", ".att.txt", 1)
 	gpsFile := strings.Replace(r.Params.AuxRawFile, ".AUX", ".gps.txt", 1)
+	timeFile := strings.Replace(r.Params.AuxRawFile, ".AUX", ".imgtime.txt", 1)
 	r.AttQuaternion, _ = auxilary.StoreAtt(r.AuxPlatforms, attFile)
 	r.GPSs, _ = auxilary.StoreGPS(r.AuxPlatforms, gpsFile)
 	r.ImageTime = auxilary.NewImageTime(r.AuxPlatforms)
+	r.ImageTime.Store(timeFile)
 
 	return err
 }
@@ -183,7 +185,7 @@ func (r *Registrator) calculateLatLonH(scene *Scene, row, col, H int) calculator
 
 	imgrow := row + scene.Y
 	imgtime, _ := r.ImageTime.Interp(imgrow, cross)
-	nanosecond := (imgtime - math.Floor(imgtime)) * 1000000000
+	nanosecond := (imgtime - math.Floor(imgtime)) * 1e9
 	sattime := time.Unix(int64(imgtime), int64(nanosecond)).UTC()
 
 	// 球面线性插值得到姿态四元数
@@ -195,5 +197,6 @@ func (r *Registrator) calculateLatLonH(scene *Scene, row, col, H int) calculator
 	groudPoint84, _ := calculator.Camera2GroundPoint(cECI,
 		[]float64{p84.X84, p84.Y84, p84.Z84},
 		sattime, ucam, H)
+
 	return groudPoint84
 }

pkg/producer/grid_img.go

@@ -6,15 +6,15 @@ type GridPoint struct {
 
 // 网格点要覆盖边界，甚至大于边界
 func gridImage2(m, n, height, width, k, hmin, hmax int) []*GridPoint {
-	a := int((height) / (m))
+	a := int((height) / (m + 1))
 	var lines []int
-	for i := 0; i <= m; i++ {
+	for i := 1; i <= m; i++ {
 		lines = append(lines, a*i)
 	}
 
-	b := int((width) / (n))
+	b := int((width) / (n + 1))
 	var samples []int
-	for i := 0; i <= n; i++ {
+	for i := 1; i <= n; i++ {
 		samples = append(samples, b*i)
 	}
 
@@ -23,14 +23,14 @@ func gridImage2(m, n, height, width, k, hmin, hmax int) []*GridPoint {
 	dh := (hmax - hmin) / (k)
 
 	var h []int
-	for i := 0; i <= k; i++ {
+	for i := 1; i <= k; i++ {
 		h = append(h, hmin+dh*i)
 	}
 
 	var points []*GridPoint
-	for i := 0; i < len(lines); i++ {
+	for l := 0; l < len(h); l++ {
-		for j := 0; j < len(samples); j++ {
+		for i := 0; i < len(lines); i++ {
-			for l := 0; l < len(h); l++ {
+			for j := 0; j < len(samples); j++ {
 				p := GridPoint{lines[i], samples[j], h[l]}
 				points = append(points, &p)
 			}

pkg/producer/rpc.go

@@ -56,8 +56,8 @@ type RPCModel struct {
 // rational polynomial coeffients
 func NewRPC(r *Registrator, scene *Scene, rpb string) *RPC {
 	rpc := RPC{
-		elevationLayer: 3,
+		elevationLayer: 5,
-		gridsize:       19,
+		gridsize:       20,
 		registrator:    r,
 		scene:          scene,
 		rpb:            rpb,
@@ -97,6 +97,8 @@ func (rpc *RPC) generateVirtualGCP() {
 		rpc.scene.Height, rpc.scene.Width,
 		rpc.elevationLayer, int(rpc.minH), int(rpc.maxH))
 
+	ft, _ := os.Create("log/scene/" + rpc.scene.SceneId + ".time.txt")
+	defer ft.Close()
 	for _, p := range points {
 		p84 := rpc.registrator.calculateLatLonH(rpc.scene, p.Row, p.Col, p.H)
 		rpc.GCPs = append(rpc.GCPs, &GroundPoint{
@@ -106,6 +108,8 @@ func (rpc *RPC) generateVirtualGCP() {
 			Y: float64(p.Row),
 			X: float64(p.Col),
 		})
+		t, dt := rpc.registrator.ImageTime.Interp(p.Row+rpc.scene.Y, 16)
+		ft.WriteString(fmt.Sprintf("%d\t%.9f\t%.9f\n", p.Row, t, dt))
 	}
 
 	name := strings.Replace(rpc.scene.Tiff, ".tiff", ".gcp.geojson", -1)