相机偏置矩阵

pkg/calculator/camera.go

@@ -15,8 +15,9 @@ const (
 	PANCellSize = 3.2 // µm
 	MSSPixels   = float64(payload.MSS_PIXEL_WIDTH)
 	MSSCellSize = 12.8   // µm
-	CameraRoll  = 0.0  // 相机与卫星本体X轴的安装角度, degree FIXME: 安装矩阵应该由卫星方提供
+	CameraRoll  = -0.010 // 相机与卫星本体X轴的安装角度, degree FIXME: 安装矩阵应该由卫星方提供
-	CameraPitch = 0.0  // 相机与卫星本体Y轴的安装角度, degree
+	CameraPitch = 0.183  // 相机与卫星本体Y轴的安装角度, degree
+	CameraYaw   = 0.012  // 相机与卫星本体Z轴的安装角度, degree
 )
 
 // 计算过程使用PAN分辨率
@@ -80,9 +81,17 @@ func CameraRotMatrix(phi, theta, psi float64) *mat.Dense {
 		-math.Sin(theta), 0, math.Cos(theta),
 	})
 
+	Rz := mat.NewDense(3, 3, []float64{
+		math.Cos(psi), -math.Sin(psi), 0,
+		math.Sin(psi), math.Cos(psi), 0,
+		0, 0, 1,
+	})
+
 	// R = Rz * Ry * Rx
 	RyRx := mat.NewDense(3, 3, nil)
 	RyRx.Mul(Ry, Rx)
+	Rzxy := mat.NewDense(3, 3, nil)
+	Rzxy.Mul(Rz, RyRx)
 
-	return RyRx
+	return Rzxy
 }

pkg/calculator/intersection.go

@@ -23,7 +23,7 @@ func Camera2GroundPoint(q Quaternion, satPos84 []float64, satTime time.Time, uca
 	direction := CameraDirectionVec(0, float64(ucam))
 
 	// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
-	Rcam := CameraRotMatrix(CameraRoll*math.Pi/180.0, CameraPitch*math.Pi/180.0, 0)
+	Rcam := CameraRotMatrix(CameraRoll*math.Pi/180.0, CameraPitch*math.Pi/180.0, CameraYaw*math.Pi/180.0)
 	var dCam mat.VecDense
 	dCam.MulVec(Rcam, mat.NewVecDense(3, direction))
 

pkg/producer/rpc.go

@@ -157,7 +157,7 @@ func (rpc *RPC) RPC() error {
 	// B := setupSystemOfEquations(rowVec, latVec, lonVec, heightVec)
 	// J, err := SolveNormalEquation(B, rowVec)
 	log.Println("solving row coefficients")
-	J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec)
+	J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec, SolveMethodNelderMead)
 	if err != nil {
 		return err
 	}
@@ -166,7 +166,7 @@ func (rpc *RPC) RPC() error {
 	// D := setupSystemOfEquations(colVec, latVec, lonVec, heightVec)
 	// K, err := SolveNormalEquation(D, colVec)
 	log.Println("solving col coefficients")
-	K, err := solveCoefficients(colVec, latVec, lonVec, heightVec)
+	K, err := solveCoefficients(colVec, latVec, lonVec, heightVec, SolveMethodNelderMead)
 	if err != nil {
 		return err
 	}

pkg/producer/rpc_helper.go

@@ -14,6 +14,14 @@ type VX struct {
 	x mat.VecDense
 }
 
+type SolveMethod int
+
+const (
+	SolveMethodOptimize SolveMethod = iota
+	SolveMethodNelderMead
+	SolveMethodLeastSquare
+)
+
 const epsilon = 1e-4
 
 func normalize(v *mat.VecDense) (*mat.VecDense, float64, float64) {
@@ -35,7 +43,7 @@ func normalize2(v *mat.VecDense, vOff, vScale float64) *mat.VecDense {
 	return v
 }
 
-func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, error) {
+func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense, method SolveMethod) ([]float64, error) {
 	M := setupSystemOfEquations(f, latVec, lonVec, heightVec)
 	n := f.Len()
 	weights := mat.NewDiagDense(n, nil)
@@ -61,6 +69,7 @@ func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, e
 	MtW2F.MulVec(&MtW2, f)
 	x0.MulVec(invMtW2M, &MtW2F)
 
+	if method == SolveMethodNelderMead {
 		numerator := mat.NewVecDense(20, nil)
 		denominator := mat.NewVecDense(20, nil)
 		denominator.SetVec(0, 1.0)
@@ -70,17 +79,18 @@ func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, e
 			denominator.SetVec(i, x0.AtVec(i+19))
 		}
 
-	// num, den, err := solveNelderMead(numerator, denominator, f, latVec, lonVec, heightVec)
+		num, den, err := solveNelderMead(numerator, denominator, f, latVec, lonVec, heightVec)
-	// if err != nil {
+		if err != nil {
-	// 	return nil, err
+			return nil, err
-	// }
+		}
 
-	// var coeffs []float64
+		var coeffs []float64
-	// coeffs = append(coeffs, num.RawVector().Data...)
+		coeffs = append(coeffs, num.RawVector().Data...)
-	// for i := 1; i < 20; i++ {
+		for i := 1; i < 20; i++ {
-	// 	coeffs = append(coeffs, den.AtVec(i))
+			coeffs = append(coeffs, den.AtVec(i))
-	// }
+		}
-	// return coeffs, nil
+		return coeffs, nil
+	}
 
 	// 迭代
 	var wm mat.Dense
@@ -91,7 +101,7 @@ func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, e
 	v0 := 0.0
 	iterations := 0
 	maxIterations := 10
-
+	denominator := mat.NewVecDense(20, nil)
 	for iterations < maxIterations {
 		denominator.SetVec(0, 1.0)
 		for idx := 1; idx < 20; idx++ {
@@ -243,9 +253,9 @@ func invertRPCMatrix(At *mat.Dense) (*mat.Dense, error) {
 	if err != nil {
 		// 岭估计方法调整法方程状态，使得矩阵非奇异，最小二乘平差可以收敛
 		r, c := At.Dims()
-		log.Infof("cannot inverse matrix(%d*%d): %v", r, c, err)
+		log.Debugf("cannot inverse matrix(%d*%d): %v", r, c, err)
 		k := 0.0000001 // [0.00000005, 0.000005]
-		log.Infof("try to adjust matrix with +kI, k=%.8f", k)
+		log.Debugf("try to adjust matrix with +kI, k=%.8f", k)
 		I := mat.NewDiagDense(r, nil)
 		for i := 0; i < r; i++ {
 			I.SetDiag(i, k)
@@ -256,7 +266,7 @@ func invertRPCMatrix(At *mat.Dense) (*mat.Dense, error) {
 	}
 
 	if err != nil {
-		log.Infof("cannot inverse matrix: %v, try SVD method", err)
+		log.Debugf("cannot inverse matrix: %v, try SVD method", err)
 		// 计算矩阵的 SVD 分解
 		var svd mat.SVD
 		ok := svd.Factorize(At, mat.SVDThin)

pkg/utils/tiff.go

@@ -87,7 +87,7 @@ func SaveBGRToGDALGTiff(bgr gocv.Mat,
 	defer ds.Close()
 
 	// ds.SetMetadata("NBITS", "16")
-	setGeoTransform(ds, topLeftX, topLeftY, resolution)
+	// setGeoTransform(ds, topLeftX, topLeftY, resolution)
 
 	for b := 0; b < bands; b++ {
 		band := ds.Bands()[b]