lankun/sjy01-image-proc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
nuknal
2024-06-07 10:51:20 +08:00
parent f7c4237c77
commit cf5012f2a8
8 changed files with 192 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
go.mod
View File

@@ -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

2
go.sum
View File

@@ -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=

1
pkg/calculator/const.go
View File

@@ -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
)

64
pkg/calculator/intersection.go Normal file
View File

@@ -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],
}
}

11
pkg/calculator/j2000.go
View File

@@ -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

72
pkg/calculator/quaternion.go
View File

@@ -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,
})
}

96
pkg/producer/aux.go
View File

@@ -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)

27
pkg/producer/scenes.go
View File

@@ -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)