前视扫描初始化过程优化

main.cpp

@@ -434,7 +434,7 @@ void ProcessFrontVL(string filePath)
 
         if (i == 0)
         {
-            stitcher->Init(info,80,50);
+            stitcher->Init(info, ScanRange{0,100}, ScanRange{ 56,90 });
 
             pan = stitcher->ExportPanAddr();
 
@@ -442,7 +442,7 @@ void ProcessFrontVL(string filePath)
         }
         else
         {
-            if (i % 30 != 0)
+            if (i % 60 != 0)
             {
                 i = i + 1;
                 continue;
@@ -488,12 +488,12 @@ void ProcessFrontVL(string filePath)
 int main(int, char**)
 {
     //ProcessIR("H:/ir_1280_1024_para40_y16/20241219155238_20.xraw", "20241219155238_20");
-    ProcessVL("H:/vl_1920_1080_para40_y8/22.video","22");
+    //ProcessVL("H:/vl_1920_1080_para40_y8/22.video","22");
     //ProcessVL("H:/vl_1920_1080_para40_y8/20241219153557_11.video", "20241219152643_1");
     //ProcessVL("H:/vl_1920_1080_para40_y8/20241219152917_4.video", "20241219152917_4");
     //ProcessVL("H:/vl_1920_1080_para40_y8/20241219153515_10.video", "20241219153515_10");
     //
     //ProcessVL("H:/vl_1920_1080_para40_y8/1.video", "1");
 
-    //ProcessFrontVL("H:/vl_1920_1080_para40_y8/1.video");
+    ProcessFrontVL("H:/vl_1920_1080_para40_y8/1.video");
 }

stitch/src/API_FrontStitch.h

@@ -19,16 +19,14 @@
 
 
 
-
-
 // 视频帧前视拼接,基于极坐标
 class STD_STITCH_API API_FrontStitch
 {
 public:
     virtual ~API_FrontStitch() = default;
 
-    // 初始化拼接
+    // 初始化拼接,以info给定的伺服指向为扫描中心，结合方位范围和俯仰范围初始化拼接图
-    virtual FPanInfo		Init(FrameInfo info, float AzRange, float PtRange) = 0;
+    virtual FPanInfo		Init(FrameInfo info, ScanRange stAzRange, ScanRange stPtRange) = 0;
 
     // 极坐标快拼
     virtual BYTE8       PoleStitch(GD_VIDEO_FRAME_S img, FrameInfo para) = 0;

stitch/src/Arith_FrontProj.cpp

@@ -1,61 +0,0 @@
-#include "Arith_FrontProj.h"
-#include "Arith_Utils.h"
-#include "StitchStruct.h"
-#include "Arith_GeoSolver.h"
-Pole getPoleFromImgWithH(cv::Mat H, cv::Point2f pt, float dep)
-{
-    // 投影到地面坐标，这一步可以利用前面优化成果
-    cv::Point2f grdPt = warpPointWithH(H, pt);
-
-    // 补深度信息并转为常用的NUE坐标系
-    PointXYZ grdPtXYZ = { 0 };
-    grdPtXYZ.X = grdPt.y;
-    grdPtXYZ.Y = -dep;
-    grdPtXYZ.Z = grdPt.x;
-
-    // 地面点转极坐标
-    Pole pole = getPoleFromXYZ(grdPtXYZ);
-
-    return pole;
-
-}
-
-cv::Point2f getImgPosFromPole(cv::Mat H_inv, Pole _pole, float dep)
-{
-    PointXYZ virPt = getXYZFromPole(_pole);
-
-    //虚拟点投影到地面
-    float ratio = -dep / virPt.Y;
-    PointXYZ realPt = { 0 };
-    realPt.X = virPt.X * ratio;
-    realPt.Y = virPt.Y * ratio;
-    realPt.Z = virPt.Z * ratio;
-
-
-    // 转东北地
-    PointXYZ realPtGeo = { 0 };
-    realPtGeo.X = realPt.Z;
-    realPtGeo.Y = realPt.X;
-    realPtGeo.Z = -realPt.Y;
-
-    // 投影回像方
-    cv::Point2f px = warpPointWithH(H_inv, cv::Point2f(realPtGeo.X, realPtGeo.Y));
-
-    return px;
-}
-
-Pole getPoleFromFPan(cv::Point2f pt, FPanInfo _panPara)
-{
-    Pole _pole = { 0 };
-    _pole.beta = DEGLIM(_panPara.center.fAz + (pt.x - _panPara.m_pan_width / 2) * _panPara.fAglRes);
-    _pole.alpha = DEGLIM(_panPara.center.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes);
-    return _pole;
-}
-
-cv::Point2f getFPanFromPole(Pole _pole, FPanInfo _panPara)
-{
-    cv::Point2f pt = { 0 };
-    pt.x = DEGLIM(_pole.beta - _panPara.center.fAz) / _panPara.fAglRes + _panPara.m_pan_width / 2;
-    pt.y = DEGLIM(_panPara.center.fPt - _pole.alpha) / _panPara.fAglRes + _panPara.m_pan_height / 2;
-    return pt;
-}

stitch/src/Arith_FrontProj.h

@@ -1,23 +0,0 @@
-
-#pragma once
-
-#include "Arith_CoordModule.h"
-#include "Arith_SysStruct.h"
-#include "opencv2/opencv.hpp"
-#include "StitchStruct.h"
-
-// 前视扫描投影变换模型
-// 
-// cuda显存资源
-struct cuda_Mem
-{
-    unsigned char* global_cuda_Frame;
-    unsigned char* global_cuda_Pan;
-    unsigned char* global_cuda_pan_mask;
-    double* global_cuda_H_inv_data;
-};
-
-Pole getPoleFromImgWithH(cv::Mat H, cv::Point2f pt, float dep);
-cv::Point2f getImgPosFromPole(cv::Mat H_inv, Pole _pole, float dep);
-Pole getPoleFromFPan(cv::Point2f pt, FPanInfo _panPara);
-cv::Point2f getFPanFromPole(Pole _pole, FPanInfo _panPara);

stitch/src/Arith_FrontStitch.cpp

@@ -3,7 +3,6 @@
 #include "Arith_Utils.h"
 #include "Arith_CoordModule.h"
 #include "Arith_FeaMatch.h"
-#include "Arith_FrontProj.h"
 #include <opencv2/opencv.hpp>
 #include <omp.h>
 
@@ -36,28 +35,57 @@ FrontStitch::~FrontStitch()
 }
 
 
-FPanInfo FrontStitch::Init(FrameInfo info, float AzRange, float PtRange)
+FPanInfo FrontStitch::Init(FrameInfo info, ScanRange stAzRange, ScanRange stPtRange)
 {
-    _panPara.fAglRes = 0.02;
+    // 帧角分辨率
+    if (info.camInfo.fAglReso == 0)
+    {
+        info.camInfo.fAglReso = info.camInfo.fPixelSize / info.camInfo.nFocus * 0.06;
+    }
+
+    // 视场角计算
+    float FOV_W = info.camInfo.fAglReso * info.nWidth;
+    float FOV_H = info.camInfo.fAglReso * info.nHeight;
+
     // 设置当前拼接起始位置
     _GeoSolver->SetOriginPoint(info);
 
-    cv::Mat H = _GeoSolver->findHomography(info);
 
-    // 计算中心点的极坐标
+    // 大地系下范围计算
-    Pole centPole = getPoleFromImgWithH(H, cv::Point2f(info.nWidth / 2, info.nHeight / 2), info.nEvHeight);
+
+    // west
+    info.servoInfo.fServoAz = stAzRange.Agl0;
+    cv::Mat H1 = _GeoSolver->findHomography(info);
+    Pole Pole_west = getPoleFromImgWithH(H1, cv::Point2f(0, info.nHeight / 2), info.nEvHeight);
+    // east
+    info.servoInfo.fServoAz = stAzRange.Agl1;
+    cv::Mat H2 = _GeoSolver->findHomography(info);
+    Pole Pole_east = getPoleFromImgWithH(H2, cv::Point2f(info.nWidth, info.nHeight / 2), info.nEvHeight);
+    // up
+    info.servoInfo.fServoPt = stPtRange.Agl1;
+    cv::Mat H3 = _GeoSolver->findHomography(info);
+    Pole Pole_up = getPoleFromImgWithH(H3, cv::Point2f(info.nWidth/2, 0), info.nEvHeight);
+    // bottom
+    info.servoInfo.fServoPt = stPtRange.Agl0;
+    cv::Mat H4 = _GeoSolver->findHomography(info);
+    Pole Pole_bottom = getPoleFromImgWithH(H4, cv::Point2f(info.nWidth/2, info.nHeight), info.nEvHeight);
+
+
 
-    // 可以正反验证
+    _panPara.centerGeo.fAz = (Pole_east.beta + Pole_west.beta)/2;
-    //cv::Point2f imgpos = getImgPosFromPole(H, centPole, info.nEvHeight);
+    _panPara.centerGeo.fPt = (Pole_up.alpha + Pole_bottom.alpha)/2;
     
-    _panPara.center.fAz = centPole.beta;
+    _panPara.stAzRangeGeo.Agl0 = Pole_west.beta;
-    _panPara.center.fPt = centPole.alpha;
+    _panPara.stAzRangeGeo.Agl1 = Pole_east.beta;
-    _panPara.range.fAz = AzRange;
+    _panPara.stPtRangeGeo.Agl0 = Pole_bottom.alpha;
-    _panPara.range.fPt = PtRange;
+    _panPara.stPtRangeGeo.Agl1 = Pole_up.alpha;
+
+
+    _panPara.fAglRes = info.camInfo.fAglReso * 0.5;
 
     // 全景图规模
-    _panPara.m_pan_width = AzRange / _panPara.fAglRes;
+    _panPara.m_pan_width = DEGLIM360(Pole_east.beta - Pole_west.beta) / _panPara.fAglRes;
-    _panPara.m_pan_height = PtRange / _panPara.fAglRes;
+    _panPara.m_pan_height = DEGLIM360(Pole_up.alpha - Pole_bottom.alpha) / _panPara.fAglRes;
 
     _panImage = cv::Mat::zeros(_panPara.m_pan_height, _panPara.m_pan_width, CV_8UC3);
 
@@ -180,4 +208,61 @@ void UpdatePan_CPU(cv::Mat rgbFrame, cv::Mat pan, cv::Rect2d roi, cv::Mat H_inv,
 
 }
 
+Pole getPoleFromImgWithH(cv::Mat H, cv::Point2f pt, float dep)
+{
+    // 投影到地面坐标，这一步可以利用前面优化成果
+    cv::Point2f grdPt = warpPointWithH(H, pt);
+
+    // 补深度信息并转为常用的NUE坐标系
+    PointXYZ grdPtXYZ = { 0 };
+    grdPtXYZ.X = grdPt.y;
+    grdPtXYZ.Y = -dep;
+    grdPtXYZ.Z = grdPt.x;
+
+    // 地面点转极坐标
+    Pole pole = getPoleFromXYZ(grdPtXYZ);
+
+    return pole;
+
+}
+
+cv::Point2f getImgPosFromPole(cv::Mat H_inv, Pole _pole, float dep)
+{
+    PointXYZ virPt = getXYZFromPole(_pole);
+
+    //虚拟点投影到地面
+    float ratio = -dep / virPt.Y;
+    PointXYZ realPt = { 0 };
+    realPt.X = virPt.X * ratio;
+    realPt.Y = virPt.Y * ratio;
+    realPt.Z = virPt.Z * ratio;
+
+
+    // 转东北地
+    PointXYZ realPtGeo = { 0 };
+    realPtGeo.X = realPt.Z;
+    realPtGeo.Y = realPt.X;
+    realPtGeo.Z = -realPt.Y;
+
+    // 投影回像方
+    cv::Point2f px = warpPointWithH(H_inv, cv::Point2f(realPtGeo.X, realPtGeo.Y));
+
+    return px;
+}
+
+Pole getPoleFromFPan(cv::Point2f pt, FPanInfo _panPara)
+{
+    Pole _pole = { 0 };
+    _pole.beta = DEGLIM(_panPara.centerGeo.fAz + (pt.x - _panPara.m_pan_width / 2) * _panPara.fAglRes);
+    _pole.alpha = DEGLIM(_panPara.centerGeo.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes);
+    return _pole;
+}
+
+cv::Point2f getFPanFromPole(Pole _pole, FPanInfo _panPara)
+{
+    cv::Point2f pt = { 0 };
+    pt.x = DEGLIM(_pole.beta - _panPara.centerGeo.fAz) / _panPara.fAglRes + _panPara.m_pan_width / 2;
+    pt.y = DEGLIM(_panPara.centerGeo.fPt - _pole.alpha) / _panPara.fAglRes + _panPara.m_pan_height / 2;
+    return pt;
+}
 

stitch/src/Arith_FrontStitch.h

@@ -16,8 +16,14 @@
 #include "Arith_GeoSolver.h"
 #include <cuda_runtime.h>
 #include "opencv2/opencv.hpp"
-#include "Arith_FrontProj.h"
 
+// 前视扫描投影变换模型
+
+
+Pole getPoleFromImgWithH(cv::Mat H, cv::Point2f pt, float dep);
+cv::Point2f getImgPosFromPole(cv::Mat H_inv, Pole _pole, float dep);
+Pole getPoleFromFPan(cv::Point2f pt, FPanInfo _panPara);
+cv::Point2f getFPanFromPole(Pole _pole, FPanInfo _panPara);
 
 
 class FrontStitch:public API_FrontStitch
@@ -27,7 +33,7 @@ public:
 	~FrontStitch();
 
     // 初始化拼接
-    FPanInfo		Init(FrameInfo info, float AzRange, float PtRange);
+    FPanInfo		Init(FrameInfo info, ScanRange stAzRange, ScanRange stPtRange);
 
     // 极坐标快拼
     BYTE8       PoleStitch(GD_VIDEO_FRAME_S img, FrameInfo para);

stitch/src/StitchStruct.h

@@ -31,6 +31,12 @@ struct UPanInfo
     float map_shiftY;// 平移Y
 };
 
+// 扫描范围（-180-180）
+struct ScanRange
+{
+    float Agl0;
+    float Agl1;
+};
 
 // 前视全景图配置
 struct FPanInfo
@@ -38,8 +44,11 @@ struct FPanInfo
     int m_pan_width;
     int m_pan_height;
     float fAglRes;
-    ANGLE32F range;
+
-    ANGLE32F center;
+    // 大地系范围
+    ANGLE32F centerGeo;
+    ScanRange stAzRangeGeo;
+    ScanRange stPtRangeGeo;
 };
 
 
@@ -83,3 +92,12 @@ struct PoleArea
     float bottom;
 };
 
+// 
+// cuda显存资源
+struct cuda_Mem
+{
+    unsigned char* global_cuda_Frame;
+    unsigned char* global_cuda_Pan;
+    unsigned char* global_cuda_pan_mask;
+    double* global_cuda_H_inv_data;
+};

stitch/src/mapKernel.cu

@@ -1,14 +1,13 @@
 #include <cuda_runtime.h>
-#include "Arith_FrontProj.h"
 #include "Arith_Utils.h"
 #include "Arith_SysStruct.h"
-
+#include "StitchStruct.h"
 
 __device__ Pole device_getPoleFromFPan(POINT32S pt, FPanInfo _panPara)
 {
     Pole _pole = { 0 };
-    _pole.beta = _panPara.center.fAz + (pt.x - _panPara.m_pan_width / 2) * _panPara.fAglRes;
+    _pole.beta = _panPara.centerGeo.fAz + (pt.x - _panPara.m_pan_width / 2) * _panPara.fAglRes;
-    _pole.alpha = _panPara.center.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes;
+    _pole.alpha = _panPara.centerGeo.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes;
     return _pole;
 }