IHS pansharpen

pkg/auxilary/attitude.go

@@ -0,0 +1,126 @@
+package auxilary
+
+import (
+	"fmt"
+	"math"
+	"os"
+)
+
+type Attitudes struct {
+	Atts []*Attitude
+}
+
+func (atts Attitudes) Save(attFile string) error {
+	f, err := os.Create(attFile)
+	if err != nil {
+		return err
+	}
+	defer f.Close()
+
+	for _, att := range atts.Atts {
+		content := fmt.Sprintf("%.6f %.8f %.8f %.8f %.8f\n", att.UTCTimestampSec, att.Q0, att.Q1, att.Q2, att.Q3)
+		f.WriteString(content)
+	}
+
+	return nil
+}
+
+// 四元数球面线性插值
+func (atts Attitudes) Slerp(t float64) *Attitude {
+	// 取 t 前后的四元数
+	var q0, q1 *Attitude
+
+	for i, att := range atts.Atts {
+		if att.UTCTimestampSec >= t {
+			if i == 0 {
+				q0 = att
+				q1 = atts.Atts[i+1]
+			} else {
+				q0 = atts.Atts[i-1]
+				q1 = att
+			}
+			break
+		}
+	}
+
+	if q0 == nil || q1 == nil {
+		return atts.Atts[len(atts.Atts)-1]
+	}
+
+	var w0, x0, y0, z0, w1, x1, y1, z1 float64
+	w0, x0, y0, z0 = q0.Q0, q0.Q1, q0.Q2, q0.Q3
+	w1, x1, y1, z1 = q1.Q0, q1.Q1, q1.Q2, q1.Q3
+
+	// 用点乘计算两个四元数夹角的cos值
+	cosOmega := w0*w1 + x0*x1 + y0*y1 + z0*z1
+
+	// 如果点乘为负，则反转一个四元数以取得短的4D弧
+	if cosOmega < 0.0 {
+		w1 = -w1
+		x1 = -x1
+		y1 = -y1
+		z1 = -z1
+		cosOmega = -cosOmega
+	}
+
+	var k0, k1 float64
+	t0, t1 := q0.UTCTimestampSec, q1.UTCTimestampSec
+	if cosOmega > 0.99999999 { // cos=1的时候就是夹角为0，重合
+		k0 = (t1 - t) / (t1 - t0)
+		k1 = (t - t0) / (t1 - t0)
+	} else {
+		// 用三角公式sin^2+cos^2=1计算sin值
+		sinOmega := math.Sqrt(1.0 - cosOmega*cosOmega)
+		// 通过sin和cos计算角度
+		omega := math.Atan2(sinOmega, cosOmega) // 计算点(cosOmega,sinOmega)与x轴正向的夹角
+		// 计算分母的倒数，这样就只需要一次除法
+		oneOverSinOmega := 1.0 / sinOmega
+		//计算插值变量
+		k0 = math.Sin((t1-t)/(t1-t0)*omega) * oneOverSinOmega
+		k1 = math.Sin((t-t0)/(t1-t0)*omega) * oneOverSinOmega
+	}
+	// 插值
+	w := w0*k0 + w1*k1
+	x := x0*k0 + x1*k1
+	y := y0*k0 + y1*k1
+	z := z0*k0 + z1*k1
+
+	return &Attitude{
+		UTCTimestampSec: t,
+		Q0:              w,
+		Q1:              x,
+		Q2:              y,
+		Q3:              z,
+	}
+}
+
+type Attitude struct {
+	UTCTimestampSec float64
+	Q0, Q1, Q2, Q3  float64
+}
+
+func StoreAtt(aps []*AuxPlatform, attFile string) (*Attitudes, error) {
+	atts := ExtractAttitude(aps)
+	return atts, atts.Save(attFile)
+}
+
+func ExtractAttitude(aps []*AuxPlatform) *Attitudes {
+	var atts Attitudes
+	var sec, microsec uint32
+	for _, ap := range aps {
+		if ap.UTCTimeSec != sec || ap.Microsecond != microsec {
+			sec, microsec = ap.UTCTimeSec, ap.Microsecond
+			att := Attitude{
+				UTCTimestampSec: float64(sec) + float64(ReferenceTime2000) +
+					float64(transfromGPSandAttMicrosec(microsec))/1e6,
+				Q0: ap.QuatAttstarQ0,
+				Q1: ap.QuatAttstarQ1,
+				Q2: ap.QuatAttstarQ2,
+				Q3: ap.QuatAttstarQ3,
+			}
+			atts.Atts = append(atts.Atts, &att)
+		}
+	}
+
+	return &atts
+}