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-13 15:07:15 +08:00
parent ee427949e3
commit cd9a534458
8 changed files with 118 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
Dockerfile.build
View File

@@ -16,8 +16,7 @@ ENV GOROOT="/opt/go" \
RUN cd /opt && git clone -b v0.36.1 https://github.com/hybridgroup/gocv.git && cd gocv && \
make install
WORKDIR /src
COPY . .
# WORKDIR /src
# COPY . .
# RUN cd /sjy01/image-proc && go mod download && make linux
# CMD ["/src/build/build.sh"]
RUN cd /src && make linux
CMD ["/bin/bash", "/src/build/build.sh"]

3
build.sh Executable file
View File

@@ -0,0 +1,3 @@
#!/bin/sh
docker build -t nuknal/gdal38-cv49-builder -f Dockerfile.build .
docker run -v .:/src nuknal/gdal38-cv49-builder sh -c "cd /src && make linux"

0
build/.gitkeep Normal file
View File

3
build/build-in-docker.sh
View File

@@ -1,3 +0,0 @@
# !/bin/bash
cd /src && go mod download && make linux

31
pkg/calculator/intersection.go
View File

@@ -1,9 +1,11 @@
package calculator
import (
"fmt"
"math"
"time"
"github.com/sirupsen/logrus"
"gonum.org/v1/gonum/mat"
)
@@ -13,7 +15,7 @@ type IntersectionPoint struct {
H float64
}
func Intersection(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint {
func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) {
// alpha := FOV * math.Pi / 180.0
// alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
// direction := []float64{0, math.Tan(alpha), -1.3}
@@ -35,12 +37,16 @@ func Intersection(q Quaternion, satPos84 []float64, satTime time.Time, ucam int)
dECEF := []float64{x, y, z}
// -------- 计算与地球表面的交点 --------
intersection := intersectWithEllipsoid(satPos84, dECEF)
lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
return IntersectionPoint{Lat: lat, Lon: lon, H: h}
intersection, err := intersectWithEllipsoid(satPos84, dECEF)
if err != nil {
return IntersectionPoint{}, err
}
func Intersection2(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint {
lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil
}
func IntersectionECI(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) {
alpha := FOV * math.Pi / 180.0
alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
direction := []float64{0, math.Tan(alpha), -1.3} // 卫星（相机）坐标系下CCD成像方向向量
@@ -67,14 +73,18 @@ func Intersection2(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTim
dECEF := []float64{x, y, z}
// -------- 计算交点 --------}
intersection := intersectWithEllipsoid(satPos84, dECEF)
intersection, err := intersectWithEllipsoid(satPos84, dECEF)
if err != nil {
return IntersectionPoint{}, err
}
lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
return IntersectionPoint{Lat: lat, Lon: lon, H: h}
return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil
}
// 计算与椭球表面的交点
func intersectWithEllipsoid(p0, d []float64) []float64 {
func intersectWithEllipsoid(p0, d []float64) ([]float64, error) {
a2 := a * a
b2 := b * b
@@ -84,7 +94,8 @@ func intersectWithEllipsoid(p0, d []float64) []float64 {
delta := B*B - 4*A*C
if delta < 0 {
return nil // No intersection
logrus.Error("line of sight: no intersection with ellipsoid")
return nil, fmt.Errorf("line of sight: no intersection with ellipsoid") // No intersection
}
t1 := (-B + math.Sqrt(delta)) / (2 * A)
t2 := (-B - math.Sqrt(delta)) / (2 * A)
@@ -93,5 +104,5 @@ func intersectWithEllipsoid(p0, d []float64) []float64 {
p0[0] + t*d[0],
p0[1] + t*d[1],
p0[2] + t*d[2],
}
}, nil
}

65
pkg/calculator/orbit.go Normal file
View File

@@ -0,0 +1,65 @@
package calculator
import (
"math"
"gonum.org/v1/gonum/mat"
"gonum.org/v1/gonum/spatial/r3"
)
// OrbitToECMatrix 轨道坐标系到ECI、ECEF坐标系的变换矩阵
func OrbitToECMatrix(pos, vec []float64) *mat.Dense {
r := r3.Vec{X: pos[0], Y: pos[1], Z: pos[2]}
rmag := r3.Norm(r) // Magnitude
v := r3.Vec{X: vec[0], Y: vec[1], Z: vec[2]}
vmag := r3.Norm(v) // Magnitude of velocity vector
w := r3.Cross(v, r)
wmag := r3.Norm(w)
z0 := r3.Scale(-1/rmag, r) // z方向指向地心
y0 := r3.Scale(1/wmag, w)
x0 := r3.Scale(1/vmag, v)
m := mat.NewDense(3, 3, []float64{
x0.X, y0.X, z0.X,
x0.Y, y0.Y, z0.Y,
x0.Z, y0.Z, z0.Z,
})
return m
}
// IntersectionECEF 计算卫星与相机的交点，返回经纬度和高度
// FIXME: 该计算方法有误，待修正
func IntersectionECEF(Qsat2orbit Quaternion, satPos84, vec84 []float64, ucam int) (IntersectionPoint, error) {
alpha := FOV * math.Pi / 180.0
alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
direction := []float64{0, math.Tan(alpha), -1.3} // 卫星（相机）坐标系下CCD成像方向向量
// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
Rcam := CameraRotMatrix(AngleCamSatX*math.Pi/180.0, AngleCamSatY*math.Pi/180.0, 0)
var dCam mat.VecDense
dCam.MulVec(Rcam, mat.NewVecDense(3, direction))
// -------- 转到轨道坐标系 --------
Rsat2orbit := Qsat2orbit.ToRotationMatrix()
var r0 mat.VecDense
r0.MulVec(Rsat2orbit, &dCam)
dOrbit := r0.RawVector().Data
// -------- 转到ECEF坐标系 --------
Rorbit2ecef := OrbitToECMatrix(satPos84, vec84)
var r1 mat.VecDense
r1.MulVec(Rorbit2ecef, mat.NewVecDense(3, dOrbit))
dECEF := r1.RawVector().Data
// -------- 计算交点 --------}
intersection, err := intersectWithEllipsoid(satPos84, dECEF)
if err != nil {
return IntersectionPoint{}, err
}
lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
return IntersectionPoint{Lat: lat, Lon: lon, H: h}, err
}

44
pkg/producer/aux.go
View File

@@ -39,38 +39,53 @@ func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.P
startPosInAux, endPosInAux := r.SceneInAuxIndex(scene)
as := r.AuxPlatforms[startPosInAux]
startTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(as.UTCTimeSec), int64(as.Microsecond)*1000).UTC()
startPos84 := []float64{as.W84PosX, as.W84PosY, as.W84PosZ}
ae := r.AuxPlatforms[endPosInAux]
startTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(as.UTCTimeSec), int64(as.Microsecond)*1000).UTC()
endTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(ae.UTCTimeSec), int64(ae.Microsecond)*1000).UTC()
startPos84 := []float64{as.W84PosX, as.W84PosY, as.W84PosZ}
endPos84 := []float64{ae.W84PosX, ae.W84PosY, ae.W84PosZ}
// ------------------ 使用定姿态四元数计算图像边界 ------------------
log.Info("using attitude quaternion to calculate image boundary...")
Qsat2eci := calculator.Quaternion{W: as.QuatAttstarQ0, X: as.QuatAttstarQ1, Y: as.QuatAttstarQ2, Z: as.QuatAttstarQ3}
line0Start := calculator.Intersection(Qsat2eci, startPos84, startTime, 0)
line0End := calculator.Intersection(Qsat2eci, startPos84, startTime, 9344)
line0Start, _ := calculator.IntersectionAttitude(Qsat2eci, startPos84, startTime, 0)
line0End, _ := calculator.IntersectionAttitude(Qsat2eci, startPos84, startTime, 9344)
Qsat2eci = calculator.Quaternion{W: ae.QuatAttstarQ0, X: ae.QuatAttstarQ1, Y: ae.QuatAttstarQ2, Z: ae.QuatAttstarQ3}
lineNStart := calculator.Intersection(Qsat2eci, endPos84, endTime, 0)
lineNEnd := calculator.Intersection(Qsat2eci, endPos84, endTime, 9344)
lineNStart, _ := calculator.IntersectionAttitude(Qsat2eci, endPos84, endTime, 0)
lineNEnd, _ := calculator.IntersectionAttitude(Qsat2eci, endPos84, endTime, 9344)
// ------------------ 使用本体和轨道四元数计算图像边界 ------------------
// ------------------ 使用本体和轨道四元数计算图像边界 ECI------------------
// log.Info("using orbit and body quaternion to calculate image boundary...")
// Qsat2orbit := calculator.Quaternion{X: as.QuatOrbitQ1, Y: as.QuatOrbitQ2, Z: as.QuatOrbitQ3}
// Qsat2orbit.W = math.Sqrt(1 - Qsat2orbit.X*Qsat2orbit.X - Qsat2orbit.Y*Qsat2orbit.Y - Qsat2orbit.Z*Qsat2orbit.Z)
// Qorbit2eci := calculator.Quaternion{X: as.QuatOrbJQ1, Y: as.QuatOrbJQ2, Z: as.QuatOrbJQ3}
// Qorbit2eci.W = math.Sqrt(1 - Qorbit2eci.X*Qorbit2eci.X - Qorbit2eci.Y*Qorbit2eci.Y - Qorbit2eci.Z*Qorbit2eci.Z)
// line0Start := calculator.Intersection2(Qsat2orbit, Qorbit2eci, startPos84, startTime, 0)
// line0End := calculator.Intersection2(Qsat2orbit, Qorbit2eci, startPos84, startTime, 9344)
// line0Start,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, startPos84, startTime, 0)
// line0End,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, startPos84, startTime, 9344)
// Qsat2orbit = calculator.Quaternion{X: ae.QuatOrbitQ1, Y: ae.QuatOrbitQ2, Z: ae.QuatOrbitQ3}
// Qsat2orbit.W = math.Sqrt(1 - Qsat2orbit.X*Qsat2orbit.X - Qsat2orbit.Y*Qsat2orbit.Y - Qsat2orbit.Z*Qsat2orbit.Z)
// Qorbit2eci = calculator.Quaternion{X: ae.QuatOrbJQ1, Y: ae.QuatOrbJQ2, Z: ae.QuatOrbJQ3}
// Qorbit2eci.W = math.Sqrt(1 - Qorbit2eci.X*Qorbit2eci.X - Qorbit2eci.Y*Qorbit2eci.Y - Qorbit2eci.Z*Qorbit2eci.Z)
// lineNStart := calculator.Intersection2(Qsat2orbit, Qorbit2eci, endPos84, endTime, 0)
// lineNEnd := calculator.Intersection2(Qsat2orbit, Qorbit2eci, endPos84, endTime, 9344)
// lineNStart,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, endPos84, endTime, 0)
// lineNEnd,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, endPos84, endTime, 9344)
// ------------------ 使用本体和轨道四元数计算图像边界 ECEF------------------
// log.Info("using orbit and body quaternion to calculate image boundary...")
// Qsat2orbit := calculator.Quaternion{X: as.QuatOrbitQ1, Y: as.QuatOrbitQ2, Z: as.QuatOrbitQ3}
// Qsat2orbit.W = math.Sqrt(1 - Qsat2orbit.X*Qsat2orbit.X - Qsat2orbit.Y*Qsat2orbit.Y - Qsat2orbit.Z*Qsat2orbit.Z)
// vec84 := []float64{as.W84VelX, as.W84VelY, as.W84VelZ}
// line0Start, _ := calculator.IntersectionECEF(Qsat2orbit, startPos84, vec84, 0)
// line0End, _ := calculator.IntersectionECEF(Qsat2orbit, startPos84, vec84, 9344)
// Qsat2orbit = calculator.Quaternion{X: ae.QuatOrbitQ1, Y: ae.QuatOrbitQ2, Z: ae.QuatOrbitQ3}
// Qsat2orbit.W = math.Sqrt(1 - Qsat2orbit.X*Qsat2orbit.X - Qsat2orbit.Y*Qsat2orbit.Y - Qsat2orbit.Z*Qsat2orbit.Z)
// vec84 = []float64{ae.W84VelX, ae.W84VelY, ae.W84VelZ}
// lineNStart, _ := calculator.IntersectionECEF(Qsat2orbit, endPos84, vec84, 0)
// lineNEnd, _ := calculator.IntersectionECEF(Qsat2orbit, endPos84, vec84, 9344)
// ------------------ 计算图像边界距离和分辨率 ------------------
W0 := geo.Distance(orb.Point{line0Start.Lon, line0Start.Lat}, orb.Point{line0End.Lon, line0End.Lat})
@@ -80,11 +95,6 @@ func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.P
xResolution := W0 / float64(scene.Width)
yResolution := H0 / float64(scene.Height)
scene.Meta.Gsd = math.Min(xResolution, yResolution)
// log.Debug("distance 0: ", W0)
// log.Debug("distance N: ", WN)
// log.Debug("distance 0-0: ", H0)
// log.Debug("distance N-N: ", HN)
log.Debug("resolution x: ", xResolution)
log.Debug("resolution y: ", yResolution)

9
run.sh
View File

@@ -1,9 +0,0 @@
go run cmd/*.go proc -p /Users/lan/workspace/sjy01/preprocessing/demo/output/052022/SJY01_PAN_20240520_115428_052022_103.RAW -m /Users/lan/workspace/sjy01/preprocessing/demo/output/052022/SJY01_MSS_20240520_115428_052022_103.RAW -o data/052022 --fus
go run cmd/*.go proc -p /Users/lan/workspace/sjy01/preprocessing/demo/output/051622/SJY01_PAN_20240516_101236_051622_096.RAW -m /Users/lan/workspace/sjy01/preprocessing/demo/output/051622/SJY01_MSS_20240516_101236_051622_096.RAW -o data/051622 --fus
go run cmd/*.go proc -p /Users/lan/workspace/sjy01/preprocessing/demo/output/051922/SJY01_PAN_20240519_121433_051922_102.RAW -m /Users/lan/workspace/sjy01/preprocessing/demo/output/051922/SJY01_MSS_20240519_121433_051922_102.RAW -o data/051922 --fus
go run cmd/*.go proc -p /Users/lan/workspace/sjy01/preprocessing/demo/output/051814/SJY01_PAN_20240517_111910_051814_098.RAW -m /Users/lan/workspace/sjy01/preprocessing/demo/output/051814/SJY01_MSS_20240517_111910_051814_098.RAW -o data/051814 --fus
go run cmd/*.go proc -p /Users/lan/workspace/sjy01/preprocessing/demo/output/052723/SJY01_PAN_20240524_120820_052723_011.RAW -m /Users/lan/workspace/sjy01/preprocessing/demo/output/052723/SJY01_MSS_20240524_120820_052723_011.RAW -o data/052723 --fus