diff --git a/cmd/proc.go b/cmd/proc.go index 47f1688..43a2550 100644 --- a/cmd/proc.go +++ b/cmd/proc.go @@ -68,7 +68,7 @@ var procCmd = &cobra.Command{ reg.SaveRegisteredMssToGDALGTiff(reg.Params.MssTiffFile) } - if params.DoPansharpen { + if reg.Params.DoPansharpen { reg.DoScenePansharpen(panScenes, mssScenes) } diff --git a/pkg/calculator/intersection.go b/pkg/calculator/intersection.go index 173822f..8c5c624 100644 --- a/pkg/calculator/intersection.go +++ b/pkg/calculator/intersection.go @@ -14,7 +14,13 @@ const ( nPixels = 9344 // 像素数 ) -func Intersection(q Quaternion, satPos []float64, satTime time.Time, ucam int) (float64, float64) { +type IntersectionPoint struct { + Lat float64 + Lon float64 + H float64 +} + +func Intersection(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint { alpha := FOV * math.Pi / 180.0 alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(nPixels)) direction := []float64{0, math.Tan(alpha), -1.3} @@ -33,11 +39,42 @@ func Intersection(q Quaternion, satPos []float64, satTime time.Time, ucam int) ( 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]) + intersection := intersectWithEllipsoid(satPos84, ecefDirection) + lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2]) + return IntersectionPoint{Lat: lat, Lon: lon, H: h} - return lat, lon +} +func Intersection2(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint { + alpha := FOV * math.Pi / 180.0 + alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(nPixels)) + direction := []float64{0, math.Tan(alpha), -1.3} // 卫星（相机）坐标系下CCD成像方向向量 + + // -------- 转到轨道坐标系 -------- + Rsat2orbit := Qsat2orbit.ToRotationMatrix() + Rsat2orbitT := &mat.Dense{} + Rsat2orbitT.Inverse(Rsat2orbit) + var r0 mat.VecDense + r0.MulVec(Rsat2orbit, mat.NewVecDense(3, direction)) + dOrbit := r0.RawVector().Data + + // -------- 转到ECI坐标系 -------- + Rorbit2eci := Qorbit2eci.ToRotationMatrix() + Rorbit2eciT := &mat.Dense{} + Rorbit2eciT.Inverse(Rorbit2eci) + var r1 mat.VecDense + r1.MulVec(Rorbit2eci, mat.NewVecDense(3, dOrbit)) + dECI := r1.RawVector().Data + + // -------- 转到ECEF坐标系 -------- + x, y, z := ECItoECEF(dECI[0], dECI[1], dECI[2], satTime) + 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} } // 计算与椭球表面的交点 diff --git a/pkg/calculator/quaternion.go b/pkg/calculator/quaternion.go index f8bd73b..f8cdc1a 100644 --- a/pkg/calculator/quaternion.go +++ b/pkg/calculator/quaternion.go @@ -1,6 +1,8 @@ package calculator import ( + "math" + "gonum.org/v1/gonum/mat" ) @@ -18,3 +20,118 @@ func (q Quaternion) ToRotationMatrix() *mat.Dense { 2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y, }) } + +// EulerToQuaternion converts Euler angles (yaw, pitch, roll) to a quaternion. +// Assuming the order of rotations is ZYX (yaw-pitch-roll). +func EulerToQuaternion(yaw, pitch, roll float64) Quaternion { + halfYaw := yaw / 2 + halfPitch := pitch / 2 + halfRoll := roll / 2 + + cy := math.Cos(halfYaw) + sy := math.Sin(halfYaw) + cp := math.Cos(halfPitch) + sp := math.Sin(halfPitch) + cr := math.Cos(halfRoll) + sr := math.Sin(halfRoll) + + q := Quaternion{ + W: cy*cp*cr + sy*sp*sr, + X: cy*cp*sr - sy*sp*cr, + Y: sy*cp*sr + cy*sp*cr, + Z: sy*cp*cr - cy*sp*sr, + } + + return q +} + +// EulerToRotMatrix converts Euler angles (roll, pitch, yaw) to a rotation matrix. +func EulerToRotMatrix(phi, theta, psi float64) *mat.Dense { + // Calculate individual rotation matrices + Rx := mat.NewDense(3, 3, []float64{ + 1, 0, 0, + 0, math.Cos(phi), -math.Sin(phi), + 0, math.Sin(phi), math.Cos(phi), + }) + + Ry := mat.NewDense(3, 3, []float64{ + math.Cos(theta), 0, math.Sin(theta), + 0, 1, 0, + -math.Sin(theta), 0, math.Cos(theta), + }) + + Rz := mat.NewDense(3, 3, []float64{ + math.Cos(psi), -math.Sin(psi), 0, + math.Sin(psi), math.Cos(psi), 0, + 0, 0, 1, + }) + + // R = Rz * Ry * Rx + RyRx := mat.NewDense(3, 3, nil) + RyRx.Mul(Ry, Rx) + + R := mat.NewDense(3, 3, nil) + R.Mul(Rz, RyRx) + + return R +} + +// RotMatrixToQuaternion converts a rotation matrix to a quaternion. +func RotMatrixToQuaternion(R *mat.Dense) Quaternion { + m := R.RawMatrix().Data + + trace := m[0] + m[4] + m[8] + var q Quaternion + + if trace > 0 { + S := 0.5 / math.Sqrt(trace+1.0) + q.W = 0.25 / S + q.X = (m[7] - m[5]) * S + q.Y = (m[2] - m[6]) * S + q.Z = (m[3] - m[1]) * S + } else { + if m[0] > m[4] && m[0] > m[8] { + S := 2.0 * math.Sqrt(1.0+m[0]-m[4]-m[8]) + q.W = (m[7] - m[5]) / S + q.X = 0.25 * S + q.Y = (m[1] + m[3]) / S + q.Z = (m[2] + m[6]) / S + } else if m[4] > m[8] { + S := 2.0 * math.Sqrt(1.0+m[4]-m[0]-m[8]) + q.W = (m[2] - m[6]) / S + q.X = (m[1] + m[3]) / S + q.Y = 0.25 * S + q.Z = (m[5] + m[7]) / S + } else { + S := 2.0 * math.Sqrt(1.0+m[8]-m[0]-m[4]) + q.W = (m[3] - m[1]) / S + q.X = (m[2] + m[6]) / S + q.Y = (m[5] + m[7]) / S + q.Z = 0.25 * S + } + } + + return q +} + +func RotMatrixToEuler(R *mat.Dense) (yaw, pitch, roll float64) { + m := R.RawMatrix().Data + + if m[6] < 1 { + if m[6] > -1 { + pitch = math.Asin(-m[6]) + roll = math.Atan2(m[7], m[8]) + yaw = math.Atan2(m[3], m[0]) + } else { + pitch = math.Pi / 2 + roll = -math.Atan2(-m[1], m[4]) + yaw = 0 + } + } else { + pitch = -math.Pi / 2 + roll = math.Atan2(-m[1], m[4]) + yaw = 0 + } + + return yaw, pitch, roll +} diff --git a/pkg/producer/aux.go b/pkg/producer/aux.go index 1f4385a..484887c 100644 --- a/pkg/producer/aux.go +++ b/pkg/producer/aux.go @@ -1,10 +1,11 @@ package producer import ( - "fmt" "math" "time" + log "github.com/sirupsen/logrus" + "github.com/duke-git/lancet/v2/mathutil" "github.com/paulmach/orb" "github.com/paulmach/orb/geo" @@ -33,118 +34,97 @@ func (r *Registrator) SceneImageTime(scene *Scene) (start, center, end time.Time return } -// 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) - - // 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}, - // }, - // } - +// FIXME: This function is not accurate enough. +func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.Point) { 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})) + startPos84 := []float64{as.W84PosX, as.W84PosY, as.W84PosZ} + startTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(as.UTCTimeSec), int64(as.Microsecond)*1000).UTC() 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, - ) + endPos84 := []float64{ae.W84PosX, ae.W84PosY, ae.W84PosZ} + endTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(ae.UTCTimeSec), int64(ae.Microsecond)*1000).UTC() - fmt.Println("distance 1: ", geo.Distance(orb.Point{lng2, lat2}, orb.Point{lng3, lat3})) + // ------------------ 使用定姿态四元数计算图像边界 ------------------ + 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) + + 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) + + // ------------------ 使用本体和轨道四元数计算图像边界 ------------------ + // 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) + + // 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) + + // ------------------ 计算图像边界距离和分辨率 ------------------ + W0 := geo.Distance(orb.Point{line0Start.Lon, line0Start.Lat}, orb.Point{line0End.Lon, line0End.Lat}) + // WN := geo.Distance(orb.Point{lineNStart.Lon, lineNStart.Lat}, orb.Point{lineNEnd.Lon, lineNEnd.Lat}) + H0 := geo.Distance(orb.Point{line0Start.Lon, line0Start.Lat}, orb.Point{lineNStart.Lon, lineNStart.Lat}) + // HN := geo.Distance(orb.Point{line0End.Lon, line0End.Lat}, orb.Point{lineNEnd.Lon, lineNEnd.Lat}) + xResolution := W0 / float64(scene.Width) + yResolution := H0 / float64(scene.Height) + scene.Meta.Gsd = math.Max(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) // 求外接矩形 - 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) + latMin := mathutil.Min(line0Start.Lat, line0End.Lat, lineNStart.Lat, lineNEnd.Lat) + lngMin := mathutil.Min(line0Start.Lon, line0End.Lon, lineNStart.Lon, lineNEnd.Lon) + latMax := mathutil.Max(line0Start.Lat, line0End.Lat, lineNStart.Lat, lineNEnd.Lat) + lngMax := mathutil.Max(line0Start.Lon, line0End.Lon, lineNStart.Lon, lineNEnd.Lon) poly := orb.Polygon{ { - {lng, lat}, - {lng1, lat}, - {lng1, lat1}, - {lng, lat1}, - {lng, lat}, + {lngMin, latMin}, + {lngMax, latMin}, + {lngMax, latMax}, + {lngMin, latMax}, + {lngMin, latMin}, }, } centroid, _ := planar.CentroidArea(poly) scene.Meta.CentreLocation.Latitude = centroid.Y() scene.Meta.CentreLocation.Longitude = centroid.X() - scene.Meta.Corners.UpperLeft.Latitude = lat - scene.Meta.Corners.UpperLeft.Longitude = lng - scene.Meta.Corners.UpperRight.Latitude = lat - scene.Meta.Corners.UpperRight.Longitude = lng1 - scene.Meta.Corners.LowerLeft.Latitude = lat1 - scene.Meta.Corners.LowerLeft.Longitude = lng - scene.Meta.Corners.LowerRight.Latitude = lat1 - scene.Meta.Corners.LowerRight.Longitude = lng1 - 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.Corners.UpperLeft.Latitude = line0Start.Lat + scene.Meta.Corners.UpperLeft.Longitude = line0Start.Lon + scene.Meta.Corners.UpperRight.Latitude = line0End.Lat + scene.Meta.Corners.UpperRight.Longitude = line0End.Lon + scene.Meta.Corners.LowerLeft.Latitude = lineNStart.Lat + scene.Meta.Corners.LowerLeft.Longitude = lineNStart.Lon + scene.Meta.Corners.LowerRight.Latitude = lineNEnd.Lat + scene.Meta.Corners.LowerRight.Longitude = lineNEnd.Lon - scene.Meta.SatPosX = ae.WGS84PosX - scene.Meta.SatPosY = ae.WGS84PosY - scene.Meta.SatPosZ = ae.WGS84PosZ + scene.Meta.SatPosX = startPos84[0] + scene.Meta.SatPosY = startPos84[1] + scene.Meta.SatPosZ = startPos84[2] 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) - - // fd, _ := fcs.MarshalJSON() - // fmt.Println(string(fd)) - return } diff --git a/pkg/producer/output.go b/pkg/producer/output.go index a01c55c..f5a2d76 100644 --- a/pkg/producer/output.go +++ b/pkg/producer/output.go @@ -13,7 +13,7 @@ import ( ) func (r *Registrator) SaveOriginalPanToGDALGTiff(tiffFile string) error { - err := savePanToGDALGTiff(r.PanImage, 0, 0, tiffFile) + err := savePanToGDALGTiff(r.PanImage, 0, 0, tiffFile, PanResolution) if err != nil { return err } @@ -21,7 +21,7 @@ func (r *Registrator) SaveOriginalPanToGDALGTiff(tiffFile string) error { return nil } -func savePanToGDALGTiff(pan gocv.Mat, topLeftX, topLeftY float64, tiffFile string) error { +func savePanToGDALGTiff(pan gocv.Mat, topLeftX, topLeftY float64, tiffFile string, resolution float64) error { // log.Println("Saving PAN image to TIFF file:", tiffFile) width := pan.Cols() @@ -34,7 +34,7 @@ func savePanToGDALGTiff(pan gocv.Mat, topLeftX, topLeftY float64, tiffFile strin } defer ds.Close() - setGeoTransform(ds, topLeftX, topLeftY, PanResolution) + setGeoTransform(ds, topLeftX, topLeftY, resolution) ds.SetMetadata("NBITS", "16") // 将通道的数据转换为uint16数组 diff --git a/pkg/producer/scenes.go b/pkg/producer/scenes.go index 6cdaced..989d1d9 100644 --- a/pkg/producer/scenes.go +++ b/pkg/producer/scenes.go @@ -38,8 +38,8 @@ func (s *Scene) Cleanup() { // MSS 2336 * 2336 - 1764 // PAN 9344 * 9344 - 7056 func (r *Registrator) SubScenes() (panScenes []*Scene, mssScenes []*Scene, err error) { - hPAN := (7056 - 1764) - hPANOverlap := 1764 + hPAN := (9344 - 2336) + hPANOverlap := 2336 panScenesCnt := r.PanHeight / hPAN for i := 0; i < panScenesCnt; i++ { @@ -110,12 +110,13 @@ func (r *Registrator) SaveScenesToTiff(panScenes []*Scene, mssScenes []*Scene) e filename := fmt.Sprintf("%s_L1A.tiff", scene.SceneId) scene.Tiff = filepath.Join(dir, filename) scene.Meta = r.makeProductMeta(scene) - r.ScenePosition(scene) + r.SetSceneBoundary(scene) err := savePanToGDALGTiff(scene.Mat[0], scene.Meta.Corners.UpperLeft.Longitude, scene.Meta.Corners.UpperLeft.Latitude, - scene.Tiff) + scene.Tiff, + scene.Meta.Gsd) if err != nil { log.Errorf("save pan scene %d to tiff failed: %v", i+1, err) return err @@ -147,7 +148,7 @@ func (r *Registrator) SaveScenesToTiff(panScenes []*Scene, mssScenes []*Scene) e data, _ := json.Marshal(fc) // 输出 GeoJSON 数据 - fmt.Println(string(data)) + log.Debug(string(data)) for i, scene := range mssScenes { dir := filepath.Join(r.Params.OutputDir, fmt.Sprintf("%03d", i+1), "MSS") @@ -156,14 +157,14 @@ func (r *Registrator) SaveScenesToTiff(panScenes []*Scene, mssScenes []*Scene) e filename := fmt.Sprintf("%s_L1A.tiff", scene.SceneId) scene.Tiff = filepath.Join(dir, filename) scene.Meta = r.makeProductMeta(scene) - r.ScenePosition(scene) + r.SetSceneBoundary(scene) rgbirImage, _ := r.MergeMSSToBGRNIR(scene.Mat) err := SaveBGRToGDALGTiff(rgbirImage, 4, scene.Meta.Corners.UpperLeft.Longitude, scene.Meta.Corners.UpperLeft.Latitude, - MssResolution, + scene.Meta.Gsd, []godal.ColorInterp{godal.CIBlue, godal.CIGreen, godal.CIRed, godal.CIUndefined}, scene.Tiff) if err != nil {