相机在x-y上有角度

pkg/calculator/camera.go

@@ -1,37 +1,40 @@
 package calculator
 
 import (
-	"fmt"
 	"math"
+
+	"gonum.org/v1/gonum/mat"
 )
 
 const (
-	FocalLength = 1300.0 // 焦距, mm
+	FocalLength  = 1300.0 // 焦距, mm
-	FOV         = 1.7    // 对角线视场角,degree
+	FOV          = 1.7    // 对角线视场角,degree
-	FOVCALC     = 1.86   // 对角线视场角,degree
+	FOVCALC      = 1.86   // 对角线视场角,degree
-	PANPixels   = 9344.0
+	PANPixels    = 9344.0
-	PANCellSize = 3.2 // µm
+	PANCellSize  = 3.2 // µm
-	MSSPixels   = 2336.0
+	MSSPixels    = 2336.0
-	MSSCellSize = 12.8 // µm
+	MSSCellSize  = 12.8 // µm
+	AngleCamSatX = 0.0  // 相机与卫星本体X轴的安装角度, degree FIXME: 安装矩阵应该由卫星方提供
+	AngleCamSatY = 0.5  // 相机与卫星本体Y轴的安装角度, degree
 )
 
 // 计算过程使用PAN分辨率
 func CameraDirectionVec(u, v float64) []float64 {
-	w := PANPixels * PANCellSize / 1000.0 // 像素宽度, mm
+	// w := PANPixels * PANCellSize / 1000.0 // 像素宽度, mm
-	h := w
+	// h := w
-	d := math.Sqrt(math.Pow(w, 2) + math.Pow(h, 2)) // 对角线长度, mm
+	// d := math.Sqrt(math.Pow(w, 2) + math.Pow(h, 2)) // 对角线长度, mm
-	fov := 2 * math.Atan2(d/2, FocalLength)         // 对角线视场角
+	// fov := 2 * math.Atan2(w/2, FocalLength)         // 对角线视场角
 
-	fmt.Println("Camera Parameters:")
+	// fmt.Println("Camera Parameters:")
-	fmt.Printf("Focal Length: %.6f mm\n", FocalLength)
+	// fmt.Printf("Focal Length: %.6f mm\n", FocalLength)
-	fmt.Printf("FOV (calculated): %.6f degree\n", fov*180/math.Pi)
+	// fmt.Printf("FOV (calculated): %.6f degree\n", fov*180/math.Pi)
-	fmt.Printf("Width: %.6f mm\n", w)
+	// fmt.Printf("Width: %.6f mm\n", w)
-	fmt.Printf("Height: %.6f mm\n", h)
+	// fmt.Printf("Height: %.6f mm\n", h)
-	fmt.Printf("Diagonal: %.6f mm\n", d)
+	// fmt.Printf("Diagonal: %.6f mm\n", d)
 
 	// 从给定FOV计算d
-	dCalcOfFOV := 2 * FocalLength * math.Tan(FOV/2*math.Pi/180)
+	// dCalcOfFOV := 2 * FocalLength * math.Tan(FOV/2*math.Pi/180)
-	fmt.Printf("Diagonal (calculated from FOV): %.6f mm\n", dCalcOfFOV)
+	// fmt.Printf("Diagonal (calculated from FOV): %.6f mm\n", dCalcOfFOV)
 
 	directionVec := []float64{0, 0, 0}
 	directionVec[0] = 0                                        // x方向, mm，线性CCD每次单行成像
@@ -39,9 +42,9 @@ func CameraDirectionVec(u, v float64) []float64 {
 	directionVec[2] = -FocalLength                             // z方向, mm
 
 	// 归一化
-	fmt.Printf("Direction Vector: (%.6f, %.6f, %.6f) \n", directionVec[0], directionVec[1], directionVec[2])
+	// fmt.Printf("Direction Vector: (%.6f, %.6f, %.6f) \n", directionVec[0], directionVec[1], directionVec[2])
 	directionVec = normalizeVec(directionVec)
-	fmt.Printf("Direction Vector (normalized): (%.6f, %.6f, %.6f) \n", directionVec[0], directionVec[1], directionVec[2])
+	// fmt.Printf("Direction Vector (normalized): (%.6f, %.6f, %.6f) \n", directionVec[0], directionVec[1], directionVec[2])
 
 	return directionVec
 }
@@ -57,3 +60,24 @@ func normalizeVec(vec []float64) []float64 {
 	}
 	return vec
 }
+
+// 假设相机相对卫星本体的安装矩阵只在Y轴方向旋转角度为θ
+func CameraRotMatrix(phi, theta, psi float64) *mat.Dense {
+	Rx := mat.NewDense(3, 3, []float64{
+		1, 0, 0,
+		0, math.Cos(phi), -math.Sin(phi),
+		0, math.Sin(phi), math.Cos(phi),
+	})
+
+	Ry := mat.NewDense(3, 3, []float64{
+		math.Cos(theta), 0, math.Sin(theta),
+		0, 1, 0,
+		-math.Sin(theta), 0, math.Cos(theta),
+	})
+
+	// R = Rz * Ry * Rx
+	RyRx := mat.NewDense(3, 3, nil)
+	RyRx.Mul(Ry, Rx)
+
+	return RyRx
+}

pkg/calculator/intersection.go

@@ -14,16 +14,13 @@ type IntersectionPoint struct {
 }
 
 func Intersection(q Quaternion, satPos84 []float64, satTime time.Time, ucam int) IntersectionPoint {
-	alpha := FOV * math.Pi / 180.0
+	// alpha := FOV * math.Pi / 180.0
-	alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
+	// alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels))
-	direction := []float64{0, math.Tan(alpha), -1.3}
+	// direction := []float64{0, math.Tan(alpha), -1.3}
+	direction := CameraDirectionVec(0, float64(ucam))
 
 	// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
-	Rcam := mat.NewDense(3, 3, []float64{
+	Rcam := CameraRotMatrix(AngleCamSatX*math.Pi/180.0, AngleCamSatY*math.Pi/180.0, 0)
-		1, 0, 0,
-		0, 1, 0,
-		0, 0, 1,
-	})
 	var dCam mat.VecDense
 	dCam.MulVec(Rcam, mat.NewVecDense(3, direction))
 
@@ -49,11 +46,7 @@ func Intersection2(Qsat2orbit, Qorbit2eci Quaternion, satPos84 []float64, satTim
 	direction := []float64{0, math.Tan(alpha), -1.3} // 卫星（相机）坐标系下CCD成像方向向量
 
 	// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
-	Rcam := mat.NewDense(3, 3, []float64{
+	Rcam := CameraRotMatrix(AngleCamSatX*math.Pi/180.0, AngleCamSatY*math.Pi/180.0, 0)
-		1, 0, 0,
-		0, 1, 0,
-		0, 0, 1,
-	})
 	var dCam mat.VecDense
 	dCam.MulVec(Rcam, mat.NewVecDense(3, direction))