200 lines
7.0 KiB
Go
200 lines
7.0 KiB
Go
package producer
|
|
|
|
import (
|
|
"encoding/json"
|
|
"fmt"
|
|
"math"
|
|
"os"
|
|
"path/filepath"
|
|
"strings"
|
|
"time"
|
|
|
|
log "github.com/sirupsen/logrus"
|
|
|
|
"github.com/duke-git/lancet/v2/mathutil"
|
|
"github.com/paulmach/orb"
|
|
"github.com/paulmach/orb/geo"
|
|
"github.com/paulmach/orb/geojson"
|
|
"github.com/paulmach/orb/planar"
|
|
"starwiz.cn/sjy01/image-proc/pkg/auxilary"
|
|
"starwiz.cn/sjy01/image-proc/pkg/calculator"
|
|
"starwiz.cn/sjy01/image-proc/pkg/config"
|
|
)
|
|
|
|
func (r *Registrator) LoadAuxData() error {
|
|
var err error
|
|
r.auxHeads, r.auxBoxes, r.AuxPlatforms, err = auxilary.ExtractAux(r.Params.AuxRawFile)
|
|
|
|
attFile := strings.Replace(r.Params.AuxRawFile, ".AUX", ".att.txt", 1)
|
|
gpsFile := strings.Replace(r.Params.AuxRawFile, ".AUX", ".gps.txt", 1)
|
|
r.AttQuaternion, _ = auxilary.StoreAtt(r.AuxPlatforms, attFile)
|
|
r.GPSs, _ = auxilary.StoreGPS(r.AuxPlatforms, gpsFile)
|
|
r.ImageTime = auxilary.NewImageTime(r.AuxPlatforms)
|
|
|
|
return err
|
|
}
|
|
|
|
// 数据校验和测试
|
|
func (r *Registrator) AuxPrint() {
|
|
var fcPos84 geojson.FeatureCollection
|
|
var fcPos84Interp geojson.FeatureCollection
|
|
for _, p := range r.AuxPlatforms {
|
|
lat, lon, _ := calculator.ECEFGeocentricToGeodetic(p.W84PosX, p.W84PosY, p.W84PosZ)
|
|
point := orb.Point{lon, lat}
|
|
fcPos84.Features = append(fcPos84.Features, geojson.NewFeature(point))
|
|
|
|
tp := float64(p.UTCTimeSec) + float64(auxilary.ReferenceTime2000) + float64(p.Microsecond)/1e6
|
|
interp := r.GPSs.Lagrange(tp)
|
|
lat, lon, _ = calculator.ECEFGeocentricToGeodetic(interp.X84, interp.Y84, interp.Z84)
|
|
point = orb.Point{lon, lat}
|
|
fcPos84Interp.Features = append(fcPos84Interp.Features, geojson.NewFeature(point))
|
|
}
|
|
|
|
data, _ := json.Marshal(fcPos84)
|
|
f, _ := os.Create(filepath.Join(config.GCONFIG.Log.LogDir, fmt.Sprintf("%s_aux_pos_84.geojson", r.Params.DataId)))
|
|
defer f.Close()
|
|
f.Write(data)
|
|
|
|
data, _ = json.Marshal(fcPos84Interp)
|
|
f, _ = os.Create(filepath.Join(config.GCONFIG.Log.LogDir, fmt.Sprintf("%s_aux_pos_84_interp.geojson", r.Params.DataId)))
|
|
defer f.Close()
|
|
f.Write(data)
|
|
}
|
|
|
|
func (r *Registrator) SceneImageTime(scene *Scene) (start, center, end time.Time) {
|
|
startPosInAux, endPosInAux := r.SceneInAuxIndex(scene)
|
|
centerPosInAux := (startPosInAux + endPosInAux) / 2
|
|
|
|
start = time.Unix(int64(auxilary.ReferenceTime2000)+int64(r.AuxPlatforms[startPosInAux].UTCTimeSec),
|
|
int64(r.AuxPlatforms[startPosInAux].Microsecond)*1000)
|
|
center = time.Unix(int64(auxilary.ReferenceTime2000)+int64(r.AuxPlatforms[centerPosInAux].UTCTimeSec),
|
|
int64(r.AuxPlatforms[centerPosInAux].Microsecond)*1000)
|
|
end = time.Unix(int64(auxilary.ReferenceTime2000)+int64(r.AuxPlatforms[endPosInAux].UTCTimeSec),
|
|
int64(r.AuxPlatforms[endPosInAux].Microsecond)*1000)
|
|
|
|
return
|
|
}
|
|
|
|
// FIXME: This function is not accurate enough. 四元数、成像时刻、GPS 等需要修改为插值获取
|
|
func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.Point) {
|
|
log.Info("using attitude quaternion to calculate image boundary...")
|
|
line0Start := r.calculateLatLonH(scene, 0, 0, 0)
|
|
line0End := r.calculateLatLonH(scene, 0, scene.Width, 0)
|
|
lineNStart := r.calculateLatLonH(scene, scene.Height, 0, 0)
|
|
lineNEnd := r.calculateLatLonH(scene, scene.Height, scene.Width, 0)
|
|
|
|
// ------------------ 计算图像边界距离和分辨率 ------------------
|
|
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.Min(xResolution, yResolution)
|
|
|
|
// FIXME: 临时设置分辨率
|
|
if scene.Meta.Gsd < 2 {
|
|
scene.Meta.Gsd = 1.3
|
|
} else {
|
|
scene.Meta.Gsd = 5.2
|
|
}
|
|
log.Debug("resolution x: ", xResolution)
|
|
log.Debug("resolution y: ", yResolution)
|
|
|
|
// 求外接矩形
|
|
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{
|
|
{
|
|
{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 = 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 = 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
|
|
|
|
// 计算RPC
|
|
rpc := NewRPC(r, scene, strings.Replace(scene.Tiff, ".tiff", ".rpb", 1))
|
|
if err := rpc.RPC(); err != nil {
|
|
log.Error("calculate RPC failed: ", err)
|
|
}
|
|
|
|
return
|
|
}
|
|
|
|
func (r *Registrator) SceneInAuxIndex(scene *Scene) (int, int) {
|
|
startPosInAux := r.sceneOffsetInAuxIndex(scene, 0)
|
|
endPosInAux := r.sceneOffsetInAuxIndex(scene, scene.Height)
|
|
return startPosInAux, endPosInAux
|
|
}
|
|
|
|
func (r *Registrator) sceneOffsetInAuxIndex(scene *Scene, offset int) int {
|
|
var auxForImageRow int
|
|
switch scene.Type {
|
|
case "MSS":
|
|
auxForImageRow = 4
|
|
case "PAN":
|
|
auxForImageRow = 16
|
|
case "FUS":
|
|
auxForImageRow = 16
|
|
}
|
|
|
|
idx := (scene.Y + offset) / auxForImageRow
|
|
if idx >= len(r.AuxPlatforms) {
|
|
idx = len(r.AuxPlatforms) - 1
|
|
}
|
|
return idx
|
|
}
|
|
|
|
// row, col 相对于图像景左上角, H 为地面目标点高度
|
|
func (r *Registrator) calculateLatLonH(scene *Scene, row, col, H int) calculator.IntersectionPoint {
|
|
// 内插值获取图像行时刻
|
|
ucam := col
|
|
cross := 16
|
|
if scene.Type == "MSS" {
|
|
cross = 4
|
|
ucam = 4 * col // 统一使用相机PAN像元宽度进行计算
|
|
}
|
|
|
|
imgrow := row + scene.Y
|
|
imgtime, _ := r.ImageTime.Interp(imgrow, cross)
|
|
nanosecond := (imgtime - math.Floor(imgtime)) * 1000000000
|
|
sattime := time.Unix(int64(imgtime), int64(nanosecond)).UTC()
|
|
|
|
// 球面线性插值得到姿态四元数
|
|
qECI := r.AttQuaternion.Slerp(imgtime)
|
|
// 拉格朗日插值得到卫星GPS坐标
|
|
p84 := r.GPSs.Lagrange(imgtime)
|
|
// 计算目标点在WGS84坐标系下的坐标
|
|
cECI := calculator.Quaternion{W: qECI.Q0, X: qECI.Q1, Y: qECI.Q2, Z: qECI.Q3}
|
|
groudPoint84, _ := calculator.Camera2GroundPoint(cECI,
|
|
[]float64{p84.X84, p84.Y84, p84.Z84},
|
|
sattime, ucam, H)
|
|
return groudPoint84
|
|
}
|