diff --git a/pkg/calculator/camera.go b/pkg/calculator/camera.go index f1e1422..853bbad 100644 --- a/pkg/calculator/camera.go +++ b/pkg/calculator/camera.go @@ -1,37 +1,40 @@ package calculator import ( - "fmt" "math" + + "gonum.org/v1/gonum/mat" ) const ( - FocalLength = 1300.0 // 焦距, mm - FOV = 1.7 // 对角线视场角,degree - FOVCALC = 1.86 // 对角线视场角,degree - PANPixels = 9344.0 - PANCellSize = 3.2 // µm - MSSPixels = 2336.0 - MSSCellSize = 12.8 // µm + FocalLength = 1300.0 // 焦距, mm + FOV = 1.7 // 对角线视场角,degree + FOVCALC = 1.86 // 对角线视场角,degree + PANPixels = 9344.0 + PANCellSize = 3.2 // µm + MSSPixels = 2336.0 + 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 - h := w - d := math.Sqrt(math.Pow(w, 2) + math.Pow(h, 2)) // 对角线长度, mm - fov := 2 * math.Atan2(d/2, FocalLength) // 对角线视场角 + // w := PANPixels * PANCellSize / 1000.0 // 像素宽度, mm + // h := w + // d := math.Sqrt(math.Pow(w, 2) + math.Pow(h, 2)) // 对角线长度, mm + // fov := 2 * math.Atan2(w/2, FocalLength) // 对角线视场角 - fmt.Println("Camera Parameters:") - fmt.Printf("Focal Length: %.6f mm\n", FocalLength) - fmt.Printf("FOV (calculated): %.6f degree\n", fov*180/math.Pi) - fmt.Printf("Width: %.6f mm\n", w) - fmt.Printf("Height: %.6f mm\n", h) - fmt.Printf("Diagonal: %.6f mm\n", d) + // fmt.Println("Camera Parameters:") + // fmt.Printf("Focal Length: %.6f mm\n", FocalLength) + // fmt.Printf("FOV (calculated): %.6f degree\n", fov*180/math.Pi) + // fmt.Printf("Width: %.6f mm\n", w) + // fmt.Printf("Height: %.6f mm\n", h) + // fmt.Printf("Diagonal: %.6f mm\n", d) // 从给定FOV计算d - dCalcOfFOV := 2 * FocalLength * math.Tan(FOV/2*math.Pi/180) - fmt.Printf("Diagonal (calculated from FOV): %.6f mm\n", dCalcOfFOV) + // dCalcOfFOV := 2 * FocalLength * math.Tan(FOV/2*math.Pi/180) + // 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 +} diff --git a/pkg/calculator/intersection.go b/pkg/calculator/intersection.go index cf31b9f..7f3cce3 100644 --- a/pkg/calculator/intersection.go +++ b/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 = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels)) - direction := []float64{0, math.Tan(alpha), -1.3} + // alpha := FOV * math.Pi / 180.0 + // alpha = -alpha/2.0 + float64(ucam)*(alpha/float64(PANPixels)) + // direction := []float64{0, math.Tan(alpha), -1.3} + direction := CameraDirectionVec(0, float64(ucam)) // -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 -------- - Rcam := mat.NewDense(3, 3, []float64{ - 1, 0, 0, - 0, 1, 0, - 0, 0, 1, - }) + Rcam := CameraRotMatrix(AngleCamSatX*math.Pi/180.0, AngleCamSatY*math.Pi/180.0, 0) 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{ - 1, 0, 0, - 0, 1, 0, - 0, 0, 1, - }) + Rcam := CameraRotMatrix(AngleCamSatX*math.Pi/180.0, AngleCamSatY*math.Pi/180.0, 0) var dCam mat.VecDense dCam.MulVec(Rcam, mat.NewVecDense(3, direction))