This commit is contained in:
nuknal
2024-08-22 16:32:27 +08:00
parent ca3e91b1d8
commit 6f2cfa797a
9 changed files with 300 additions and 305 deletions

View File

@@ -4,15 +4,12 @@ import (
"fmt"
"math"
"os"
"time"
"github.com/duke-git/lancet/v2/mathutil"
"github.com/duke-git/lancet/v2/slice"
log "github.com/sirupsen/logrus"
"gonum.org/v1/gonum/mat"
"starwiz.cn/sjy01/image-proc/pkg/auxilary"
"starwiz.cn/sjy01/image-proc/pkg/calculator"
"starwiz.cn/sjy01/image-proc/pkg/dem"
)
@@ -66,10 +63,7 @@ func NewRPC(r *Registrator, scene *Scene, rpb string) *RPC {
}
log.Info("start RPC initialization for scene: ", scene.Tiff)
rpc.init()
rpc.generateVirtualGCP()
rpc.regularizeGCPs()
return &rpc
}
@@ -82,35 +76,6 @@ func (rpc *RPC) init() {
rpc.maxLat = -90.0
rpc.minLon = 180.0
rpc.maxLon = -180.0
// for row := 0; row < rpc.scene.Height; row++ {
// for col := 0; col < rpc.scene.Width; col++ {
// p84 := rpc.calculateLatLonH(row, col)
// rpc.GroundPoints = append(rpc.GroundPoints, &p84)
// rpc.heightOffset += p84.H
// rpc.latOffset += p84.P
// rpc.longOffset += p84.L
// if p84.H < rpc.minH {
// rpc.minH = p84.H
// }
// if p84.H > rpc.maxH {
// rpc.maxH = p84.H
// }
// if p84.P < rpc.minLat {
// rpc.minLat = p84.P
// }
// if p84.P > rpc.maxLat {
// rpc.maxLat = p84.P
// }
// if p84.L < rpc.minLon {
// rpc.minLon = p84.L
// }
// if p84.L > rpc.maxLon {
// rpc.maxLon = p84.L
// }
// }
// }
rpc.minLat = mathutil.Min(rpc.scene.Meta.Corners.LowerLeft.Latitude,
rpc.scene.Meta.Corners.LowerRight.Latitude,
@@ -154,39 +119,167 @@ func (rpc *RPC) init() {
rpc.scene.Meta.Corners.LowerRight.Latitude,
)
rpc.heightOffset = (rpc.minH + rpc.maxH) / 2.0
rpc.calculateRegularizedParams()
}
// 虚拟控制点
func (rpc *RPC) generateVirtualGCP() {
log.Info("Generating virtual GCPs...")
// elevations := []float64{rpc.minH, (rpc.minH + rpc.maxH) / 2.0, rpc.maxH}
deltaRow := float64(rpc.scene.Height) / float64(rpc.gridsize-1) // 像素平面Y方向步长
deltaCol := float64(rpc.scene.Width) / float64(rpc.gridsize-1) // 像素平面X方向步长
points := gridImage(rpc.gridsize, rpc.gridsize,
rpc.scene.Height, rpc.scene.Width,
rpc.elevationLayer, int(rpc.minH), int(rpc.maxH))
for i := 0; i < rpc.gridsize; i++ {
for j := 0; j < rpc.gridsize; j++ {
imagePoint := ImagePoint{
Y: deltaRow * float64(i),
X: deltaCol * float64(j),
}
groudPoint84 := rpc.calculateLatLonH(int(imagePoint.Y), int(imagePoint.X))
rpc.GCPs = append(rpc.GCPs, groudPoint84)
}
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),
})
}
}
func (rpc *RPC) regularizeGCPs() {
log.Info("Regularizing virtual GCPs...")
for i := range rpc.GCPs {
gcp := &rpc.GCPs[i]
gcp.P = (gcp.P - rpc.latOffset) / rpc.latScale
gcp.L = (gcp.L - rpc.longOffset) / rpc.longScale
gcp.H = (gcp.H - rpc.heightOffset) / rpc.heightScale
gcp.Y = (gcp.Y - rpc.lineOffset) / rpc.lineScale
gcp.X = (gcp.X - rpc.sampOffset) / rpc.sampScale
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)
}
rowVec, rowOff, rowScale := normalize(rowVec)
colVec, colOff, colScale := normalize(colVec)
latVec, latOff, latScale := normalize(latVec)
lonVec, lonOff, lonScale := normalize(lonVec)
heightVec, heightOff, heightScale := normalize(heightVec)
rpc.lineOffset = rowOff
rpc.lineScale = rowScale
rpc.sampOffset = colOff
rpc.sampScale = colScale
rpc.latOffset = latOff
rpc.latScale = latScale
rpc.longOffset = lonOff
rpc.longScale = lonScale
rpc.heightOffset = heightOff
rpc.heightScale = heightScale
// fmt.Printf("lineOffset: %f, lineScale: %f\n", rpc.lineOffset, rpc.lineScale)
// fmt.Printf("sampOffset: %f, sampScale: %f\n", rpc.sampOffset, rpc.sampScale)
// fmt.Printf("latOffset: %f, latScale: %f\n", rpc.latOffset, rpc.latScale)
// fmt.Printf("longOffset: %f, longScale: %f\n", rpc.longOffset, rpc.longScale)
// fmt.Printf("heightOffset: %f, heightScale: %f\n", rpc.heightOffset, rpc.heightScale)
// fmt.Printf("X0: %f, Y0: %f\n", colVec.At(111, 0), rowVec.At(111, 0))
// fmt.Printf("lat0: %f, lon0: %f, height0: %f\n", latVec.At(111, 0), lonVec.At(111, 0), heightVec.At(111, 0))
// 设计矩阵 B = [ 20个分子系数 19个分母系数 ]
B := buildDesignMatrix(latVec, lonVec, heightVec)
// x = (B^T * B)^-1 * B^T * l, 其中 x = [a1..a20 b2..b20]^T
// 行参数
J, err := SolveNormalEquation(B, rowVec)
if err != nil {
return err
}
for i := 0; i < 20; i++ {
rpc.LineCoef.NumCoefficients[i] = J[i]
}
rpc.LineCoef.DenCoefficients[0] = 1.0
for i := 20; i < 39; i++ {
rpc.LineCoef.DenCoefficients[i-19] = J[i]
}
// 列参数
K, err := SolveNormalEquation(B, colVec)
if err != nil {
return err
}
for i := 0; i < 20; i++ {
rpc.SampleCoef.NumCoefficients[i] = K[i]
}
rpc.SampleCoef.DenCoefficients[0] = 1.0
for i := 20; i < 39; i++ {
rpc.SampleCoef.DenCoefficients[i-19] = K[i]
}
return nil
}
func normalize(v *mat.VecDense) (*mat.VecDense, float64, float64) {
var vOff, vScale float64
vOff = mat.Sum(v) / float64(v.Len())
vScale = math.Max(math.Abs(mat.Max(v)-vOff), math.Abs(mat.Min(v)-vOff))
for i := 0; i < v.Len(); i++ {
v.SetVec(i, (v.AtVec(i)-vOff)/vScale)
}
return v, vOff, vScale
}
func buildDesignMatrix(latVec, lonVec, heightVec *mat.VecDense) *mat.Dense {
n := latVec.Len()
// 设计矩阵 B = [ 20个分子系数 19个分母系数 ]
B := mat.NewDense(n, 39, nil)
for i := 0; i < n; i++ {
P := latVec.AtVec(i)
L := lonVec.AtVec(i)
H := heightVec.AtVec(i)
B.Set(i, 0, 1)
B.Set(i, 1, L)
B.Set(i, 2, P)
B.Set(i, 3, H)
B.Set(i, 4, L*P)
B.Set(i, 5, L*H)
B.Set(i, 6, P*H)
B.Set(i, 7, L*L)
B.Set(i, 8, P*P)
B.Set(i, 9, H*H)
B.Set(i, 10, P*L*H)
B.Set(i, 11, L*L*L)
B.Set(i, 12, L*P*P)
B.Set(i, 13, L*H*H)
B.Set(i, 14, L*L*P)
B.Set(i, 15, P*P*P)
B.Set(i, 16, P*H*H)
B.Set(i, 17, L*L*H)
B.Set(i, 18, P*P*H)
B.Set(i, 19, H*H*H)
B.Set(i, 20, -L)
B.Set(i, 21, -P)
B.Set(i, 22, -H)
B.Set(i, 23, -L*P)
B.Set(i, 24, -L*H)
B.Set(i, 25, -P*H)
B.Set(i, 26, -L*L)
B.Set(i, 27, -P*P)
B.Set(i, 28, -H*H)
B.Set(i, 29, -P*L*H)
B.Set(i, 30, -L*L*L)
B.Set(i, 31, -L*P*P)
B.Set(i, 32, -L*H*H)
B.Set(i, 33, -L*L*P)
B.Set(i, 34, -P*P*P)
B.Set(i, 35, -P*H*H)
B.Set(i, 36, -L*L*H)
B.Set(i, 37, -P*P*H)
B.Set(i, 38, -H*H*H)
}
return B
}
// 计算 RPC 正则化参数
@@ -202,131 +295,6 @@ func (rpc *RPC) calculateRegularizedParams() {
rpc.longScale = math.Max(math.Abs(rpc.minLon-rpc.longOffset), math.Abs(rpc.maxLon-rpc.longOffset))
}
func (rpc *RPC) calculateLatLonH(row, col int) GroundPoint {
auxIdx := rpc.registrator.sceneOffsetInAuxIndex(rpc.scene, row)
as := rpc.registrator.AuxPlatforms[auxIdx]
t := time.Unix(int64(auxilary.ReferenceTime2000)+int64(as.UTCTimeSec), int64(as.Microsecond)*1000).UTC()
p84 := []float64{as.W84PosX, as.W84PosY, as.W84PosZ}
Qsat2eci := calculator.Quaternion{W: as.QuatAttstarQ0, X: as.QuatAttstarQ1, Y: as.QuatAttstarQ2, Z: as.QuatAttstarQ3}
groudPoint84, _ := calculator.IntersectionAttitude(Qsat2eci, p84, t, col)
elv := dem.Dem1KmLT.Elevation(groudPoint84.Lon, groudPoint84.Lat)
if elv < 0.0 {
elv = 0.0
}
return GroundPoint{
P: groudPoint84.Lat,
L: groudPoint84.Lon,
H: float64(elv),
Y: float64(row),
X: float64(col),
}
}
// BuildDesignMatrix 构建设计矩阵
func (rpc *RPC) buildDesignMatrix(removeConstant bool) *mat.Dense {
n := len(rpc.GCPs)
numCols := 20
if removeConstant {
numCols = 19
}
A := mat.NewDense(n, numCols, nil)
for i, pt := range rpc.GCPs {
idx := 0
if !removeConstant {
A.Set(i, 0, 1)
} else {
idx = -1
}
A.Set(i, idx+1, pt.L)
A.Set(i, idx+2, pt.P)
A.Set(i, idx+3, pt.H)
A.Set(i, idx+4, pt.L*pt.P)
A.Set(i, idx+5, pt.L*pt.H)
A.Set(i, idx+6, pt.P*pt.H)
A.Set(i, idx+7, pt.L*pt.L)
A.Set(i, idx+8, pt.P*pt.P)
A.Set(i, idx+9, pt.H*pt.H)
A.Set(i, idx+10, pt.P*pt.L*pt.H)
A.Set(i, idx+11, pt.L*pt.L*pt.L)
A.Set(i, idx+12, pt.L*pt.P*pt.P)
A.Set(i, idx+13, pt.L*pt.H*pt.H)
A.Set(i, idx+14, pt.L*pt.L*pt.P)
A.Set(i, idx+15, pt.P*pt.P*pt.P)
A.Set(i, idx+16, pt.P*pt.H*pt.H)
A.Set(i, idx+17, pt.L*pt.L*pt.H)
A.Set(i, idx+18, pt.P*pt.P*pt.H)
A.Set(i, idx+19, pt.H*pt.H*pt.H)
}
return A
}
// SolveLeastSquares 使用最小二乘法求解RPC模型的系数
func (rpc *RPC) SolveLeastSquares() error {
log.Info("Solving least squares...")
n := len(rpc.GCPs)
rowVec := mat.NewVecDense(n, nil)
colVec := mat.NewVecDense(n, nil)
for i, ip := range rpc.GCPs {
rowVec.SetVec(i, ip.Y)
colVec.SetVec(i, ip.X)
}
var rowNum, rowDen, colNum, colDen []float64
var err error
// 使用正规方程法求解分子系数
A := rpc.buildDesignMatrix(false)
log.Debug("solving normal equation for linenumcoef...")
r, c := A.Dims()
log.Debug("A:", r, c)
// fmt.Printf("设计矩阵:\n%v\n", mat.Formatted(A, mat.Prefix(" "), mat.Excerpt(0)))
rowNum, err = SolveNormalEquation(A, rowVec)
if err != nil {
return err
}
log.Debug("solving normal equation for samplenumcoef...")
colNum, err = SolveNormalEquation(A, colVec)
if err != nil {
return err
}
// 对分母系数求解,添加固定项
rowDen = make([]float64, 20)
colDen = make([]float64, 20)
rowDen[0] = 1.0
colDen[0] = 1.0
A_reduced := rpc.buildDesignMatrix(true)
log.Debug("solving normal equation for linedemcoef...")
rowDenCoeffs, err := SolveNormalEquation(A_reduced, rowVec)
if err != nil {
return err
}
copy(rowDen[1:], rowDenCoeffs)
log.Debug("solving normal equation for sampledencoef...")
colDenCoeffs, err := SolveNormalEquation(A_reduced, colVec)
if err != nil {
return err
}
copy(colDen[1:], colDenCoeffs)
for i := 0; i < 20; i++ {
rpc.LineCoef.NumCoefficients[i] = rowNum[i]
rpc.LineCoef.DenCoefficients[i] = rowDen[i]
rpc.SampleCoef.NumCoefficients[i] = colNum[i]
rpc.SampleCoef.DenCoefficients[i] = colDen[i]
}
return nil
}
// SolveNormalEquation 使用正规方程法求解最小二乘问题
func SolveNormalEquation(A *mat.Dense, b *mat.VecDense) ([]float64, error) {
var At mat.Dense
@@ -336,7 +304,33 @@ func SolveNormalEquation(A *mat.Dense, b *mat.VecDense) ([]float64, error) {
var AtInv mat.Dense
err := AtInv.Inverse(&At)
if err != nil {
return nil, fmt.Errorf("矩阵不可逆: %v", err)
// 计算矩阵的 SVD 分解
var svd mat.SVD
ok := svd.Factorize(&At, mat.SVDThin)
if !ok {
fmt.Println("SVD 分解失败")
return nil, fmt.Errorf("设计矩阵不可逆, SVD 分解失败: %v", err)
}
// 获取 U、Σ 和 V^T
var u, v mat.Dense
svd.UTo(&u)
svd.VTo(&v)
sigma := svd.Values(nil)
// 计算 Σ^+ (Sigma pseudo-inverse)
m, n := u.Dims()
sigmaInv := mat.NewDense(n, m, nil)
for i := 0; i < len(sigma); i++ {
if sigma[i] > 1e-10 { // 避免除以零
sigmaInv.Set(i, i, 1/sigma[i])
}
}
// 计算 V * Σ^+ * U^T
var temp mat.Dense
temp.Mul(&v, sigmaInv)
AtInv.Mul(&temp, u.T())
}
var Atb mat.VecDense
@@ -352,15 +346,15 @@ func (rpc *RPC) Output() string {
var lineNumCoef, lineDenCoef, sampNumCoef, sampDenCoef string
for i := 0; i < 20; i++ {
if i < 19 {
lineNumCoef += fmt.Sprintf("%f,\n", rpc.LineCoef.NumCoefficients[i])
lineDenCoef += fmt.Sprintf("%f,\n", rpc.LineCoef.DenCoefficients[i])
sampNumCoef += fmt.Sprintf("%f,\n", rpc.SampleCoef.NumCoefficients[i])
sampDenCoef += fmt.Sprintf("%f,\n", rpc.SampleCoef.DenCoefficients[i])
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.SampleCoef.NumCoefficients[i])
sampDenCoef += fmt.Sprintf("\t\t%.15e,\n", rpc.SampleCoef.DenCoefficients[i])
} else {
lineNumCoef += fmt.Sprintf("%f", rpc.LineCoef.NumCoefficients[i])
lineDenCoef += fmt.Sprintf("%f", rpc.LineCoef.DenCoefficients[i])
sampNumCoef += fmt.Sprintf("%f", rpc.SampleCoef.NumCoefficients[i])
sampDenCoef += fmt.Sprintf("%f", rpc.SampleCoef.DenCoefficients[i])
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.SampleCoef.NumCoefficients[i])
sampDenCoef += fmt.Sprintf("\t\t%.15e", rpc.SampleCoef.DenCoefficients[i])
}
}
@@ -370,24 +364,24 @@ SpecId = "";
BEGIN_GROUP = IMAGE
errBias = 1.0;
errRand = 0.0;
lineOffset = %f;
sampOffset = %f;
latOffset = %f;
longOffset = %f;
heightOffset = %f;
lineScale = %f;
sampScale = %f;
latScale = %f;
longScale = %f;
heightScale = %f;
lineOffset = %.8f;
sampOffset = %.8f;
latOffset = %.8f;
longOffset = %.8f;
heightOffset = %.8f;
lineScale = %.8f;
sampScale = %.8f;
latScale = %.8f;
longScale = %.8f;
heightScale = %.8f;
lineNumCoef = (
%s);
%s);
lineDenCoef = (
%s);
%s);
sampNumCoef = (
%s);
%s);
sampDenCoef = (
%s);
%s);
END_GROUP = IMAGE
END;
`,
@@ -400,7 +394,7 @@ END;
}
func (rpc *RPC) SaveRpb() error {
log.Infof("Saving RPC model to %s...", rpc.rpb)
log.Infof("save RPC model to %s", rpc.rpb)
model := rpc.Output()
f, err := os.Create(rpc.rpb)
if err != nil {
@@ -408,7 +402,6 @@ func (rpc *RPC) SaveRpb() error {
return err
}
defer f.Close()
fmt.Println(model)
_, err = f.WriteString(model)
return err
}