From cf5012f2a8fec7d6c38ff584d924d813be5407cc Mon Sep 17 00:00:00 2001 From: nuknal Date: Fri, 7 Jun 2024 10:51:20 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BD=BF=E7=94=A8=E5=AE=9A=E5=A7=BF=E5=9B=9B?= =?UTF-8?q?=E5=85=83=E6=95=B0=E8=AE=A1=E7=AE=97=E5=9B=BE=E5=83=8F=E4=BD=8D?= =?UTF-8?q?=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- go.mod | 1 + go.sum | 2 + pkg/calculator/const.go | 1 + pkg/calculator/intersection.go | 64 +++++++++++++++++++++++ pkg/calculator/j2000.go | 11 ++-- pkg/calculator/quaternion.go | 72 ++++--------------------- pkg/producer/aux.go | 96 +++++++++++++++++++++++++++++----- pkg/producer/scenes.go | 27 ++++++++-- 8 files changed, 192 insertions(+), 82 deletions(-) create mode 100644 pkg/calculator/intersection.go diff --git a/go.mod b/go.mod index 6f7e895..27aa7ac 100644 --- a/go.mod +++ b/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 diff --git a/go.sum b/go.sum index ffa353d..3b7cd19 100644 --- a/go.sum +++ b/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= diff --git a/pkg/calculator/const.go b/pkg/calculator/const.go index 05f92d7..ad64ce4 100644 --- a/pkg/calculator/const.go +++ b/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 ) diff --git a/pkg/calculator/intersection.go b/pkg/calculator/intersection.go new file mode 100644 index 0000000..173822f --- /dev/null +++ b/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], + } +} diff --git a/pkg/calculator/j2000.go b/pkg/calculator/j2000.go index fac7b8c..305b220 100644 --- a/pkg/calculator/j2000.go +++ b/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) - latitude, longitude, height := ECEFToGeodetic(itrsX, itrsY, itrsZ) - return latitude, longitude, height + return itrsX, itrsY, itrsZ } // Function to convert UTC to Julian Date diff --git a/pkg/calculator/quaternion.go b/pkg/calculator/quaternion.go index e19b58b..f8bd73b 100644 --- a/pkg/calculator/quaternion.go +++ b/pkg/calculator/quaternion.go @@ -1,70 +1,20 @@ package calculator import ( - "fmt" - "math" + "gonum.org/v1/gonum/mat" ) -// 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 -func (q1 Quaternion) Mul(q2 Quaternion) Quaternion { - return Quaternion{ - 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, - y: q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x, - z: q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w, - } -} - -// 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) - } +// ToRotationMatrix converts a quaternion to a rotation matrix. +func (q Quaternion) ToRotationMatrix() *mat.Dense { + w, x, y, z := q.W, q.X, q.Y, q.Z + + return mat.NewDense(3, 3, []float64{ + 1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y, + 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, + }) } diff --git a/pkg/producer/aux.go b/pkg/producer/aux.go index 327b52f..1f4385a 100644 --- 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) diff --git a/pkg/producer/scenes.go b/pkg/producer/scenes.go index 35ebf7a..6cdaced 100644 --- 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)