使用定姿四元数计算图像位置

2024-06-07 10:51:20 +08:00
parent f7c4237c77
commit cf5012f2a8
8 changed files with 192 additions and 82 deletions
--- a/pkg/producer/aux.go
+++ b/pkg/producer/aux.go
@@ -1,10 +1,13 @@
 package producer

 import (
+	"fmt"
 	"math"
 	"time"

+	"github.com/duke-git/lancet/v2/mathutil"
 	"github.com/paulmach/orb"
+	"github.com/paulmach/orb/geo"
 	"github.com/paulmach/orb/planar"
 	"starwiz.cn/sjy01/image-proc/pkg/auxilary"
 	"starwiz.cn/sjy01/image-proc/pkg/calculator"
@@ -32,15 +35,71 @@ func (r *Registrator) SceneImageTime(scene *Scene) (start, center, end time.Time

 // FIXME: 位置像元经纬度计算方法，暂时使用星下点替代
 func (r *Registrator) ScenePosition(scene *Scene) (topLeft, bottomRight orb.Point) {
-	// startPosInAux, endPosInAux := r.SceneInAuxIndex(scene)
-	ap := r.AuxPlatforms[0]
-	// ap1 := r.AuxPlatforms[endPosInAux]
-	lat0, lng0, _ := calculator.WGS84XYZtoLatLngH(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ)
+	// // startPosInAux, endPosInAux := r.SceneInAuxIndex(scene)
+	// ap := r.AuxPlatforms[0]
+	// // ap1 := r.AuxPlatforms[endPosInAux]
+	// lat0, lng0, _ := calculator.WGS84XYZtoLatLngH(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ)

-	lat, lng := calculator.CalculateDestination(lat0, lng0,
-		float64(scene.Width)*scene.Meta.Gsd, float64(-scene.Y)*scene.Meta.Gsd)
-	lat1, lng1 := calculator.CalculateDestination(lat, lng,
-		float64(scene.Width)*scene.Meta.Gsd, float64(-scene.Height)*scene.Meta.Gsd)
+	// lat, lng := calculator.CalculateDestination(lat0, lng0,
+	// 	float64(scene.Width)*scene.Meta.Gsd, float64(-scene.Y)*scene.Meta.Gsd)
+	// lat1, lng1 := calculator.CalculateDestination(lat, lng,
+	// 	float64(scene.Width)*scene.Meta.Gsd, float64(-scene.Height)*scene.Meta.Gsd)
+
+	// poly := orb.Polygon{
+	// 	{
+	// 		{lng, lat},
+	// 		{lng1, lat},
+	// 		{lng1, lat1},
+	// 		{lng, lat1},
+	// 		{lng, lat},
+	// 	},
+	// }
+
+	startPosInAux, endPosInAux := r.SceneInAuxIndex(scene)
+
+	as := r.AuxPlatforms[startPosInAux]
+	startPos := []float64{as.W84PosX, as.W84PosY, as.W84PosZ}
+	startTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(as.UTCTimeSec),
+		int64(as.Microsecond)*1000).UTC()
+	lat0, lng0 := calculator.Intersection(
+		calculator.Quaternion{W: as.QuatAttstarQ0, X: as.QuatAttstarQ1, Y: as.QuatAttstarQ2, Z: as.QuatAttstarQ3},
+		startPos,
+		startTime,
+		0,
+	)
+	lat01, lng01 := calculator.Intersection(
+		calculator.Quaternion{W: as.QuatAttstarQ0, X: as.QuatAttstarQ1, Y: as.QuatAttstarQ2, Z: as.QuatAttstarQ3},
+		startPos,
+		startTime,
+		9344,
+	)
+
+	fmt.Println("distance 0: ", geo.Distance(orb.Point{lng0, lat0}, orb.Point{lng01, lat01}))
+
+	ae := r.AuxPlatforms[endPosInAux]
+	endPos := []float64{ae.W84PosX, ae.W84PosY, ae.W84PosZ}
+	endTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(ae.UTCTimeSec),
+		int64(ae.Microsecond)*1000).UTC()
+	lat2, lng2 := calculator.Intersection(
+		calculator.Quaternion{W: ae.QuatAttstarQ0, X: ae.QuatAttstarQ1, Y: ae.QuatAttstarQ2, Z: ae.QuatAttstarQ3},
+		endPos,
+		endTime,
+		0,
+	)
+	lat3, lng3 := calculator.Intersection(
+		calculator.Quaternion{W: ae.QuatAttstarQ0, X: ae.QuatAttstarQ1, Y: ae.QuatAttstarQ2, Z: ae.QuatAttstarQ3},
+		endPos,
+		endTime,
+		9344,
+	)
+
+	fmt.Println("distance 1: ", geo.Distance(orb.Point{lng2, lat2}, orb.Point{lng3, lat3}))
+
+	// 求外接矩形
+	lat := mathutil.Min(lat0, lat01, lat2, lat3)
+	lng := mathutil.Min(lng0, lng01, lng2, lng3)
+	lat1 := mathutil.Max(lat0, lat01, lat2, lat3)
+	lng1 := mathutil.Max(lng0, lng01, lng2, lng3)

 	poly := orb.Polygon{
 		{
@@ -64,12 +123,21 @@ func (r *Registrator) ScenePosition(scene *Scene) (topLeft, bottomRight orb.Poin
 	scene.Meta.Corners.LowerRight.Latitude = lat1
 	scene.Meta.Corners.LowerRight.Longitude = lng1

-	scene.Meta.SatPosX = ap.WGS84PosX
-	scene.Meta.SatPosY = ap.WGS84PosY
-	scene.Meta.SatPosZ = ap.WGS84PosZ
-	scene.Meta.Yaw = ap.Eular3 * 180 / math.Pi
-	scene.Meta.Pitch = ap.Eular2 * 180 / math.Pi
-	scene.Meta.Roll = ap.Eular1 * 180 / math.Pi
+	scene.Meta.Corners.UpperLeft.Latitude = lat0
+	scene.Meta.Corners.UpperLeft.Longitude = lng0
+	scene.Meta.Corners.UpperRight.Latitude = lat01
+	scene.Meta.Corners.UpperRight.Longitude = lng01
+	scene.Meta.Corners.LowerLeft.Latitude = lat2
+	scene.Meta.Corners.LowerLeft.Longitude = lng2
+	scene.Meta.Corners.LowerRight.Latitude = lat3
+	scene.Meta.Corners.LowerRight.Longitude = lng3
+
+	scene.Meta.SatPosX = ae.WGS84PosX
+	scene.Meta.SatPosY = ae.WGS84PosY
+	scene.Meta.SatPosZ = ae.WGS84PosZ
+	scene.Meta.Yaw = ae.Eular3 * 180 / math.Pi
+	scene.Meta.Pitch = ae.Eular2 * 180 / math.Pi
+	scene.Meta.Roll = ae.Eular1 * 180 / math.Pi

 	// feature := geojson.NewFeature(poly)
 	// fcs.Features = append(fcs.Features, feature)
--- a/pkg/producer/scenes.go
+++ b/pkg/producer/scenes.go
@@ -1,6 +1,7 @@
 package producer

 import (
+	"encoding/json"
 	"fmt"
 	"image"
 	"os"
@@ -8,6 +9,8 @@ import (
 	"strings"

 	"github.com/airbusgeo/godal"
+	"github.com/paulmach/orb"
+	"github.com/paulmach/orb/geojson"
 	log "github.com/sirupsen/logrus"
 	"gocv.io/x/gocv"
 )
@@ -32,11 +35,11 @@ func (s *Scene) Cleanup() {

 // 对 PAN 和 配准后的MSS 在 Y 方向进行分景，景之间有25%的重叠
 // 默认分景大小：
-// MSS  2336 * 2336
-// PAN  9344 * 9344
+// MSS  2336 * 2336 - 1764
+// PAN  9344 * 9344 - 7056
 func (r *Registrator) SubScenes() (panScenes []*Scene, mssScenes []*Scene, err error) {
-	hPAN := (9344 - 2336)
-	hPANOverlap := 2336
+	hPAN := (7056 - 1764)
+	hPANOverlap := 1764
 	panScenesCnt := r.PanHeight / hPAN

 	for i := 0; i < panScenesCnt; i++ {
@@ -99,6 +102,7 @@ func (r *Registrator) SubScenes() (panScenes []*Scene, mssScenes []*Scene, err e
 }

 func (r *Registrator) SaveScenesToTiff(panScenes []*Scene, mssScenes []*Scene) error {
+	var fc geojson.FeatureCollection
 	for i, scene := range panScenes {
 		dir := filepath.Join(r.Params.OutputDir, fmt.Sprintf("%03d", i+1), "PAN")
 		os.MkdirAll(dir, 0755)
@@ -128,8 +132,23 @@ func (r *Registrator) SaveScenesToTiff(panScenes []*Scene, mssScenes []*Scene) e
 			MetaData:    metaFile,
 			BrowserData: jpg,
 		})
+
+		feature := geojson.NewFeature(orb.Polygon{
+			{
+				{scene.Meta.Corners.UpperLeft.Longitude, scene.Meta.Corners.UpperLeft.Latitude},
+				{scene.Meta.Corners.UpperRight.Longitude, scene.Meta.Corners.UpperRight.Latitude},
+				{scene.Meta.Corners.LowerRight.Longitude, scene.Meta.Corners.LowerRight.Latitude},
+				{scene.Meta.Corners.LowerLeft.Longitude, scene.Meta.Corners.LowerLeft.Latitude},
+				{scene.Meta.Corners.UpperLeft.Longitude, scene.Meta.Corners.UpperLeft.Latitude},
+			},
+		})
+		fc.Features = append(fc.Features, feature)
 	}

+	data, _ := json.Marshal(fc)
+	// 输出 GeoJSON 数据
+	fmt.Println(string(data))
+
 	for i, scene := range mssScenes {
 		dir := filepath.Join(r.Params.OutputDir, fmt.Sprintf("%03d", i+1), "MSS")
 		os.MkdirAll(dir, 0755)