使用定姿四元数计算图像位置

pkg/calculator/quaternion.go

@@ -1,70 +1,20 @@
 package calculator
 
 import (
-	"fmt"
-	"math"
+	"gonum.org/v1/gonum/mat"
 )
 
-// Quaternion represents a quaternion with scalar (w) and vector (x, y, z) parts
 type Quaternion struct {
-	w, x, y, z float64
+	W, X, Y, Z float64
 }
 
-// Quaternion multiplication
-func (q1 Quaternion) Mul(q2 Quaternion) Quaternion {
-	return Quaternion{
-		w: q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
-		x: q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y,
-		y: q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x,
-		z: q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w,
-	}
-}
-
-// Quaternion conjugate
-func (q Quaternion) Conjugate() Quaternion {
-	return Quaternion{w: q.w, x: -q.x, y: -q.y, z: -q.z}
-}
-
-// Rotate vector by quaternion
-func (q Quaternion) Rotate(v [3]float64) [3]float64 {
-	qv := Quaternion{w: 0, x: v[0], y: v[1], z: v[2]}
-	qConj := q.Conjugate()
-	qvRotated := q.Mul(qv).Mul(qConj)
-	return [3]float64{qvRotated.x, qvRotated.y, qvRotated.z}
-}
-
-func main() {
-	// 示例数据
-	qBI := Quaternion{w: 1, x: 0, y: 0, z: 0} // 本体相对惯性系四元数
-	posJ2000 := [3]float64{7000, 0, 0}        // J2000位置
-	// velJ2000 := [3]float64{0, 7.5, 0}         // J2000速度
-
-	// 相机参数
-	const numPixels = 9520
-	const fov = 10.0 * math.Pi / 180 // 假设视场角为10度
-
-	// 逐像素计算地面交点
-	for i := 0; i < numPixels; i++ {
-		// 计算像素点相对光轴的偏角
-		pixelOffset := (float64(i) - float64(numPixels)/2) / float64(numPixels)
-		angle := pixelOffset * fov
-
-		// 假设光轴在本体坐标系中指向-z方向，计算视线方向
-		dBody := [3]float64{-math.Sin(angle), 0, -math.Cos(angle)}
-
-		// 转换到惯性系
-		dInertial := qBI.Rotate(dBody)
-
-		// 计算地面交点（假设dInertial已经标准化）
-		k := -posJ2000[2] / dInertial[2] // 简化的交点计算
-		groundPoint := [3]float64{
-			posJ2000[0] + k*dInertial[0],
-			posJ2000[1] + k*dInertial[1],
-			posJ2000[2] + k*dInertial[2],
-		}
-
-		// 转换到地理坐标
-		lat, lon, _ := ECEFToGeodetic(groundPoint[0], groundPoint[1], groundPoint[2])
-		fmt.Printf("Pixel %d: Latitude: %f, Longitude: %f\n", i, lat, lon)
-	}
+// ToRotationMatrix converts a quaternion to a rotation matrix.
+func (q Quaternion) ToRotationMatrix() *mat.Dense {
+	w, x, y, z := q.W, q.X, q.Y, q.Z
+
+	return mat.NewDense(3, 3, []float64{
+		1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y,
+		2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x,
+		2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y,
+	})
 }