84 lines
2.5 KiB
Go
84 lines
2.5 KiB
Go
package calculator
|
||
|
||
import (
|
||
"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
|
||
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(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)
|
||
|
||
// 从给定FOV计算d
|
||
// 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每次单行成像
|
||
directionVec[1] = (v - PANPixels/2) * PANCellSize / 1000.0 // y方向, mm
|
||
directionVec[2] = -FocalLength // z方向, mm
|
||
|
||
// 归一化
|
||
// 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])
|
||
|
||
return directionVec
|
||
}
|
||
|
||
func normalizeVec(vec []float64) []float64 {
|
||
var norm float64
|
||
for i := 0; i < len(vec); i++ {
|
||
norm += vec[i] * vec[i]
|
||
}
|
||
|
||
for i := 0; i < len(vec); i++ {
|
||
vec[i] /= math.Sqrt(norm)
|
||
}
|
||
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
|
||
}
|