From 156692eef348eea5a7d4b7df8b84c6ce982af799 Mon Sep 17 00:00:00 2001 From: wangchongwu <759291707@qq.com> Date: Thu, 6 Mar 2025 11:16:38 +0800 Subject: [PATCH] =?UTF-8?q?=E5=89=8D=E8=A7=86=E6=89=AB=E6=8F=8F=E5=88=9D?= =?UTF-8?q?=E5=A7=8B=E5=8C=96=E8=BF=87=E7=A8=8B=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- main.cpp | 8 +-- stitch/src/API_FrontStitch.h | 6 +- stitch/src/Arith_FrontProj.cpp | 61 ----------------- stitch/src/Arith_FrontProj.h | 23 ------- stitch/src/Arith_FrontStitch.cpp | 113 +++++++++++++++++++++++++++---- stitch/src/Arith_FrontStitch.h | 10 ++- stitch/src/StitchStruct.h | 22 +++++- stitch/src/mapKernel.cu | 7 +- 8 files changed, 136 insertions(+), 114 deletions(-) delete mode 100644 stitch/src/Arith_FrontProj.cpp delete mode 100644 stitch/src/Arith_FrontProj.h diff --git a/main.cpp b/main.cpp index c475e5f..1999c4a 100644 --- a/main.cpp +++ b/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"); } \ No newline at end of file diff --git a/stitch/src/API_FrontStitch.h b/stitch/src/API_FrontStitch.h index 97c3a76..ee868d0 100644 --- a/stitch/src/API_FrontStitch.h +++ b/stitch/src/API_FrontStitch.h @@ -19,16 +19,14 @@ - - // 视频帧前视拼接,基于极坐标 class STD_STITCH_API API_FrontStitch { public: virtual ~API_FrontStitch() = default; - // 初始化拼接 - virtual FPanInfo Init(FrameInfo info, float AzRange, float PtRange) = 0; + // 初始化拼接,以info给定的伺服指向为扫描中心，结合方位范围和俯仰范围初始化拼接图 + virtual FPanInfo Init(FrameInfo info, ScanRange stAzRange, ScanRange stPtRange) = 0; // 极坐标快拼 virtual BYTE8 PoleStitch(GD_VIDEO_FRAME_S img, FrameInfo para) = 0; diff --git a/stitch/src/Arith_FrontProj.cpp b/stitch/src/Arith_FrontProj.cpp deleted file mode 100644 index b4a941f..0000000 --- a/stitch/src/Arith_FrontProj.cpp +++ /dev/null @@ -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; -} diff --git a/stitch/src/Arith_FrontProj.h b/stitch/src/Arith_FrontProj.h deleted file mode 100644 index 0a830bc..0000000 --- a/stitch/src/Arith_FrontProj.h +++ /dev/null @@ -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); \ No newline at end of file diff --git a/stitch/src/Arith_FrontStitch.cpp b/stitch/src/Arith_FrontStitch.cpp index 1af8be5..a14b0ee 100644 --- a/stitch/src/Arith_FrontStitch.cpp +++ b/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 #include @@ -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); - // 可以正反验证 - //cv::Point2f imgpos = getImgPosFromPole(H, centPole, info.nEvHeight); - _panPara.center.fAz = centPole.beta; - _panPara.center.fPt = centPole.alpha; - _panPara.range.fAz = AzRange; - _panPara.range.fPt = PtRange; + + _panPara.centerGeo.fAz = (Pole_east.beta + Pole_west.beta)/2; + _panPara.centerGeo.fPt = (Pole_up.alpha + Pole_bottom.alpha)/2; + + _panPara.stAzRangeGeo.Agl0 = Pole_west.beta; + _panPara.stAzRangeGeo.Agl1 = Pole_east.beta; + _panPara.stPtRangeGeo.Agl0 = Pole_bottom.alpha; + _panPara.stPtRangeGeo.Agl1 = Pole_up.alpha; + + + _panPara.fAglRes = info.camInfo.fAglReso * 0.5; // 全景图规模 - _panPara.m_pan_width = AzRange / _panPara.fAglRes; - _panPara.m_pan_height = PtRange / _panPara.fAglRes; + _panPara.m_pan_width = DEGLIM360(Pole_east.beta - Pole_west.beta) / _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; +} diff --git a/stitch/src/Arith_FrontStitch.h b/stitch/src/Arith_FrontStitch.h index 4e80438..3c462c5 100644 --- a/stitch/src/Arith_FrontStitch.h +++ b/stitch/src/Arith_FrontStitch.h @@ -16,8 +16,14 @@ #include "Arith_GeoSolver.h" #include #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); diff --git a/stitch/src/StitchStruct.h b/stitch/src/StitchStruct.h index c7d1d10..a35aeb5 100644 --- a/stitch/src/StitchStruct.h +++ b/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; +}; \ No newline at end of file diff --git a/stitch/src/mapKernel.cu b/stitch/src/mapKernel.cu index a57e467..0d66e6b 100644 --- a/stitch/src/mapKernel.cu +++ b/stitch/src/mapKernel.cu @@ -1,14 +1,13 @@ #include -#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.alpha = _panPara.center.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes; + _pole.beta = _panPara.centerGeo.fAz + (pt.x - _panPara.m_pan_width / 2) * _panPara.fAglRes; + _pole.alpha = _panPara.centerGeo.fPt + (_panPara.m_pan_height / 2 - pt.y) * _panPara.fAglRes; return _pole; }