rpc

2024-08-22 16:32:27 +08:00
parent ca3e91b1d8
commit 6f2cfa797a
9 changed files with 300 additions and 305 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -11,3 +11,4 @@ deployment
 build/go
 *.tif
 *.tiff
+dem/*
--- a/pkg/auxilary/image_time.go
+++ b/pkg/auxilary/image_time.go
@@ -15,10 +15,12 @@ type auxT struct {
 	Row  int
 }

-func NewImageTime() *ImageTime {
-	return &ImageTime{
+func NewImageTime(aps []*AuxPlatform) *ImageTime {
+	it := &ImageTime{
 		auxT: make([]*auxT, 0),
 	}
+	it.Extract(aps)
+	return it
 }

 func (it *ImageTime) Extract(aps []*AuxPlatform) {
--- a/pkg/calculator/camera.go
+++ b/pkg/calculator/camera.go
@@ -16,7 +16,7 @@ const (
 	MSSPixels   = float64(payload.MSS_PIXEL_WIDTH)
 	MSSCellSize = 12.8 // µm
 	CameraRoll  = 0.0  // 相机与卫星本体X轴的安装角度, degree FIXME: 安装矩阵应该由卫星方提供
-	CameraPitch = 0.5  // 相机与卫星本体Y轴的安装角度, degree
+	CameraPitch = 0.0  // 相机与卫星本体Y轴的安装角度, degree
 )

 // 计算过程使用PAN分辨率
--- a/pkg/calculator/intersection.go
+++ b/pkg/calculator/intersection.go
@@ -10,12 +10,13 @@ import (
 )

 type IntersectionPoint struct {
-	Lat float64
-	Lon float64
-	H   float64
+	X84, Y84, Z84 float64
+	Lat           float64
+	Lon           float64
+	H             float64
 }

-func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) (IntersectionPoint, error) {
+func Camera2GroundPoint(q Quaternion, satPos84 []float64, satTime time.Time, ucam int, groundH int) (IntersectionPoint, error) {
 	// alpha := FOV * math.Pi / 180.0
 	// alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
 	// direction := []float64{0, math.Tan(alpha), -1.3}
@@ -37,7 +38,7 @@ func IntersectionAttitude(q Quaternion, satPos84 []float64, satTime time.Time, u
 	dECEF := []float64{x, y, z}

 	// -------- 计算与地球表面的交点 --------
-	intersection, err := intersectWithEllipsoid(satPos84, dECEF)
+	intersection, err := SolveEllipticEquation(satPos84, dECEF, groundH)
 	if err != nil {
 		return IntersectionPoint{}, err
 	}
@@ -73,20 +74,23 @@ func IntersectionECI(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satT
 	dECEF := []float64{x, y, z}

 	// -------- 计算交点 --------}
-	intersection, err := intersectWithEllipsoid(satPos84, dECEF)
+	intersection, err := SolveEllipticEquation(satPos84, dECEF, 0)
 	if err != nil {
 		return IntersectionPoint{}, err
 	}

 	lat, lon, h := ECEFGeocentricToGeodetic(intersection[0], intersection[1], intersection[2])

-	return IntersectionPoint{Lat: lat, Lon: lon, H: h}, nil
+	return IntersectionPoint{
+		X84: intersection[0], Y84: intersection[1], Z84: intersection[2],
+		Lat: lat, Lon: lon, H: h,
+	}, nil
 }

 // 计算与椭球表面的交点
-func intersectWithEllipsoid(p0, d []float64) ([]float64, error) {
-	a2 := a * a
-	b2 := b * b
+func SolveEllipticEquation(p0, d []float64, groundH int) ([]float64, error) {
+	a2 := (a + float64(groundH)) * (a + float64(groundH))
+	b2 := (b + float64(groundH)) * (b + float64(groundH))

 	A := d[0]*d[0]/a2 + d[1]*d[1]/a2 + d[2]*d[2]/b2
 	B := 2 * (p0[0]*d[0]/a2 + p0[1]*d[1]/a2 + p0[2]*d[2]/b2)
--- a/pkg/calculator/orbit.go
+++ b/pkg/calculator/orbit.go
@@ -1,8 +1,6 @@
 package calculator

 import (
-	"math"
-
 	"gonum.org/v1/gonum/mat"
 	"gonum.org/v1/gonum/spatial/r3"
 )
@@ -28,38 +26,3 @@ func OrbitToECMatrix(pos, vec []float64) *mat.Dense {

 	return m
 }
-
-// IntersectionECEF 计算卫星与相机的交点，返回经纬度和高度
-// FIXME: 该计算方法有误，待修正
-func IntersectionECEF(Qsat2orbit Quaternion, satPos84, vec84 []float64, ucam int) (IntersectionPoint, error) {
-	alpha := FOV * math.Pi / 180.0
-	alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
-	direction := []float64{0, math.Tan(alpha), -1.3} // 卫星（相机）坐标系下CCD成像方向向量
-
-	// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
-	Rcam := CameraRotMatrix(CameraRoll*math.Pi/180.0, CameraPitch*math.Pi/180.0, 0)
-	var dCam mat.VecDense
-	dCam.MulVec(Rcam, mat.NewVecDense(3, direction))
-
-	// -------- 转到轨道坐标系 --------
-	Rsat2orbit := Qsat2orbit.ToRotationMatrix()
-	var r0 mat.VecDense
-	r0.MulVec(Rsat2orbit, &dCam)
-	dOrbit := r0.RawVector().Data
-
-	// -------- 转到ECEF坐标系 --------
-	Rorbit2ecef := OrbitToECMatrix(satPos84, vec84)
-	var r1 mat.VecDense
-	r1.MulVec(Rorbit2ecef, mat.NewVecDense(3, dOrbit))
-	dECEF := r1.RawVector().Data
-
-	// -------- 计算交点 --------}
-	intersection, err := intersectWithEllipsoid(satPos84, dECEF)
-	if err != nil {
-		return IntersectionPoint{}, err
-	}
-
-	lat, lon, h := ECEFToGeodetic(intersection[0], intersection[1], intersection[2])
-
-	return IntersectionPoint{Lat: lat, Lon: lon, H: h}, err
-}
--- a/pkg/producer/aux.go
+++ b/pkg/producer/aux.go
@@ -19,7 +19,6 @@ import (
 	"starwiz.cn/sjy01/image-proc/pkg/auxilary"
 	"starwiz.cn/sjy01/image-proc/pkg/calculator"
 	"starwiz.cn/sjy01/image-proc/pkg/config"
-	"starwiz.cn/sjy01/image-proc/pkg/payload"
 )

 func (r *Registrator) LoadAuxData() error {
@@ -30,10 +29,7 @@ func (r *Registrator) LoadAuxData() error {
 	gpsFile := strings.Replace(r.Params.AuxRawFile, ".AUX", ".gps.txt", 1)
 	r.AttQuaternion, _ = auxilary.StoreAtt(r.AuxPlatforms, attFile)
 	r.GPSs, _ = auxilary.StoreGPS(r.AuxPlatforms, gpsFile)
-
-	imgtime := auxilary.NewImageTime()
-	imgtime.Extract(r.AuxPlatforms)
-	imgtime.Print(100, 16)
+	r.ImageTime = auxilary.NewImageTime(r.AuxPlatforms)

 	return err
 }
@@ -79,44 +75,14 @@ func (r *Registrator) SceneImageTime(scene *Scene) (start, center, end time.Time
 	return
 }

-// FIXME: This function is not accurate enough.
+// FIXME: This function is not accurate enough. 四元数、成像时刻、GPS 等需要修改为插值获取
 func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.Point) {
-	startPosInAux, endPosInAux := r.SceneInAuxIndex(scene)
-
-	as := r.AuxPlatforms[startPosInAux]
-	ae := r.AuxPlatforms[endPosInAux]
-
-	startTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(as.UTCTimeSec), int64(as.Microsecond)*1000).UTC()
-	endTime := time.Unix(int64(auxilary.ReferenceTime2000)+int64(ae.UTCTimeSec), int64(ae.Microsecond)*1000).UTC()
-
-	startPos84 := []float64{as.W84PosX, as.W84PosY, as.W84PosZ}
-	endPos84 := []float64{ae.W84PosX, ae.W84PosY, ae.W84PosZ}
-
-	// FIXME: GPS 拟合效果不佳
-	// x0 := float64(r.auxHeads[startPosInAux].TimeSec) + float64(r.auxHeads[startPosInAux].TimeSecFrac)/10e6
-	// x1 := float64(r.auxHeads[endPosInAux].TimeSec) + float64(r.auxHeads[endPosInAux].TimeSecFrac)/10e6
-	// startPos84 = []float64{r.w84FitPre[0].Predict(x0), r.w84FitPre[1].Predict(x0), r.w84FitPre[2].Predict(x0)}
-	// endPos84 = []float64{r.w84FitPre[0].Predict(x1), r.w84FitPre[1].Predict(x1), r.w84FitPre[2].Predict(x1)}
-	// stepN := 2
-	// startPos84 = []float64{
-	// 	utils.InterpPolynomial(r.w84PositionTime, r.w84PositionX, x0, stepN),
-	// 	utils.InterpPolynomial(r.w84PositionTime, r.w84PositionY, x0, stepN),
-	// 	utils.InterpPolynomial(r.w84PositionTime, r.w84PositionZ, x0, stepN),
-	// }
-	// endPos84 = []float64{
-	// 	utils.InterpPolynomial(r.w84PositionTime, r.w84PositionX, x1, stepN),
-	// 	utils.InterpPolynomial(r.w84PositionTime, r.w84PositionY, x1, stepN),
-	// 	utils.InterpPolynomial(r.w84PositionTime, r.w84PositionZ, x1, stepN),
-	// }
-	// ------------------ 使用定姿态四元数计算图像边界 ------------------
 	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.IntersectionAttitude(Qsat2eci, startPos84, startTime, 0)
-	line0End, _ := calculator.IntersectionAttitude(Qsat2eci, startPos84, startTime, payload.PAN_PIXEL_WIDTH)

-	Qsat2eci = calculator.Quaternion{W: ae.QuatAttstarQ0, X: ae.QuatAttstarQ1, Y: ae.QuatAttstarQ2, Z: ae.QuatAttstarQ3}
-	lineNStart, _ := calculator.IntersectionAttitude(Qsat2eci, endPos84, endTime, 0)
-	lineNEnd, _ := calculator.IntersectionAttitude(Qsat2eci, endPos84, endTime, payload.PAN_PIXEL_WIDTH)
+	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})
@@ -166,16 +132,16 @@ func (r *Registrator) SetSceneBoundary(scene *Scene) (topLeft, bottomRight orb.P
 	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
+	// 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.SolveLeastSquares(); err != nil {
+	if err := rpc.RPC(); err != nil {
 		log.Error("calculate RPC failed: ", err)
 	} else {
 		rpc.SaveRpb()
@@ -207,3 +173,30 @@ func (r *Registrator) sceneOffsetInAuxIndex(scene *Scene, offset int) int {
 	}
 	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
+}
--- a/pkg/producer/grid_img.go
+++ b/pkg/producer/grid_img.go
@@ -0,0 +1,38 @@
+package producer
+
+type GridPoint struct {
+	Row, Col, H int
+}
+
+func gridImage(m, n, height, width, k, hmin, hmax int) []*GridPoint {
+	a := int(height / (m + 1))
+	var lines []int
+	for i := 0; i < m; i++ {
+		lines = append(lines, a*(i+1))
+	}
+
+	b := int(width / (n + 1))
+	var samples []int
+	for i := 0; i < n; i++ {
+		samples = append(samples, b*(i+1))
+	}
+
+	averageH := (hmax - hmin) / 2
+	dh := 500 // 高度差500m
+	var h []int
+	for i := 0; i < k; i++ {
+		h = append(h, averageH+(i-k/2)*dh)
+	}
+
+	var points []*GridPoint
+	for i := 0; i < len(lines); i++ {
+		for j := 0; j < len(samples); j++ {
+			for l := 0; l < len(h); l++ {
+				p := GridPoint{lines[i], samples[j], h[l]}
+				points = append(points, &p)
+			}
+		}
+	}
+
+	return points
+}
--- a/pkg/producer/image_registration.go
+++ b/pkg/producer/image_registration.go
@@ -64,6 +64,7 @@ type Registrator struct {

 	GPSs          *auxilary.GPSs
 	AttQuaternion *auxilary.Attitudes
+	ImageTime     *auxilary.ImageTime

 	report Report
 }
--- a/pkg/producer/rpc.go
+++ b/pkg/producer/rpc.go
@@ -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
 }