换方向扫描重置起始点

stitch/src/Arith_FrontStitch.cpp

@@ -10,6 +10,11 @@
 using namespace std;
 using namespace cv;
 
+
+
+
+
+
 API_FrontStitch* API_FrontStitch::Create(std::string cachedir)
 {
     return new FrontStitch(cachedir);
@@ -142,6 +147,13 @@ BYTE8 FrontStitch::Run(GD_VIDEO_FRAME_S img, FrameInfo info)
     // 转为内部俯仰向下零位
     info.servoInfo.fServoPt += 90;
 
+    // 扫描方向改变，重新拼接
+    if (AglMonitor.update(info.servoInfo.fServoAz))
+    {
+        ResetOrigin(info);
+    }
+
+
     // 极坐标拼接
     PoleStitch(img, info);
 
@@ -311,6 +323,15 @@ GD_VIDEO_FRAME_S FrontStitch::ExportPanAddr()
     return pan_out;
 }
 
+void FrontStitch::ResetOrigin(FrameInfo para)
+{
+    // 设置当前拼接起始位置
+    _GeoSolver->SetOriginPoint(para);
+
+    _panImage.setTo(0);
+    _panMask.setTo(0);
+}
+
 PointBLH FrontStitch::getBLHFromPanUV(POINT32F pt)
 {
     Pole pole = getPoleFromFPan(cv::Point2f(pt.x, pt.y),  _panPara);

stitch/src/Arith_FrontStitch.h

@@ -24,6 +24,73 @@
 #include "Arith_FrontSolver.h"
 #include "Arith_BlendMap.h"
 
+// 水平扫描方向
+enum FSCAN_DIR
+{
+    LEFT,
+    RIGHT
+};
+
+
+class AngleDirection {
+public:
+    AngleDirection(size_t confirmCount = 3, double thr = 0.05)
+        : lastAngle(-1), lastConfirmedDir(0),
+        confirmCount(confirmCount), threshold(thr) {
+    }
+
+    // 输入新角度，返回：true=方向变化，false=方向未变
+    bool update(double angle) {
+        bool changed = false;
+
+        if (std::isnan(lastAngle)) { // 第一次输入
+            lastAngle = angle;
+            return false;
+        }
+
+        // 计算差值，处理跨越 0/360 的情况
+        double diff = angle - lastAngle;
+        if (diff > 180) diff -= 360;
+        if (diff < -180) diff += 360;
+        lastAngle = angle;
+
+        // 小于阈值认为无效
+        if (std::fabs(diff) < threshold) return false;
+
+        int dir = (diff > 0) ? 1 : -1;
+
+        printf("dir:%d\n", dir);
+
+
+        diffs.push_back(dir);
+        if (diffs.size() > confirmCount) diffs.pop_front();
+
+        // 检查是否连续 confirmCount 个相同方向
+        if (diffs.size() == confirmCount) {
+            bool allSame = true;
+            for (size_t i = 1; i < diffs.size(); i++) {
+                if (diffs[i] != diffs[0]) {
+                    allSame = false;
+                    break;
+                }
+            }
+            if (allSame) {
+                if (lastConfirmedDir != 0 && diffs[0] != lastConfirmedDir) {
+                    changed = true; // 确认方向变化
+                }
+                lastConfirmedDir = diffs[0];
+            }
+        }
+        return changed;
+    }
+
+private:
+    double lastAngle;          // 上一帧角度
+    int lastConfirmedDir;      // 已确认的方向
+    std::deque<int> diffs;     // 存最近差值符号
+    size_t confirmCount;       // 连续确认帧数
+    double threshold;          // 抖动阈值
+};
 
 class FrontStitch:public API_FrontStitch
 {
@@ -43,6 +110,9 @@ public:
     // 获取全景图
     GD_VIDEO_FRAME_S ExportPanAddr();
 
+    // 重置地面起始点
+    void ResetOrigin(FrameInfo para);
+
     // 像方转经纬高
     PointBLH getBLHFromPanUV(POINT32F pt);
 
@@ -83,6 +153,9 @@ private:
     FPanInfo _panPara;//全景配置
     cv::Mat _panImage;
     cv::Mat _panMask; //覆盖区域遮罩
+
+    AngleDirection AglMonitor;
+
     // device mem
 private:
     cuda_Mem _cuda_mem;