361 lines
9.3 KiB
Go
361 lines
9.3 KiB
Go
package producer
|
|
|
|
import (
|
|
"encoding/json"
|
|
"fmt"
|
|
"os"
|
|
"strings"
|
|
|
|
"github.com/paulmach/orb"
|
|
"github.com/paulmach/orb/geojson"
|
|
|
|
log "github.com/sirupsen/logrus"
|
|
"gonum.org/v1/gonum/mat"
|
|
"starwiz.cn/sjy01/image-proc/pkg/config"
|
|
"starwiz.cn/sjy01/image-proc/pkg/dem"
|
|
)
|
|
|
|
const FLT_EPSILON = 1.192092896e-07
|
|
|
|
type RPC struct {
|
|
lineOffset, lineScale float64
|
|
sampOffset, sampScale float64
|
|
latOffset, longOffset, heightOffset float64
|
|
latScale, longScale, heightScale float64
|
|
|
|
LineCoef RPCModel
|
|
SampCoef RPCModel
|
|
|
|
minH, maxH float64
|
|
GCPs []*GroundPoint
|
|
elevationLayer int
|
|
gridsize int
|
|
|
|
scene *Scene
|
|
registrator *Registrator
|
|
}
|
|
|
|
// GroundPoint 表示地面点的三维坐标
|
|
type GroundPoint struct {
|
|
P, L, H float64 // P-latitude, L-longitude, H-height
|
|
Y, X float64 // X-sample, Y-line
|
|
}
|
|
|
|
// ImagePoint 表示像素平面上的点
|
|
type ImagePoint struct {
|
|
Y, X float64 // X-sample, Y-line
|
|
}
|
|
|
|
// RPCModel 包含20个系数的RPC模型
|
|
type RPCModel struct {
|
|
NumCoefficients [20]float64 // 分子系数
|
|
DenCoefficients [20]float64 // 分母系数
|
|
}
|
|
|
|
// rational polynomial coeffients
|
|
func NewRPC(r *Registrator, scene *Scene) *RPC {
|
|
rpc := RPC{
|
|
elevationLayer: config.GCONFIG.RPC.AltitudeLayer,
|
|
gridsize: config.GCONFIG.RPC.GridSize,
|
|
registrator: r,
|
|
scene: scene,
|
|
}
|
|
|
|
log.Info("start RPC initialization for scene: ", scene.Tiff)
|
|
rpc.init()
|
|
|
|
return &rpc
|
|
}
|
|
|
|
// 初始化经纬度
|
|
func (rpc *RPC) init() {
|
|
rpc.minH = 9999.0
|
|
rpc.maxH = -9999.0
|
|
|
|
rpc.minH, rpc.maxH = dem.Dem1KmLT.MinMaxElevationInRect(
|
|
rpc.scene.Meta.Corners.UpperLeft.Longitude,
|
|
rpc.scene.Meta.Corners.UpperLeft.Latitude,
|
|
rpc.scene.Meta.Corners.LowerRight.Longitude,
|
|
rpc.scene.Meta.Corners.LowerRight.Latitude,
|
|
)
|
|
|
|
if rpc.minH < -10.0 {
|
|
rpc.minH = 0.0
|
|
}
|
|
|
|
if rpc.maxH < -10.0 {
|
|
rpc.maxH = 0.0
|
|
}
|
|
|
|
}
|
|
|
|
// 虚拟控制点
|
|
func (rpc *RPC) generateVirtualGCP() {
|
|
points := gridImage2(rpc.gridsize, rpc.gridsize,
|
|
rpc.scene.Height, rpc.scene.Width,
|
|
rpc.elevationLayer, int(rpc.minH), int(rpc.maxH))
|
|
|
|
for _, p := range points {
|
|
p84 := rpc.registrator.calculateLatLonH(rpc.scene, p.Row, p.Col, p.H)
|
|
rpc.GCPs = append(rpc.GCPs, &GroundPoint{
|
|
P: p84.Lat,
|
|
L: p84.Lon,
|
|
H: p84.H,
|
|
Y: float64(p.Row),
|
|
X: float64(p.Col),
|
|
})
|
|
}
|
|
|
|
name := strings.Replace(rpc.scene.Tiff, ".tiff", ".gcp.geojson", -1)
|
|
rpc.saveGCP(rpc.GCPs, name)
|
|
name = strings.Replace(rpc.scene.Tiff, ".tiff", ".gcp_orig.txt", -1)
|
|
rpc.saveGCP2(rpc.GCPs, name)
|
|
}
|
|
|
|
func (rpc *RPC) RPC() error {
|
|
rpc.generateVirtualGCP()
|
|
n := len(rpc.GCPs)
|
|
log.Info("num of virtual GCPs: ", n)
|
|
|
|
rowVec := mat.NewVecDense(n, nil)
|
|
colVec := mat.NewVecDense(n, nil)
|
|
latVec := mat.NewVecDense(n, nil)
|
|
lonVec := mat.NewVecDense(n, nil)
|
|
heightVec := mat.NewVecDense(n, nil)
|
|
|
|
for i, ip := range rpc.GCPs {
|
|
rowVec.SetVec(i, ip.Y)
|
|
colVec.SetVec(i, ip.X)
|
|
latVec.SetVec(i, ip.P)
|
|
lonVec.SetVec(i, ip.L)
|
|
heightVec.SetVec(i, ip.H)
|
|
}
|
|
|
|
rpc.saveVec(strings.Replace(rpc.scene.Tiff, ".tiff", ".vec.txt", -1),
|
|
rowVec, colVec, latVec, lonVec, heightVec)
|
|
|
|
// rpc.lineOffset = float64(rpc.scene.Height) / 2.0
|
|
// rpc.lineScale = float64(rpc.scene.Height)
|
|
// rowVec = normalize2(rowVec, rpc.lineOffset, rpc.lineScale)
|
|
// rpc.sampOffset = float64(rpc.scene.Width) / 2.0
|
|
// rpc.sampScale = float64(rpc.scene.Width)
|
|
// colVec = normalize2(colVec, rpc.sampOffset, rpc.sampScale)
|
|
// rpc.heightOffset = (rpc.minH + rpc.maxH) / 2.0
|
|
// rpc.heightScale = 1000.0
|
|
// heightVec = normalize2(heightVec, rpc.heightOffset, rpc.heightScale)
|
|
|
|
latVec, rpc.latOffset, rpc.latScale = normalize(latVec)
|
|
lonVec, rpc.longOffset, rpc.longScale = normalize(lonVec)
|
|
heightVec, rpc.heightOffset, rpc.heightScale = normalize(heightVec)
|
|
rowVec, rpc.lineOffset, rpc.lineScale = normalize(rowVec)
|
|
colVec, rpc.sampOffset, rpc.sampScale = normalize(colVec)
|
|
|
|
rpc.saveVec(strings.Replace(rpc.scene.Tiff, ".tiff", ".vec_norm.txt", -1),
|
|
rowVec, colVec, latVec, lonVec, heightVec)
|
|
|
|
// 设计矩阵 B = [ 20个分子系数 19个分母系数 ] W = 权矩阵 R = 观测结果
|
|
method := SolveMethod(config.GCONFIG.RPC.Method)
|
|
|
|
// x = (B^T * W^2 * B)^-1 * (B^T * W^2 * R), 其中 x = [a1..a20 b2..b20]^T
|
|
// 行参数
|
|
// B := setupSystemOfEquations(rowVec, latVec, lonVec, heightVec)
|
|
// J, err := SolveNormalEquation(B, rowVec)
|
|
log.Println("solving row coefficients")
|
|
J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec, method)
|
|
if err != nil {
|
|
return err
|
|
}
|
|
|
|
// 列参数
|
|
// D := setupSystemOfEquations(colVec, latVec, lonVec, heightVec)
|
|
// K, err := SolveNormalEquation(D, colVec)
|
|
log.Println("solving col coefficients")
|
|
K, err := solveCoefficients(colVec, latVec, lonVec, heightVec, method)
|
|
if err != nil {
|
|
return err
|
|
}
|
|
|
|
rpc.LineCoef.NumCoefficients[0] = J[0]
|
|
rpc.LineCoef.DenCoefficients[0] = 1.0
|
|
rpc.SampCoef.NumCoefficients[0] = K[0]
|
|
rpc.SampCoef.DenCoefficients[0] = 1.0
|
|
for i := 1; i < 20; i++ {
|
|
rpc.LineCoef.NumCoefficients[i] = J[i]
|
|
rpc.LineCoef.DenCoefficients[i] = J[i+19]
|
|
rpc.SampCoef.NumCoefficients[i] = K[i]
|
|
rpc.SampCoef.DenCoefficients[i] = K[i+19]
|
|
}
|
|
|
|
nameRPB0 := strings.Replace(rpc.scene.Tiff, ".tiff", ".rpb", -1)
|
|
rpc.saveRPB(nameRPB0)
|
|
|
|
projectedPoints := rpc.applyRFM(
|
|
mat.NewVecDense(20, rpc.LineCoef.NumCoefficients[:]),
|
|
mat.NewVecDense(20, rpc.LineCoef.DenCoefficients[:]),
|
|
mat.NewVecDense(20, rpc.SampCoef.NumCoefficients[:]),
|
|
mat.NewVecDense(20, rpc.SampCoef.DenCoefficients[:]),
|
|
rpc.GCPs,
|
|
)
|
|
name := strings.Replace(rpc.scene.Tiff, ".tiff", ".gcp_proj.txt", -1)
|
|
rpc.saveGCP2(projectedPoints, name)
|
|
|
|
return nil
|
|
}
|
|
|
|
func (rpc *RPC) Output() string {
|
|
var lineNumCoef, lineDenCoef, sampNumCoef, sampDenCoef string
|
|
for i := 0; i < 20; i++ {
|
|
if i < 19 {
|
|
lineNumCoef += fmt.Sprintf("\t\t%.15e,\n", rpc.LineCoef.NumCoefficients[i])
|
|
lineDenCoef += fmt.Sprintf("\t\t%.15e,\n", rpc.LineCoef.DenCoefficients[i])
|
|
sampNumCoef += fmt.Sprintf("\t\t%.15e,\n", rpc.SampCoef.NumCoefficients[i])
|
|
sampDenCoef += fmt.Sprintf("\t\t%.15e,\n", rpc.SampCoef.DenCoefficients[i])
|
|
} else {
|
|
lineNumCoef += fmt.Sprintf("\t\t%.15e", rpc.LineCoef.NumCoefficients[i])
|
|
lineDenCoef += fmt.Sprintf("\t\t%.15e", rpc.LineCoef.DenCoefficients[i])
|
|
sampNumCoef += fmt.Sprintf("\t\t%.15e", rpc.SampCoef.NumCoefficients[i])
|
|
sampDenCoef += fmt.Sprintf("\t\t%.15e", rpc.SampCoef.DenCoefficients[i])
|
|
}
|
|
}
|
|
|
|
model := fmt.Sprintf(`satId = "SJY01";
|
|
bandId = "";
|
|
SpecId = "";
|
|
BEGIN_GROUP = IMAGE
|
|
errBias = 1.0;
|
|
errRand = 0.0;
|
|
lineOffset = %.8f;
|
|
sampOffset = %.8f;
|
|
latOffset = %.8f;
|
|
longOffset = %.8f;
|
|
heightOffset = %.8f;
|
|
lineScale = %.8f;
|
|
sampScale = %.8f;
|
|
latScale = %.8f;
|
|
longScale = %.8f;
|
|
heightScale = %.8f;
|
|
lineNumCoef = (
|
|
%s);
|
|
lineDenCoef = (
|
|
%s);
|
|
sampNumCoef = (
|
|
%s);
|
|
sampDenCoef = (
|
|
%s);
|
|
END_GROUP = IMAGE
|
|
END;
|
|
`,
|
|
rpc.lineOffset, rpc.sampOffset, rpc.latOffset, rpc.longOffset, rpc.heightOffset,
|
|
rpc.lineScale, rpc.sampScale, rpc.latScale, rpc.longScale, rpc.heightScale,
|
|
lineNumCoef, lineDenCoef, sampNumCoef, sampDenCoef,
|
|
)
|
|
|
|
return model
|
|
}
|
|
|
|
func (rpc *RPC) saveRPB(name string) error {
|
|
log.Infof("save RPC model to %s", name)
|
|
model := rpc.Output()
|
|
f, err := os.Create(name)
|
|
if err != nil {
|
|
log.Errorf("Failed to create RPC model file: %v", err)
|
|
return err
|
|
}
|
|
defer f.Close()
|
|
_, err = f.WriteString(model)
|
|
if err != nil {
|
|
log.Errorf("Failed to write RPC model file: %v", err)
|
|
}
|
|
f.Sync()
|
|
return err
|
|
}
|
|
|
|
func (rpc *RPC) saveGCP(gcps []*GroundPoint, name string) error {
|
|
if !config.GCONFIG.RPC.Debug {
|
|
return nil
|
|
}
|
|
log.Infof("save gcp to %s", name)
|
|
f, err := os.Create(name)
|
|
if err != nil {
|
|
log.Errorf("Failed to create GCP file: %v", err)
|
|
return err
|
|
}
|
|
defer f.Close()
|
|
|
|
var gcp geojson.FeatureCollection
|
|
for _, p := range gcps {
|
|
point := orb.Point{p.L, p.P}
|
|
feature := geojson.NewFeature(point)
|
|
feature.Properties = map[string]interface{}{
|
|
"H": p.H,
|
|
"Y": p.Y,
|
|
"X": p.X,
|
|
}
|
|
gcp.Features = append(gcp.Features, feature)
|
|
}
|
|
data, _ := json.Marshal(gcp)
|
|
f.Write(data)
|
|
f.Sync()
|
|
|
|
return nil
|
|
}
|
|
|
|
func (rpc *RPC) saveGCP2(gcps []*GroundPoint, name string) error {
|
|
if !config.GCONFIG.RPC.Debug {
|
|
return nil
|
|
}
|
|
log.Infof("save gcp to %s", name)
|
|
f, err := os.Create(name)
|
|
if err != nil {
|
|
log.Errorf("Failed to create GCP file: %v", err)
|
|
return err
|
|
}
|
|
defer f.Close()
|
|
|
|
for _, p := range gcps {
|
|
f.WriteString(fmt.Sprintf("%.8f\t%.8f\t%.8f\t%.8f\t%.8f\n", p.L, p.P, p.H, p.Y, p.X))
|
|
}
|
|
f.Sync()
|
|
|
|
return nil
|
|
}
|
|
|
|
func (rpc *RPC) saveVec(name string, rowVec, colVec, latVec, lonVec, heightVec *mat.VecDense) error {
|
|
if !config.GCONFIG.RPC.Debug {
|
|
return nil
|
|
}
|
|
f, err := os.Create(name)
|
|
|
|
if err != nil {
|
|
log.Errorf("Failed to create vec file: %v", err)
|
|
return err
|
|
}
|
|
defer f.Close()
|
|
|
|
for i := 0; i < rowVec.Len(); i++ {
|
|
f.WriteString(fmt.Sprintf("%.8f\t%.8f\t%.8f\t%.8f\t%.8f\n",
|
|
rowVec.AtVec(i), colVec.AtVec(i), latVec.AtVec(i), lonVec.AtVec(i), heightVec.AtVec(i)))
|
|
}
|
|
return nil
|
|
}
|
|
|
|
func (rpc *RPC) applyRFM(num_line, den_line, num_samp, den_samp *mat.VecDense, points []*GroundPoint) []*GroundPoint {
|
|
var res []*GroundPoint
|
|
for _, p := range points {
|
|
var r GroundPoint
|
|
P := (p.P - rpc.latOffset) / rpc.latScale
|
|
L := (p.L - rpc.longOffset) / rpc.longScale
|
|
H := (p.H - rpc.heightOffset) / rpc.heightScale
|
|
r.Y = project(num_line, den_line, P, L, H)
|
|
r.X = project(num_samp, den_samp, P, L, H)
|
|
r.P = p.P
|
|
r.L = p.L
|
|
r.H = p.H
|
|
r.Y = (r.Y*rpc.lineScale + rpc.lineOffset)
|
|
r.X = (r.X*rpc.sampScale + rpc.sampOffset)
|
|
res = append(res, &r)
|
|
}
|
|
|
|
return res
|
|
}
|