diff --git a/pkg/auxilary/att.go b/pkg/auxilary/att.go index 63aeda5..206202b 100644 --- a/pkg/auxilary/att.go +++ b/pkg/auxilary/att.go @@ -112,7 +112,7 @@ func ExtractAttitude(aps []*AuxPlatform) *Attitudes { sec, microsec = ap.UTCTimeSec, ap.Microsecond att := Attitude{ UTCTimestampSec: float64(sec) + float64(ReferenceTime2000) + - float64(transfromGPSandAttMicrosec(microsec))/1e6, + float64((microsec))/1e6, Q0: ap.QuatAttstarQ0, Q1: ap.QuatAttstarQ1, Q2: ap.QuatAttstarQ2, diff --git a/pkg/producer/aux.go b/pkg/producer/aux.go index ac8b331..069c329 100644 --- a/pkg/producer/aux.go +++ b/pkg/producer/aux.go @@ -142,8 +142,6 @@ func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.P rpc := NewRPC(r, scene, strings.Replace(scene.Tiff, ".tiff", ".rpb", 1)) if err := rpc.RPC(); err != nil { log.Error("calculate RPC failed: ", err) - } else { - rpc.SaveRpb() } return diff --git a/pkg/producer/grid_img.go b/pkg/producer/grid_img.go index 3189866..4383fce 100644 --- a/pkg/producer/grid_img.go +++ b/pkg/producer/grid_img.go @@ -14,7 +14,7 @@ func gridImage2(m, n, height, width, k, hmin, hmax int) []*GridPoint { b := int((width) / (n)) var samples []int - for i := 1; i <= n; i++ { + for i := 0; i <= n; i++ { samples = append(samples, b*i) } @@ -22,7 +22,7 @@ func gridImage2(m, n, height, width, k, hmin, hmax int) []*GridPoint { hmin = hmin - 500 dh := (hmax - hmin) / (k) var h []int - for i := 1; i <= k; i++ { + for i := 0; i <= k; i++ { h = append(h, hmin+dh*i) } diff --git a/pkg/producer/rpc.go b/pkg/producer/rpc.go index 7fe9d0a..c0c4e75 100644 --- a/pkg/producer/rpc.go +++ b/pkg/producer/rpc.go @@ -7,7 +7,6 @@ import ( "os" "strings" - "github.com/duke-git/lancet/v2/mathutil" "github.com/paulmach/orb" "github.com/paulmach/orb/geojson" @@ -25,12 +24,10 @@ type RPC struct { LineCoef RPCModel SampleCoef RPCModel - // GroundPoints []*GroundPoint - minLat, maxLat, minLon, maxLon float64 - minH, maxH float64 - GCPs []GroundPoint - elevationLayer int - gridsize int + minH, maxH float64 + GCPs []*GroundPoint + elevationLayer int + gridsize int scene *Scene registrator *Registrator @@ -58,7 +55,7 @@ type RPCModel struct { // rational polynomial coeffients func NewRPC(r *Registrator, scene *Scene, rpb string) *RPC { rpc := RPC{ - elevationLayer: 9, + elevationLayer: 4, gridsize: 19, registrator: r, scene: scene, @@ -75,31 +72,6 @@ func NewRPC(r *Registrator, scene *Scene, rpb string) *RPC { func (rpc *RPC) init() { rpc.minH = 9999.0 rpc.maxH = -9999.0 - rpc.minLat = 90.0 - rpc.maxLat = -90.0 - rpc.minLon = 180.0 - rpc.maxLon = -180.0 - - rpc.minLat = mathutil.Min(rpc.scene.Meta.Corners.LowerLeft.Latitude, - rpc.scene.Meta.Corners.LowerRight.Latitude, - rpc.scene.Meta.Corners.UpperLeft.Latitude, - rpc.scene.Meta.Corners.UpperRight.Latitude) - rpc.maxLat = mathutil.Max(rpc.scene.Meta.Corners.LowerLeft.Latitude, - rpc.scene.Meta.Corners.LowerRight.Latitude, - rpc.scene.Meta.Corners.UpperLeft.Latitude, - rpc.scene.Meta.Corners.UpperRight.Latitude) - - rpc.minLon = mathutil.Min(rpc.scene.Meta.Corners.LowerLeft.Longitude, - rpc.scene.Meta.Corners.LowerRight.Longitude, - rpc.scene.Meta.Corners.UpperLeft.Longitude, - rpc.scene.Meta.Corners.UpperRight.Longitude) - rpc.maxLon = mathutil.Max(rpc.scene.Meta.Corners.LowerLeft.Longitude, - rpc.scene.Meta.Corners.LowerRight.Longitude, - rpc.scene.Meta.Corners.UpperLeft.Longitude, - rpc.scene.Meta.Corners.UpperRight.Longitude) - - rpc.latOffset = (rpc.minLat + rpc.maxLat) / 2.0 - rpc.longOffset = (rpc.minLon + rpc.maxLon) / 2.0 rpc.minH, rpc.maxH = dem.Dem1KmLT.MinMaxElevationInRect( rpc.scene.Meta.Corners.UpperLeft.Longitude, @@ -116,7 +88,6 @@ func (rpc *RPC) init() { rpc.maxH = 0.0 } - rpc.heightOffset = (rpc.minH + rpc.maxH) / 2.0 } // 虚拟控制点 @@ -129,7 +100,7 @@ func (rpc *RPC) generateVirtualGCP() { for _, p := range points { p84 := rpc.registrator.calculateLatLonH(rpc.scene, p.Row, p.Col, p.H) - rpc.GCPs = append(rpc.GCPs, GroundPoint{ + rpc.GCPs = append(rpc.GCPs, &GroundPoint{ P: p84.Lat, L: p84.Lon, H: p84.H, @@ -137,13 +108,17 @@ func (rpc *RPC) generateVirtualGCP() { 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) - rpc.saveGCP() rowVec := mat.NewVecDense(n, nil) colVec := mat.NewVecDense(n, nil) @@ -168,32 +143,31 @@ func (rpc *RPC) RPC() error { rpc.sampScale = float64(rpc.scene.Width) rowVec = normalize2(rowVec, rpc.lineOffset, rpc.lineScale) colVec = normalize2(colVec, rpc.sampOffset, rpc.sampScale) - // rowVec.ScaleVec(1.0/rpc.lineScale, rowVec) - // colVec.ScaleVec(1.0/rpc.sampScale, colVec) - // rowVec, rpc.lineOffset, rpc.lineScale = normalize(rowVec) - // colVec, rpc.sampOffset, rpc.sampScale = normalize(colVec) latVec, rpc.latOffset, rpc.latScale = normalize(latVec) lonVec, rpc.longOffset, rpc.longScale = normalize(lonVec) heightVec, rpc.heightOffset, rpc.heightScale = normalize(heightVec) - // rpc.latOffset, rpc.latScale = (rpc.maxLat+rpc.minLat)/2.0, (rpc.maxLat - rpc.minLat) - // rpc.longOffset, rpc.longScale = (rpc.maxLon+rpc.minLon)/2.0, (rpc.maxLon - rpc.minLon) - // rpc.heightOffset, rpc.heightScale = (rpc.maxH+rpc.minH)/2.0+500.0, 500.0 - rpc.saveVec(strings.Replace(rpc.scene.Tiff, ".tiff", ".vec_norm.txt", -1), rowVec, colVec, latVec, lonVec, heightVec) // 设计矩阵 B = [ 20个分子系数 19个分母系数 ] - B := buildDesignMatrix(rowVec, latVec, lonVec, heightVec) // x = (B^T * B)^-1 * B^T * l, 其中 x = [a1..a20 b2..b20]^T // 行参数 + B := buildDesignMatrix(rowVec, latVec, lonVec, heightVec) J, err := SolveNormalEquation(B, rowVec) if err != nil { return err } + // 列参数 + D := buildDesignMatrix(colVec, latVec, lonVec, heightVec) + K, err := SolveNormalEquation(D, colVec) + if err != nil { + return err + } + for i := 0; i < 20; i++ { rpc.LineCoef.NumCoefficients[i] = J[i] } @@ -202,13 +176,6 @@ func (rpc *RPC) RPC() error { rpc.LineCoef.DenCoefficients[i-19] = J[i] } - // 列参数 - D := buildDesignMatrix(colVec, latVec, lonVec, heightVec) - K, err := SolveNormalEquation(D, colVec) - if err != nil { - return err - } - for i := 0; i < 20; i++ { rpc.SampleCoef.NumCoefficients[i] = K[i] } @@ -216,6 +183,50 @@ func (rpc *RPC) RPC() error { for i := 20; i < 39; i++ { rpc.SampleCoef.DenCoefficients[i-19] = K[i] } + + nameRPB := strings.Replace(rpc.scene.Tiff, ".tiff", ".sep.rpb", -1) + rpc.saveRPB(nameRPB) + + r, c := B.Dims() + M0 := mat.NewDense(r, c, nil) + var BM0, M0D, M mat.Dense + BM0.Augment(B, M0) + M0D.Augment(M0, D) + M.Stack(&BM0, &M0D) + var L mat.Dense + L.Stack(rowVec, colVec) + coeffs, err := SolveNormalEquation(&M, mat.NewVecDense(rowVec.Len()+colVec.Len(), mat.Col(nil, 0, &L))) + if err != nil { + return err + } + + for i := 0; i < 20; i++ { + rpc.LineCoef.NumCoefficients[i] = coeffs[i] + } + rpc.LineCoef.DenCoefficients[0] = 1.0 + for i := 20; i < 39; i++ { + rpc.LineCoef.DenCoefficients[i-19] = coeffs[i] + } + for i := 39; i < 59; i++ { + rpc.SampleCoef.NumCoefficients[i-39] = coeffs[i] + } + rpc.SampleCoef.DenCoefficients[0] = 1.0 + for i := 59; i < 78; i++ { + rpc.SampleCoef.DenCoefficients[i-58] = coeffs[i] + } + 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.SampleCoef.NumCoefficients[:]), + mat.NewVecDense(20, rpc.SampleCoef.DenCoefficients[:]), + rpc.GCPs, + ) + name := strings.Replace(rpc.scene.Tiff, ".tiff", ".gcp_proj.txt", -1) + rpc.saveGCP2(projectedPoints, name) + return nil } @@ -292,19 +303,6 @@ func buildDesignMatrix(vec, latVec, lonVec, heightVec *mat.VecDense) *mat.Dense return B } -// 计算 RPC 正则化参数 -func (rpc *RPC) calculateRegularizedParams() { - rpc.lineOffset = float64(rpc.scene.Height) / 2.0 - rpc.sampOffset = float64(rpc.scene.Width) / 2.0 - rpc.lineScale = float64(rpc.scene.Height) - rpc.sampScale = float64(rpc.scene.Width) - - // rpc.heightScale = math.Max(math.Abs(rpc.minH-rpc.heightOffset), math.Abs(rpc.maxH-rpc.heightOffset)) - rpc.heightScale = 500.0 - rpc.latScale = math.Max(math.Abs(rpc.minLat-rpc.latOffset), math.Abs(rpc.maxLat-rpc.latOffset)) - rpc.longScale = math.Max(math.Abs(rpc.minLon-rpc.longOffset), math.Abs(rpc.maxLon-rpc.longOffset)) -} - // SolveNormalEquation 使用正规方程法求解最小二乘问题 func SolveNormalEquation(A *mat.Dense, b *mat.VecDense) ([]float64, error) { var At mat.Dense @@ -420,10 +418,10 @@ END; return model } -func (rpc *RPC) SaveRpb() error { - log.Infof("save RPC model to %s", rpc.rpb) +func (rpc *RPC) saveRPB(name string) error { + log.Infof("save RPC model to %s", name) model := rpc.Output() - f, err := os.Create(rpc.rpb) + f, err := os.Create(name) if err != nil { log.Errorf("Failed to create RPC model file: %v", err) return err @@ -437,8 +435,7 @@ func (rpc *RPC) SaveRpb() error { return err } -func (rpc *RPC) saveGCP() error { - name := strings.Replace(rpc.scene.Tiff, ".tiff", ".gcp.geojson", -1) +func (rpc *RPC) saveGCP(gcps []*GroundPoint, name string) error { log.Infof("save gcp to %s", name) f, err := os.Create(name) if err != nil { @@ -448,7 +445,7 @@ func (rpc *RPC) saveGCP() error { defer f.Close() var gcp geojson.FeatureCollection - for _, p := range rpc.GCPs { + for _, p := range gcps { point := orb.Point{p.L, p.P} feature := geojson.NewFeature(point) feature.Properties = map[string]interface{}{ @@ -465,6 +462,23 @@ func (rpc *RPC) saveGCP() error { return nil } +func (rpc *RPC) saveGCP2(gcps []*GroundPoint, name string) error { + 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 { f, err := os.Create(name) @@ -480,3 +494,60 @@ func (rpc *RPC) saveVec(name string, rowVec, colVec, latVec, lonVec, heightVec * } 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 + r.Y = rpc.project(num_line, den_line, p.P, p.L, p.H) + r.Y = r.Y*rpc.lineScale + rpc.lineOffset + r.X = rpc.project(num_samp, den_samp, p.P, p.L, p.H) + r.X = r.X*rpc.sampScale + rpc.sampOffset + r.P = p.P + r.L = p.L + r.H = p.H + res = append(res, &r) + } + + return res +} + +func (rpc *RPC) localize(num, den *mat.VecDense, row, col float64) (P, L, H float64) { + + return +} + +func (rpc *RPC) project(num, den *mat.VecDense, P, L, H float64) (v float64) { + v = rpc.applyPoly(num, P, L, H) / rpc.applyPoly(den, P, L, H) + return v +} + +func (rpc *RPC) applyPoly(poly *mat.VecDense, P, L, H float64) (v float64) { + P = (P - rpc.latOffset) / rpc.latScale + L = (L - rpc.longOffset) / rpc.longScale + H = (H - rpc.heightOffset) / rpc.heightScale + + v = 0.0 + v += poly.AtVec(0) + v += poly.AtVec(1) * L + v += poly.AtVec(2) * P + v += poly.AtVec(3) * H + v += poly.AtVec(4) * L * P + v += poly.AtVec(5) * L * H + v += poly.AtVec(6) * P * H + v += poly.AtVec(7) * L * L + v += poly.AtVec(8) * P * P + v += poly.AtVec(9) * H * H + v += poly.AtVec(10) * P * L * H + v += poly.AtVec(11) * L * L * L + v += poly.AtVec(12) * L * P * P + v += poly.AtVec(13) * L * H * H + v += poly.AtVec(14) * L * L * P + v += poly.AtVec(15) * P * P * P + v += poly.AtVec(16) * P * H * H + v += poly.AtVec(17) * L * L * H + v += poly.AtVec(18) * P * P * H + v += poly.AtVec(19) * H * H * H + return v +}