[단독] AIDT 온디바이스 MUM-T 자폭/정찰 무인기 시각 주행계(Visual Odometry) 광학 흐름 지형 상대 항법 서보 v1.0

using System;
using System.Runtime.CompilerServices;
using System.Runtime.Intrinsics;
using System.Runtime.Intrinsics.X86;

namespace ZeliDesk.Defense.MUMT.VisualOdometry
{
    /// 

    /// 하방 광학 센서 및 관성/라이다 융합 데이터
    /// 
    public readonly record struct OpticalFlowSensorFrame(
        float AltitudeAboveGroundM,  // 레이저 고도계 계측 지표면 고도 h (m)
        float GyroRatePitchRadPs,    // IMU 측정 피치 각속도 (rad/s)
        float GyroRateRollRadPs,     // IMU 측정 롤 각속도 (rad/s)
        float FocalLengthPixels,     // 카메라 초점거리 f (pixels)
        float AvgFlowPixelX,         // 특징점 평균 광학 흐름 u (pixels/frame)
        float AvgFlowPixelY,         // 특징점 평균 광학 흐름 v (pixels/frame)
        float DeltaTimeSec           // 프레임 간격 (초, 예: 0.00833s)
    );

    /// 

    /// 시각 주행계 속도 및 지형 추종 출력
    /// 
    public readonly record struct VisualOdometryResult(
        float EstimatedVelocityXMps, // 복원된 종방향 대지 속도 (m/s)
        float EstimatedVelocityYMps, // 복원된 횡방향 대지 속도 (m/s)
        float GroundSpeedMps,        // 합성 수평 대지 속력 (m/s)
        bool IsVisualTrackingStable  // 시각 주행계 추적 신뢰성 여부
    );

    /// 

    /// 온디바이스 MUM-T 시각 주행계 광학 흐름 서보
    /// 
    public ref struct VisualOdometryTerrainServo
    {
        private float _filteredVx;
        private float _filteredVy;

        public VisualOdometryTerrainServo(float initialVx, float initialVy)
        {
            _filteredVx = initialVx;
            _filteredVy = initialVy;
        }

        /// 

        /// 120Hz 고속 루프: 회전 보상 광학 흐름 기반 절대 대지 속도 산출
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public VisualOdometryResult UpdateServo(in OpticalFlowSensorFrame frame)
        {
            // 1. 회전 성분으로 인한 흐름 차감 (De-rotation)
            // u_rot = -f * omega_pitch * dt, v_rot = -f * omega_roll * dt
            float uRot = -frame.FocalLengthPixels * frame.GyroRatePitchRadPs * frame.DeltaTimeSec;
            float vRot = -frame.FocalLengthPixels * frame.GyroRateRollRadPs * frame.DeltaTimeSec;

            float uTrans = frame.AvgFlowPixelX - uRot;
            float vTrans = frame.AvgFlowPixelY - vRot;

            // 2. 병진 속도 복원: V = -(Flow * h) / (f * dt)
            float safeH = Math.Max(1.0f, frame.AltitudeAboveGroundM);
            float safeDt = Math.Max(1e-4f, frame.DeltaTimeSec);
            float factor = safeH / (frame.FocalLengthPixels * safeDt);

            float rawVx = -uTrans * factor;
            float rawVy = -vTrans * factor;

            // 3. 지수 평활 칼만 필터 (알파 = 0.8)
            _filteredVx += 0.8f * (rawVx - _filteredVx);
            _filteredVy += 0.8f * (rawVy - _filteredVy);

            float groundSpeed = MathF.Sqrt(_filteredVx * _filteredVx + _filteredVy * _filteredVy);

            bool isStable = (groundSpeed > 5.0f && groundSpeed < 120.0f) && (frame.AltitudeAboveGroundM >= 5.0f);

            return new VisualOdometryResult(
                _filteredVx,
                _filteredVy,
                groundSpeed,
                isStable
            );
        }

        /// 

        /// 다중 특징점 픽셀 흐름 배열에 대한 SIMD 회전 성분 일괄 차감 커널
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void BatchComputeOpticalFlowSIMD(
            ReadOnlySpan rawFlowX,
            ReadOnlySpan rawFlowY,
            float rotFlowX,
            float rotFlowY,
            Span outTransX,
            Span outTransY)
        {
            int i = 0;
            int vectorSize = Vector256.Count;
            var rotXVec = Vector256.Create(rotFlowX);
            var rotYVec = Vector256.Create(rotFlowY);

            for (; i <= rawFlowX.Length - vectorSize; i += vectorSize)
            {
                var fx = Vector256.LoadUnsafe(in rawFlowX[i]);
                var fy = Vector256.LoadUnsafe(in rawFlowY[i]);

                var tx = Vector256.Subtract(fx, rotXVec);
                var ty = Vector256.Subtract(fy, rotYVec);

                tx.StoreUnsafe(ref outTransX[i]);
                ty.StoreUnsafe(ref outTransY[i]);
            }

            for (; i < rawFlowX.Length; i++)
            {
                outTransX[i] = rawFlowX[i] - rotFlowX;
                outTransY[i] = rawFlowY[i] - rotFlowY;
            }
        }
    }
}