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

go.mod

@@ -9,6 +9,7 @@ require (
 )
 
 require (
+	github.com/duke-git/lancet/v2 v2.3.1 // indirect
 	github.com/fsnotify/fsnotify v1.7.0 // indirect
 	github.com/gogo/protobuf v1.3.2 // indirect
 	github.com/hashicorp/hcl v1.0.0 // indirect

go.sum

@@ -844,6 +844,8 @@ github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c
 github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=
 github.com/davecgh/go-spew v1.1.2-0.20180830191138-d8f796af33cc h1:U9qPSI2PIWSS1VwoXQT9A3Wy9MM3WgvqSxFWenqJduM=
 github.com/docopt/docopt-go v0.0.0-20180111231733-ee0de3bc6815/go.mod h1:WwZ+bS3ebgob9U8Nd0kOddGdZWjyMGR8Wziv+TBNwSE=
+github.com/duke-git/lancet/v2 v2.3.1 h1:cYZHQp57CZKP41EFkV/7TGbUrmhjaPMI5vi3Q+9KJNo=
+github.com/duke-git/lancet/v2 v2.3.1/go.mod h1:zGa2R4xswg6EG9I6WnyubDbFO/+A/RROxIbXcwryTsc=
 github.com/dustin/go-humanize v1.0.0/go.mod h1:HtrtbFcZ19U5GC7JDqmcUSB87Iq5E25KnS6fMYU6eOk=
 github.com/dustin/go-humanize v1.0.1/go.mod h1:Mu1zIs6XwVuF/gI1OepvI0qD18qycQx+mFykh5fBlto=
 github.com/envoyproxy/go-control-plane v0.9.0/go.mod h1:YTl/9mNaCwkRvm6d1a2C3ymFceY/DCBVvsKhRF0iEA4=

pkg/calculator/const.go

@@ -5,6 +5,7 @@ const (
 	EarthRadius = 6378137.0         // 地球半径，单位米
 	a           = 6378137.0         // semi-major axis in meters
 	f           = 1 / 298.257223563 // flattening
+	b           = a * (1 - f)       // 短半轴
 	e2          = 2*f - f*f         // square of eccentricity
 	J2000Epoch  = 2451545.0         // Julian date of J2000 epoch
 )

pkg/calculator/intersection.go

@@ -0,0 +1,64 @@
+package calculator
+
+import (
+	"math"
+	"time"
+
+	"gonum.org/v1/gonum/mat"
+)
+
+// 常量
+const (
+	focal   = 1.3  // 焦距, m
+	FOV     = 1.7  // 视场角,degree
+	nPixels = 9344 // 像素数
+)
+
+func Intersection(q Quaternion, satPos []float64, satTime time.Time, ucam int) (float64, float64) {
+	alpha := FOV * math.Pi / 180.0
+	alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(nPixels))
+	direction := []float64{0, math.Tan(alpha), -1.3}
+
+	Ratt := q.ToRotationMatrix()
+	RattT := &mat.Dense{}
+	RattT.Inverse(Ratt)
+
+	v := mat.NewVecDense(3, direction)
+	var result mat.VecDense
+	result.MulVec(Ratt, v)
+	eciDirection := result.RawVector().Data
+
+	// intersection := intersectWithEllipsoid(satPos, eciDirection)
+	// lat, lon, _ := J2000ToWGS84(intersection[0], intersection[1], intersection[2], satTime)
+
+	x, y, z := ECItoECEF(eciDirection[0], eciDirection[1], eciDirection[2], satTime)
+	ecefDirection := []float64{x, y, z}
+	intersection := intersectWithEllipsoid(satPos, ecefDirection)
+	lat, lon, _ := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
+
+	return lat, lon
+
+}
+
+// 计算与椭球表面的交点
+func intersectWithEllipsoid(p0, d []float64) []float64 {
+	a2 := a * a
+	b2 := b * b
+
+	A := d[0]*d[0]/a2 + d[1]*d[1]/a2 + d[2]*d[2]/b2
+	B := 2 * (p0[0]*d[0]/a2 + p0[1]*d[1]/a2 + p0[2]*d[2]/b2)
+	C := p0[0]*p0[0]/a2 + p0[1]*p0[1]/a2 + p0[2]*p0[2]/b2 - 1
+
+	delta := B*B - 4*A*C
+	if delta < 0 {
+		return nil // No intersection
+	}
+	t1 := (-B + math.Sqrt(delta)) / (2 * A)
+	t2 := (-B - math.Sqrt(delta)) / (2 * A)
+	t := math.Max(t1, t2)
+	return []float64{
+		p0[0] + t*d[0],
+		p0[1] + t*d[1],
+		p0[2] + t*d[2],
+	}
+}

