From 9387fc3586464980b6949191d2f9f41e05e34d73 Mon Sep 17 00:00:00 2001 From: wangchongwu <759291707@qq.com> Date: Tue, 11 Nov 2025 14:24:01 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=9D=E7=95=99=E5=8E=9F=E5=A7=8B=E5=88=86?= =?UTF-8?q?=E8=BE=A8=E7=8E=87=E7=9A=84=E5=9C=B0=E5=9B=BE=E5=88=9D=E5=A7=8B?= =?UTF-8?q?=E5=8C=96=E3=80=82=E8=80=97=E6=97=B6=E5=A2=9E=E9=95=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- stitch/src/Arith_UnderStitch.cpp | 134 ++++++++++++++++++++++++++++--- stitch/src/Arith_UnderStitch.h | 4 + stitch/src/GoogleTile.cpp | 11 ++- stitch/src/Version.h | 2 +- tests/stitch_S732.cpp | 6 +- 5 files changed, 140 insertions(+), 17 deletions(-) diff --git a/stitch/src/Arith_UnderStitch.cpp b/stitch/src/Arith_UnderStitch.cpp index 054b5f1..e854e5a 100644 --- a/stitch/src/Arith_UnderStitch.cpp +++ b/stitch/src/Arith_UnderStitch.cpp @@ -26,6 +26,7 @@ + using namespace std; using namespace cv; @@ -170,16 +171,130 @@ UPanInfo UnderStitch::InitMap(FrameInfo para) return panPara; } + + +double getLocalScaleFromH(const cv::Mat& H, double u, double v) +{ + CV_Assert(H.rows == 3 && H.cols == 3 && H.type() == CV_64F); + + // 提取矩阵元素 + double h00 = H.at(0, 0), h01 = H.at(0, 1), h02 = H.at(0, 2); + double h10 = H.at(1, 0), h11 = H.at(1, 1), h12 = H.at(1, 2); + double h20 = H.at(2, 0), h21 = H.at(2, 1), h22 = H.at(2, 2); + + // 原始点的齐次坐标 + double X = h00 * u + h01 * v + h02; + double Y = h10 * u + h11 * v + h12; + double W = h20 * u + h21 * v + h22; + + double W2 = (std::abs(W) > 1e-12) ? W * W : 1e-12; + + // 解析求导 —— Jacobian + double dx_du = (h00 * W - h20 * X) / W2; + double dx_dv = (h01 * W - h21 * X) / W2; + double dy_du = (h10 * W - h20 * Y) / W2; + double dy_dv = (h11 * W - h21 * Y) / W2; + + // 两个方向的放缩比例（米 / 像素） + double sx = std::hypot(dx_du, dy_du); + double sy = std::hypot(dx_dv, dy_dv); + + // 返回平均尺度 + return 0.5 * (sx + sy); +} + UPanInfo UnderStitch::InitMap_SCAN(FrameInfo para, float ptRange) { + UPanInfo panPara = { 0 }; + // 地理几何计算 RtK SE3; cv::Mat H0 = _GeoSolver->findHomography(para, SE3); + // 计算下视中心的地理坐标 + para.servoInfo.fServoPt = 0; + cv::Point2f ct_geo = warpPointWithH(H0, cv::Point2f(para.nWidth / 2, para.nHeight / 2)); + + // 下视最大分辨率估计 m/p + double scale_max = getLocalScaleFromH(H0, para.nWidth / 2, para.nHeight / 2); + + // 计算扫描边界的地理坐标 + para.servoInfo.fServoPt = -ptRange; + cv::Point2f ct_geo_left = warpPointWithH(_GeoSolver->findHomography(para, SE3), cv::Point2f(para.nWidth / 2, para.nHeight / 2)); + para.servoInfo.fServoPt = ptRange; + cv::Point2f ct_geo_right = warpPointWithH(_GeoSolver->findHomography(para, SE3), cv::Point2f(para.nWidth / 2, para.nHeight / 2)); + + // 载体速度矢量，以飞行15s计算，载体速度speed = 80m/s*15 = 2400m + double yaw_RAD = para.craft.stAtt.fYaw / 180 * 3.14159; + double dx = 1200 * std::sin(yaw_RAD); + double dy = 1200 * std::cos(yaw_RAD); + cv::Point2f ct_geo_left_fly = cv::Point2f(ct_geo_left.x + dx, ct_geo_left.y + dy); + cv::Point2f ct_geo_right_fly = cv::Point2f(ct_geo_right.x + dx, ct_geo_right.y + dy); + + // 计算下个全景地理中心和范围 + float min_x = min(min(ct_geo_left.x, ct_geo_right.x), min(ct_geo_left_fly.x, ct_geo_right_fly.x)); + float max_x = max(max(ct_geo_left.x, ct_geo_right.x), max(ct_geo_left_fly.x, ct_geo_right_fly.x)); + float min_y = min(min(ct_geo_left.y, ct_geo_right.y), min(ct_geo_left_fly.y, ct_geo_right_fly.y)); + float max_y = max(max(ct_geo_left.y, ct_geo_right.y), max(ct_geo_left_fly.y, ct_geo_right_fly.y)); + + float E_range = (max_x - min_x) * 1.2; + float N_range = (max_y - min_y) * 1.2; + + // 不损失分辨率的全景图尺寸 + panPara.m_pan_width = E_range / scale_max; + panPara.m_pan_height = N_range / scale_max; + + // 救救内存 + if (panPara.m_pan_width > 12000 || panPara.m_pan_height > 12000) + { + scale_max = E_range / 12000; + panPara.m_pan_width = E_range / scale_max; + panPara.m_pan_height = N_range / scale_max; + } + + + + panPara.scale = 1.0/scale_max; + + // 重新计算投影矩阵 + cv::Mat mapH0 = getAffineFromGeo2Pan(panPara); + auto cur = warpPointWithH(mapH0, ct_geo); + + // 计算平移到全景图固定点的平移量 + int planX = panPara.m_pan_width / 2; + int planY = panPara.m_pan_height / 2; + + panPara.map_shiftX += planX - (cur.x); + panPara.map_shiftY += planY - (cur.y); + + // 获取全景图的投影矩阵 + _H_pan = getAffineFromGeo2Pan(panPara); + + // 计算全景图瓦片相关 + auto P1 = getBLHFromPan(cv::Point2f(0, 0), _H_pan); + auto P2 = getBLHFromPan(cv::Point2f(panPara.m_pan_width, 0), _H_pan); + auto P3 = getBLHFromPan(cv::Point2f(panPara.m_pan_width, panPara.m_pan_height), _H_pan); + auto P4 = getBLHFromPan(cv::Point2f(0, panPara.m_pan_height), _H_pan); + + auto minL = min(min(min(P1.L, P2.L), P3.L), P4.L); + auto maxL = max(max(max(P1.L, P2.L), P3.L), P4.L); + auto minB = min(min(min(P1.B, P2.B), P3.B), P4.B); + auto maxB = max(max(max(P1.B, P2.B), P3.B), P4.B); + + _panTile.boxLatLon.north = maxB; + _panTile.boxLatLon.south = minB; + _panTile.boxLatLon.west = minL; + _panTile.boxLatLon.east = maxL; + _panTile.ind.z = _googleProduct.ZoomLevel(panPara.scale); + + // 生成瓦片划分 + _taskTilesVec = _googleProduct.CalcTileOverlayVec(_panTile, panPara.m_pan_width, panPara.m_pan_height); + + + return panPara; - return UPanInfo(); } UPanInfo UnderStitch::ShiftMap(FrameInfo para,int& shift_x,int& shift_y) @@ -241,11 +356,13 @@ UPanInfo UnderStitch::Init(FrameInfo info) // 设置拼接原点：地理计算的基准点 _GeoSolver->SetOriginPoint(info); - // 初始化全景图 - _panPara = InitMap(info); - // 摆扫45度初始化地图尺寸 - //_panPara = InitMap_SCAN(info,45); +#ifdef SCAN_MODE + _panPara = InitMap_SCAN(info,45); +#else + // 常规初始化全景图（基于内存限制） + _panPara = InitMap(info); +#endif // 创建blender _MapBlender = new MapBlend(_cache, _panPara); @@ -343,20 +460,18 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para) _googleProduct.ExportTileSet(mat_pan, mat_mask, tilesvec, outdirpath, filename); }, "ExportTileSet"); - // _googleProduct.ExportTileSet(panImage, panMask, _taskTilesVec, _outDir, _filename + std::to_string(_lastFrameKey)); - // 重新初始化投影参数 int shift_x = 0, shift_y = 0; + // 以向下初始化 para.servoInfo.fServoPt = 0; _panPara = ShiftMap(para, shift_x, shift_y); - + // 清空全景视场 _MapBlender->ClearPan(); - // 将BatchMap操作放入线程池中后台处理 _threadPool.commit([this, recvFrameKeys=_recvFrameKey, h_pan=_H_pan, Hgeos = _frameGeoHAll]() { _MapBlender->BatchMapWithH(recvFrameKeys, h_pan, Hgeos); @@ -384,6 +499,7 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para) // 投影帧到全景 _MapBlender->FrameMap(src, H_geo, _H_pan); + // 预处理+缓存 ReceiveFrame(frame, para); diff --git a/stitch/src/Arith_UnderStitch.h b/stitch/src/Arith_UnderStitch.h index ce216b5..781bf96 100644 --- a/stitch/src/Arith_UnderStitch.h +++ b/stitch/src/Arith_UnderStitch.h @@ -13,6 +13,10 @@ #include "Logger.h" +// 定义扫描模式，使用扫描专用的拼接地图策略 +#define SCAN_MODE + + // 关键帧间邻接信息 struct EdgeInfo { diff --git a/stitch/src/GoogleTile.cpp b/stitch/src/GoogleTile.cpp index db5ac2a..5d10897 100644 --- a/stitch/src/GoogleTile.cpp +++ b/stitch/src/GoogleTile.cpp @@ -46,9 +46,10 @@ void googleTile::ExportGeoPng(cv::Mat _pan, TileInfo info, std::string dir) } #include "Log/Logger.h" +#include "utils/Arith_timer.h" void googleTile::ExportTileSet(cv::Mat _pan, cv::Mat _panMask, vector taskTilesVec, std::string dir,std::string fileString) { - + stopWatch sw; int panWidth = _pan.cols; int panHeight = _pan.rows; @@ -57,8 +58,7 @@ void googleTile::ExportTileSet(cv::Mat _pan, cv::Mat _panMask, vector // 过滤掉无效瓦片 vector vaildTilesVec; - - for (size_t i = 0; i < taskTilesVec.size(); i++) + for (int i = 0; i < taskTilesVec.size(); i++) { TileInfo tile = taskTilesVec[i]; @@ -120,10 +120,13 @@ void googleTile::ExportTileSet(cv::Mat _pan, cv::Mat _panMask, vector } - LOG_INFO("taskTilesVec : {}，{}", fileString, taskTilesVec.size()); // 输出KML WriteKml(vaildTilesVec,dir + "/" + fileString + "_z" + std::to_string(zoom) + ".kml"); + + LOG_INFO("ExportTileSet TIME:{}", sw.elapsed_ms()); + + } void googleTile::ExportOneTile(cv::Mat _pan, TileInfo tile, std::string dir, std::string fileString) diff --git a/stitch/src/Version.h b/stitch/src/Version.h index de1cb5a..46c777c 100644 --- a/stitch/src/Version.h +++ b/stitch/src/Version.h @@ -2,5 +2,5 @@ #pragma once #include -std::string BUILD_TIME = "BUILD_TIME 2025_11_10-17.19.53"; +std::string BUILD_TIME = "BUILD_TIME 2025_11_11-09.47.45"; std::string VERSION = "BUILD_VERSION 1.0.1"; diff --git a/tests/stitch_S732.cpp b/tests/stitch_S732.cpp index 9cc0517..837b24b 100644 --- a/tests/stitch_S732.cpp +++ b/tests/stitch_S732.cpp @@ -135,7 +135,7 @@ void ProcessVL(string filePath) mat_pan = cv::Mat(pan.u32Height, pan.u32Width, CV_8UC4, pan.u64VirAddr[0]); - output.open("output.mp4", VideoWriter::fourcc('M', 'P', '4', 'V'), 50, Size(pan.u32Width / 8, pan.u32Height / 8), true); + output.open("output.mp4", VideoWriter::fourcc('M', 'P', '4', 'V'), 50, Size(pan.u32Width / 16, pan.u32Height / 16), true); if (!output.isOpened()) { cout << "打开视频失败" << endl; @@ -160,8 +160,8 @@ void ProcessVL(string filePath) } Mat pan_ds; - Mat pan_rgb_ds(cv::Size(mat_pan.cols / 8, mat_pan.rows / 8),CV_8UC3); - cv::resize(mat_pan, pan_ds, cv::Size(mat_pan.cols / 8, mat_pan.rows / 8)); + Mat pan_rgb_ds(cv::Size(mat_pan.cols / 16, mat_pan.rows / 16),CV_8UC3); + cv::resize(mat_pan, pan_ds, cv::Size(mat_pan.cols / 16, mat_pan.rows / 16)); cv::cvtColor(pan_ds, pan_rgb_ds, cv::COLOR_BGRA2BGR);