从位置和速度计算验证轨道到地心坐标系的旋转矩阵

Dockerfile.build

@@ -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
+# WORKDIR /src
-COPY . .
+# COPY . .
 # RUN cd /sjy01/image-proc && go mod download && make linux
-# CMD ["/src/build/build.sh"]
+CMD ["/bin/bash", "/src/build/build.sh"]
-RUN cd /src && make linux

build.sh

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

build/build-in-docker.sh

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

pkg/calculator/intersection.go

@@ -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)
+	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 Intersection2(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint {
+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
 }

pkg/calculator/orbit.go

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

pkg/producer/aux.go

@@ -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)
+	line0Start, _ := calculator.IntersectionAttitude(Qsat2eci, startPos84, startTime, 0)
-	line0End := calculator.Intersection(Qsat2eci, startPos84, startTime, 9344)
+	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)
+	lineNStart, _ := calculator.IntersectionAttitude(Qsat2eci, endPos84, endTime, 0)
-	lineNEnd := calculator.Intersection(Qsat2eci, endPos84, endTime, 9344)
+	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)
+	// line0Start,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, startPos84, startTime, 0)
-	// line0End := calculator.Intersection2(Qsat2orbit, Qorbit2eci, startPos84, startTime, 9344)
+	// 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)
+	// lineNStart,_ := calculator.IntersectionECI(Qsat2orbit, Qorbit2eci, endPos84, endTime, 0)
-	// lineNEnd := calculator.Intersection2(Qsat2orbit, Qorbit2eci, endPos84, endTime, 9344)
+	// 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)
 

run.sh

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