pkg/calculator/j2000.go

@@ -7,6 +7,13 @@ import (
 
 // Function to convert current J2000 position to WGS84
 func J2000ToWGS84(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
+	itrsX, itrsY, itrsZ := ECItoECEF(j2000X, j2000Y, j2000Z, utc)
+	// Convert ITRS to geodetic coordinates (WGS84)
+	latitude, longitude, height := ECEFToGeodetic(itrsX, itrsY, itrsZ)
+	return latitude, longitude, height
+}
+
+func ECItoECEF(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float64, float64) {
 	julianDate := UTCToJulianDate(utc)
 	gast := CalculateGAST(julianDate, utc)
 
@@ -18,9 +25,7 @@ func J2000ToWGS84(j2000X, j2000Y, j2000Z float64, utc time.Time) (float64, float
 	itrsY := rotationMatrix[1][0]*j2000X + rotationMatrix[1][1]*j2000Y + rotationMatrix[1][2]*j2000Z
 	itrsZ := rotationMatrix[2][0]*j2000X + rotationMatrix[2][1]*j2000Y + rotationMatrix[2][2]*j2000Z
 
-	// Convert ITRS to geodetic coordinates (WGS84)
+	return itrsX, itrsY, itrsZ
-	latitude, longitude, height := ECEFToGeodetic(itrsX, itrsY, itrsZ)
-	return latitude, longitude, height
 }
 
 // Function to convert UTC to Julian Date

pkg/calculator/quaternion.go

@@ -1,70 +1,20 @@
 package calculator
 
 import (
-	"fmt"
+	"gonum.org/v1/gonum/mat"
-	"math"
 )
 
-// Quaternion represents a quaternion with scalar (w) and vector (x, y, z) parts
 type Quaternion struct {
-	w, x, y, z float64
+	W, X, Y, Z float64
 }
 
-// Quaternion multiplication
+// ToRotationMatrix converts a quaternion to a rotation matrix.
-func (q1 Quaternion) Mul(q2 Quaternion) Quaternion {
+func (q Quaternion) ToRotationMatrix() *mat.Dense {
-	return Quaternion{
+	w, x, y, z := q.W, q.X, q.Y, q.Z
-		w: q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
+
-		x: q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y,
+	return mat.NewDense(3, 3, []float64{
-		y: q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x,
+		1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y,
-		z: q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w,
+		2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x,
-	}
+		2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y,
-}
+	})
-
-// Quaternion conjugate
-func (q Quaternion) Conjugate() Quaternion {
-	return Quaternion{w: q.w, x: -q.x, y: -q.y, z: -q.z}
-}
-
-// Rotate vector by quaternion
-func (q Quaternion) Rotate(v [3]float64) [3]float64 {
-	qv := Quaternion{w: 0, x: v[0], y: v[1], z: v[2]}
-	qConj := q.Conjugate()
-	qvRotated := q.Mul(qv).Mul(qConj)
-	return [3]float64{qvRotated.x, qvRotated.y, qvRotated.z}
-}
-
-func main() {
-	// 示例数据
-	qBI := Quaternion{w: 1, x: 0, y: 0, z: 0} // 本体相对惯性系四元数
-	posJ2000 := [3]float64{7000, 0, 0}        // J2000位置
-	// velJ2000 := [3]float64{0, 7.5, 0}         // J2000速度
-
-	// 相机参数
-	const numPixels = 9520
-	const fov = 10.0 * math.Pi / 180 // 假设视场角为10度
-
-	// 逐像素计算地面交点
-	for i := 0; i < numPixels; i++ {
-		// 计算像素点相对光轴的偏角
-		pixelOffset := (float64(i) - float64(numPixels)/2) / float64(numPixels)
-		angle := pixelOffset * fov
-
-		// 假设光轴在本体坐标系中指向-z方向，计算视线方向
-		dBody := [3]float64{-math.Sin(angle), 0, -math.Cos(angle)}
-
-		// 转换到惯性系
-		dInertial := qBI.Rotate(dBody)
-
-		// 计算地面交点（假设dInertial已经标准化）
-		k := -posJ2000[2] / dInertial[2] // 简化的交点计算
-		groundPoint := [3]float64{
-			posJ2000[0] + k*dInertial[0],
-			posJ2000[1] + k*dInertial[1],
-			posJ2000[2] + k*dInertial[2],
-		}
-
-		// 转换到地理坐标
-		lat, lon, _ := ECEFToGeodetic(groundPoint[0], groundPoint[1], groundPoint[2])
-		fmt.Printf("Pixel %d: Latitude: %f, Longitude: %f\n", i, lat, lon)
-	}
 }

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)
+	// // startPosInAux, endPosInAux := r.SceneInAuxIndex(scene)
-	ap := r.AuxPlatforms[0]
+	// ap := r.AuxPlatforms[0]
-	// ap1 := r.AuxPlatforms[endPosInAux]
+	// // ap1 := r.AuxPlatforms[endPosInAux]
-	lat0, lng0, _ := calculator.WGS84XYZtoLatLngH(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ)
+	// lat0, lng0, _ := calculator.WGS84XYZtoLatLngH(ap.WGS84PosX, ap.WGS84PosY, ap.WGS84PosZ)
 
-	lat, lng := calculator.CalculateDestination(lat0, lng0,
+	// lat, lng := calculator.CalculateDestination(lat0, lng0,
-		float64(scene.Width)*scene.Meta.Gsd, float64(-scene.Y)*scene.Meta.Gsd)
+	// 	float64(scene.Width)*scene.Meta.Gsd, float64(-scene.Y)*scene.Meta.Gsd)
-	lat1, lng1 := calculator.CalculateDestination(lat, lng,
+	// lat1, lng1 := calculator.CalculateDestination(lat, lng,
-		float64(scene.Width)*scene.Meta.Gsd, float64(-scene.Height)*scene.Meta.Gsd)
+	// 	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.Corners.UpperLeft.Latitude = lat0
-	scene.Meta.SatPosY = ap.WGS84PosY
+	scene.Meta.Corners.UpperLeft.Longitude = lng0
-	scene.Meta.SatPosZ = ap.WGS84PosZ
+	scene.Meta.Corners.UpperRight.Latitude = lat01
-	scene.Meta.Yaw = ap.Eular3 * 180 / math.Pi
+	scene.Meta.Corners.UpperRight.Longitude = lng01
-	scene.Meta.Pitch = ap.Eular2 * 180 / math.Pi
+	scene.Meta.Corners.LowerLeft.Latitude = lat2
-	scene.Meta.Roll = ap.Eular1 * 180 / math.Pi
+	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)

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
+// MSS  2336 * 2336 - 1764
-// PAN  9344 * 9344
+// PAN  9344 * 9344 - 7056
 func (r *Registrator) SubScenes() (panScenes []*Scene, mssScenes []*Scene, err error) {
-	hPAN := (9344 - 2336)
+	hPAN := (7056 - 1764)
-	hPANOverlap := 2336
+	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)