diff --git a/CMakeSettings.json b/CMakeSettings.json deleted file mode 100644 index 0c5fbf94..00000000 --- a/CMakeSettings.json +++ /dev/null @@ -1,27 +0,0 @@ -{ - "configurations": [ - { - "name": "x64-Debug", - "generator": "Ninja", - "configurationType": "Debug", - "inheritEnvironments": [ "msvc_x64_x64" ], - "buildRoot": "${projectDir}\\out\\build\\${name}", - "installRoot": "${projectDir}\\out\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "", - "ctestCommandArgs": "" - }, - { - "name": "x64-Release", - "generator": "Ninja", - "configurationType": "RelWithDebInfo", - "buildRoot": "${projectDir}\\out\\build\\${name}", - "installRoot": "${projectDir}\\out\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "", - "ctestCommandArgs": "", - "inheritEnvironments": [ "msvc_x64_x64" ], - "variables": [] - } - ] -} \ No newline at end of file diff --git a/stitch/src/Arith_BAResidual.hpp b/stitch/src/Arith_BAResidual.hpp index d3e88512..ffc6b15e 100644 --- a/stitch/src/Arith_BAResidual.hpp +++ b/stitch/src/Arith_BAResidual.hpp @@ -68,7 +68,49 @@ private: }; +// H矩阵Frobenius范数正则化残差：约束整个H矩阵与初始值的差异 +// 使用Frobenius范数衡量H矩阵的整体变化 +struct HomographyRegularizationResidual +{ + HomographyRegularizationResidual(const double* initial_h, double weight = 1.0) + : initial_h_(initial_h), weight_(weight) + { + } + + template + bool operator()(const T* const h, T* residual) const + { + // 计算H矩阵与初始H矩阵的Frobenius范数差异 + // Frobenius范数：||H - H0||_F = sqrt(sum((h_i - h_i_0)^2)) + // 由于H矩阵最后一个元素固定为1.0，所以只计算前8个参数的差异 + + T diff_sum = T(0); + for (int i = 0; i < 8; ++i) + { + T diff = h[i] - T(initial_h_[i]); + diff_sum += diff * diff; + } + + // 添加小的epsilon避免数值不稳定，确保diff_sum >= epsilon + const T epsilon = T(1e-10); + diff_sum = diff_sum < epsilon ? epsilon : diff_sum; + + // Frobenius范数（平方根） + T frobenius_norm = sqrt(diff_sum); + + // 返回加权后的Frobenius范数作为残差 + // 添加检查避免NaN和Inf + T weighted_norm = T(weight_) * frobenius_norm; + // 使用条件表达式避免NaN/Inf传播 + residual[0] = (weighted_norm != weighted_norm || weighted_norm > T(1e10)) ? T(0) : weighted_norm; + + return true; + } +private: + const double* initial_h_; // 初始H矩阵参数值（8个参数） + double weight_; // 正则化权重 +}; // SE3群优化 // SE3群优化残差：将左图三角化后投影到右图，最小化相对投影误差 diff --git a/stitch/src/Arith_BATask.cpp b/stitch/src/Arith_BATask.cpp index d269ef02..3b115c2b 100644 --- a/stitch/src/Arith_BATask.cpp +++ b/stitch/src/Arith_BATask.cpp @@ -158,6 +158,27 @@ void BA_Task::OptFrame(vector frameInd,cv::Mat H_map, std::unordered_ma } } +#ifdef OPT_H + // 添加正则化残差块，使用Frobenius范数约束整个H矩阵 + // 正则化权重：可以根据需要调整，值越大约束越强 + // 初始设置较强的约束，后续可以根据效果慢慢放松 + double h_reg_weight = 20.0; // H矩阵Frobenius范数正则化权重（加强约束） + + // 为每个帧添加H矩阵正则化 + for (int i = 0; i < _currMatrix.size(); i++) + { + // 获取初始H矩阵的8个参数 + double* initial_h = (double*)_origMatrix[i].data; + ceres::CostFunction* h_reg_cost = + new ceres::AutoDiffCostFunction( + new HomographyRegularizationResidual(initial_h, h_reg_weight)); + // 使用HuberLoss，使正则化项对异常值更鲁棒 + // 可以调整阈值：值越小约束越严格，值越大约束越宽松 + ceres::LossFunction* reg_loss = new ceres::HuberLoss(0.2); + problemH.AddResidualBlock(h_reg_cost, reg_loss, h_list[i]); + } +#endif + #ifdef OPT_SE3 // 添加正则化残差块，约束所有参数不要偏离初始值太多 // 正则化权重：可以根据需要调整，值越大约束越强 @@ -231,11 +252,11 @@ void BA_Task::OptFrame(vector frameInd,cv::Mat H_map, std::unordered_ma auto Key = _frameInd[i]; // 将优化se3写入H - #ifdef OPT_SE3 + #ifdef OPT_SE3 // 使用优化后的高度值 double optimized_height = height_params[i]; _currMatrix[i] = Transe3ToH((double*)_currse3[i].data, constK, optimized_height); - #endif + #endif // 更新缓存 updateCacheH(Key, _currMatrix[i], global_H); diff --git a/stitch/src/Arith_GeoSolver.cpp b/stitch/src/Arith_GeoSolver.cpp old mode 100644 new mode 100755 index 7260869a..abfad0d5 --- a/stitch/src/Arith_GeoSolver.cpp +++ b/stitch/src/Arith_GeoSolver.cpp @@ -1,469 +1,478 @@ -#include "Arith_GeoSolver.h" -#include "Arith_UnderStitch.h" -#include "Arith_Utils.h" -#include "Arith_CoordModule.h" -#include "Arith_SysStruct.h" -#define GLOG_USE_GLOG_EXPORT -#include "ceres/ceres.h" -#include "ceres/rotation.h" -using namespace cv; - -GeoSolver::GeoSolver() -{ -} - -GeoSolver::~GeoSolver() -{ -} - - - -//cv::Point2f GeoSolver::project(cv::Point2f pos_pan, Proj m_Proj, PanInfo pan) -//{ -// cv::Point2f pos_geo = Trans_pan2Geo(pos_pan, pan); -// cv::Point2f pos_frame = Trans_Geo2uv(pos_geo, m_Proj.tf_g2p); -// return pos_frame; -//} -// -//cv::Point2f GeoSolver::back_project(cv::Point2f pos_frame, Proj m_Proj, PanInfo pan) -//{ -// cv::Point2f pos_geo = Trans_uv2Geo(pos_frame, m_Proj.tf_p2g); -// -// cv::Point2f pos_pan = Trans_Geo2pan(pos_geo, pan); -// -// return pos_pan; -//} - -void GeoSolver::SetOriginPoint(FrameInfo info) -{ - originCGCSPoint = getXYZFromBLH(info.craft.stPos); - origininfo = info; - return; -} - -PointXYZ GeoSolver::GetOriginPoint() -{ - return originCGCSPoint; -} - -int GeoSolver::GetOriginDHeight() -{ - return origininfo.nEvHeight; -} - - -PointBLH GeoSolver::getBLHFromFrame(cv::Mat H, cv::Point2f pt) -{ - cv::Point2f pt_Geo = warpPointWithH(H, pt); - return getBLHFromGeo(pt_Geo); -} - - -PointBLH GeoSolver::getBLHFromGeo(cv::Point2f ptInGeo) -{ - PointXYZ ptNUE = { 0 }; - // 本模块使用的地理系是东(x)-北(y)-地(z),需要转换一下 - ptNUE.X = ptInGeo.y; - ptNUE.Y = -origininfo.nEvHeight; - ptNUE.Z = ptInGeo.x; - PointBLH res = getBLHFromXYZ(getCGCSXYZFromNUEXYZ(ptNUE, originCGCSPoint)); - return res; -} - -cv::Point2f GeoSolver::getGeoFromBLH(PointBLH ptPos) -{ - cv::Point2f ptInGeo = { 0 }; - PointXYZ ptNUE = { 0 }; - ptNUE = getNUEXYZFromCGCSXYZ(getXYZFromBLH(ptPos), originCGCSPoint); - // 本模块使用的地理系是东(x)-北(y)-地(z),需要转换一下 - ptInGeo.x = ptNUE.Z; - ptInGeo.y = ptNUE.X; - - return ptInGeo; -} - -cv::Point2f GeoSolver::Trans_uv2Geo(cv::Point2f pos_frame, TForm form) -{ - Mat point = (Mat_(3, 1) << pos_frame.x, pos_frame.y, 1); - Mat result = form.R * point; - // 转局部地理系 - double warpedX = result.at(0, 0) / result.at(2, 0); - double warpedY = result.at(1, 0) / result.at(2, 0); - // 平移到原点地理系 - warpedX += form.T.at(0, 0); - warpedY += form.T.at(1, 0); - - return cv::Point2f(warpedX, warpedY); -} - -cv::Point2f GeoSolver::Trans_Geo2uv(cv::Point2f pos_geo, TForm form_inv) -{ - // 先平移到当前相机位置 - cv::Point2f pos_cam = pos_geo; - pos_cam.x = pos_geo.x + form_inv.T.at(0, 0); - pos_cam.y = pos_geo.y + form_inv.T.at(1, 0); - - Mat point = (Mat_(3, 1) << pos_cam.x, pos_cam.y, 1); - Mat result = form_inv.R * point; - - // 转像方 - double warpedX = result.at(0, 0) / result.at(2, 0); - double warpedY = result.at(1, 0) / result.at(2, 0); - - return cv::Point2f(warpedX, warpedY); -} - - -Mat GeoSolver::Mat_TransENGMove(FrameInfo info) -{ - PointXYZ ptCurr = getXYZFromBLH(info.craft.stPos); - PointXYZ diff = getNUEXYZFromCGCSXYZ(ptCurr, originCGCSPoint); - - Mat move = Mat::zeros(3, 1, CV_64F); - - move.at(0, 0) = diff.Z; - move.at(1, 0) = diff.X; - move.at(2, 0) = -diff.Y; - return move; -} - -Mat GeoSolver::Mat_TransENG2uv(FrameInfo info) -{ - //从地理坐标系转像素坐标 - //[u,v,1]'=Z*M*[X,Y,DH]' = Z*M*[1,0,0;0,1,0;0,0,DH]'*[X,Y,1]' - //[u,v,1]'=Z*M(内参)*M(alaph)*M(beta)*M(roll)*M(pitch)*M(yaw)*[X,Y,DH] - - // 深度矩阵 - Mat M_het = (Mat_(3, 3) << 1, 0, 0, - 0, 1, 0, - 0, 0, info.nEvHeight - ); - - Mat M = Mat_GetCamK(info) * Mat_TransENG2Cam(info) * M_het; - - return M; -} - -using namespace std; -cv::Mat GeoSolver::Mat_TransENG2Cam(FrameInfo info) -{ - - float yaw = info.craft.stAtt.fYaw; - Mat M_yaw = (Mat_(3, 3) << cosd(yaw), -sind(yaw), 0, - sind(yaw), cosd(yaw), 0, - 0, 0, 1 - ); - - float pit = info.craft.stAtt.fPitch; - Mat M_pitch = (Mat_(3, 3) << 1, 0, 0, - 0, cosd(pit), -sind(pit), - 0, sind(pit), cosd(pit) - ); - - /* 1 0 0 - 0 cos sin - 0 -sin cos - */ - - float roll = info.craft.stAtt.fRoll; - Mat M_roll = (Mat_(3, 3) << cosd(roll), 0, sind(roll), - 0, 1, 0, - -sind(roll), 0, cosd(roll) - ); - - - float beta = info.servoInfo.fServoAz; - - Mat M_beta = (Mat_(3, 3) << cosd(beta), -sind(beta), 0, - sind(beta), cosd(beta), 0, - 0, 0, 1 - ); - - - float alaph = info.servoInfo.fServoPt; - - Mat M_alaph = (Mat_(3, 3) << 1, 0, 0, - 0, cosd(alaph), -sind(alaph), - 0, sind(alaph), cosd(alaph) - - ); - - Mat M =M_alaph * M_beta * M_roll * M_pitch * M_yaw; - - return M; -} - -cv::Mat GeoSolver::Mat_GetCamK(FrameInfo info) -{ - // 内参 - FLOAT32 fd = info.camInfo.nFocus / info.camInfo.fPixelSize * 1000; - - Mat M_cam = (Mat_(3, 3) << fd, 0, info.nWidth / 2, - 0, -fd, info.nHeight / 2, - 0, 0, 1 - ); - - return M_cam; -} - -RtK GeoSolver::AnlayseRtK(FrameInfo info) -{ - RtK rtk; - rtk.R = Mat_TransENG2Cam(info); - rtk.T = Mat_TransENGMove(info); - rtk.K = Mat_GetCamK(info); - return rtk; -} - -cv::Mat TransRtKToH(RtK rtk, double Depth) -{ - // 深度矩阵 - Mat M_het = (Mat_(3, 3) << 1, 0, 0, - 0, 1, 0, - 0, 0, Depth - ); - - cv::Mat R_p2g = (rtk.K * rtk.R * M_het).inv(); - //cv::Mat R_p2g = R_g2p.inv(); - cv::Mat t_MAT = rtk.T; - - double R[9] = { 0 }; - double T[3] = { 0 }; - - memcpy(R, R_p2g.data, sizeof(double) * 9); - memcpy(T, t_MAT.data, sizeof(double) * 3); - - Mat H = (Mat_(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]), - (R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]), - R[6], R[7], R[8]); - - // 归一化 - H = H / H.at(2, 2); - - return H.clone(); -} - -cv::Mat Transe3ToH(double* se3, cv::Mat K, double Depth) -{ - RtK rtk; - cv::Rodrigues(cv::Mat(3, 1, CV_64FC1, se3), rtk.R); - - - //cv::Mat w, u, vt; - //cv::SVD::compute(rtk.R, w, u, vt); - - //// 核心：确保 u * vt 是精确的正交矩阵 - //// 而不是直接对R进行尺度归一化 - //rtk.R = u * vt; - - rtk.T = cv::Mat(3, 1, CV_64FC1, se3 + 3); - - rtk.K = K; - - return TransRtKToH(rtk, Depth); -} - - -// 检验H的计算 -cv::Mat GeoSolver::findHomography2(FrameInfo info) -{ - TForm tf_p2g; - tf_p2g.R = Mat_TransENG2uv(info).inv(); - tf_p2g.T = Mat_TransENGMove(info); - - std::vector srcPoints; - srcPoints.push_back(cv::Point2f(0, 0)); - srcPoints.push_back(cv::Point2f(1000, 0)); - srcPoints.push_back(cv::Point2f(1000, 1000)); - srcPoints.push_back(cv::Point2f(0, 1000)); - - // 同名点计算，从像方到全景 - cv::Point2f leftTop_map = Trans_uv2Geo(srcPoints[0], tf_p2g); - cv::Point2f rightTop_map = Trans_uv2Geo(srcPoints[1], tf_p2g); - cv::Point2f rightBottom_map = Trans_uv2Geo(srcPoints[2], tf_p2g); - cv::Point2f leftBottom_map = Trans_uv2Geo(srcPoints[3], tf_p2g); - - // 目标图像（全景图）的四个顶点坐标 - std::vector dstPoints; - dstPoints.push_back(leftTop_map); // 左 - dstPoints.push_back(rightTop_map); // 右上 - dstPoints.push_back(rightBottom_map); // 右下 - dstPoints.push_back(leftBottom_map); // 左下 - - // 计算单应性矩阵 H - cv::Mat H = cv::findHomography(srcPoints, dstPoints); - return H; -} - - -// 根据R和t，解析H -cv::Mat GeoSolver::findHomography(FrameInfo info) -{ - TForm tf_p2g; - tf_p2g.R = Mat_TransENG2uv(info).inv(); - tf_p2g.T = Mat_TransENGMove(info); - - double R[9] = { 0 }; - double T[3] = { 0 }; - - memcpy(R, tf_p2g.R.data, sizeof(double) * 9); - memcpy(T, tf_p2g.T.data, sizeof(double) * 3); - - Mat H = (Mat_(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]), - (R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]), - R[6], R[7], R[8]); - - - // 归一化 - H = H / H.at(2, 2); - - - return H; -} - -cv::Mat GeoSolver::findHomography_GPT(FrameInfo info) -{ - // world: Z=0 plane - cv::Mat R = Mat_TransENG2uv(info).inv(); // 3x3 - cv::Mat t = Mat_TransENGMove(info); // 3x1 - - // ground plane normal in world coord - cv::Mat n_w = (cv::Mat_(3,1) << 0, 0, 1); - - // normal in camera coord - cv::Mat n = R * n_w; // 3x1 - - // distance to ground plane - double d = info.nEvHeight; - - - cv::Mat H = R - (t * n.t()) / d; - - H /= H.at(2,2); - return H; -} - - -cv::Mat GeoSolver::findHomography(FrameInfo info, RtK& rtk) -{ - rtk = AnlayseRtK(info); - - cv::Mat H = TransRtKToH(rtk,info.nEvHeight); - - return H; -} - - -// 计算多边形的面积 -double polygonArea(const vector& points) -{ - double area = 0.0; - int n = points.size(); - for (int i = 0; i < n; i++) { - int j = (i + 1) % n; - area += points[i].x * points[j].y - points[j].x * points[i].y; - } - return abs(area) / 2.0; -} - -// 计算两个四边形的 IOU -double computeQuadrilateralIOU(const vector& quad1, const vector& quad2) -{ - // 将四边形转换为多边形 - vector poly1 = quad1; - vector poly2 = quad2; - - // 计算交集多边形 - vector intersectionPolygon; - intersectConvexConvex(poly1, poly2, intersectionPolygon); - - // 计算面积 - double area1 = polygonArea(poly1); - double area2 = polygonArea(poly2); - double intersectionArea = polygonArea(intersectionPolygon); - double unionArea = area1 + area2 - intersectionArea; - - // 计算 IOU - if (unionArea == 0) return 0.0; // 避免除零错误 - return intersectionArea / unionArea; -} - -cv::Point2f warpPointWithH(cv::Mat H, cv::Point2f srcPt) -{ - Mat point = (Mat_(3, 1) << srcPt.x, srcPt.y, 1); - Mat result = H * point; - // 转局部地理系 - double warpedX = result.at(0, 0) / result.at(2, 0); - double warpedY = result.at(1, 0) / result.at(2, 0); - - return cv::Point2f(warpedX,warpedY); -} - - - -vector warpRectWithH(cv::Mat H,cv::Size size) -{ - vector _res; - _res.push_back(warpPointWithH(H, cv::Point2f(0,0))); - _res.push_back(warpPointWithH(H, cv::Point2f(size.width,0))); - _res.push_back(warpPointWithH(H, cv::Point2f(size.width,size.height))); - _res.push_back(warpPointWithH(H, cv::Point2f(0,size.height))); - return _res; -} - -cv::Rect2f warpRectWithH_2Rect(cv::Mat H, cv::Size size) -{ - vector dstCorners = warpRectWithH(H, size); - - // 找到变换后图像的边界 - float minX = MIN(MIN(dstCorners[0].x, dstCorners[1].x), MIN(dstCorners[2].x, dstCorners[3].x)); - float minY = MIN(MIN(dstCorners[0].y, dstCorners[1].y), MIN(dstCorners[2].y, dstCorners[3].y)); - float maxX = MAX(MAX(dstCorners[0].x, dstCorners[1].x), MAX(dstCorners[2].x, dstCorners[3].x)); - float maxY = MAX(MAX(dstCorners[0].y, dstCorners[1].y), MAX(dstCorners[2].y, dstCorners[3].y)); - - - // 变换后结果的rect - cv::Rect2f roi(minX, minY, maxX - minX, maxY - minY); - - return roi; -} - - -cv::Point2f warpPointWithse3(const double* se3, double depth, cv::Mat K, cv::Point2f srcPt) -{ - Mat point = (Mat_(3, 1) << srcPt.x, srcPt.y, 1.0); - - // 轴角转R - cv::Mat rvec = (Mat_(3, 1) << -se3[0], -se3[1], -se3[2]); - cv::Mat tvec = (Mat_(3, 1) << se3[3], se3[4], se3[5]); - cv::Mat R; - cv::Rodrigues(rvec, R); - - - // 深度矩阵 - Mat M_het_inv = (Mat_(3, 3) << 1, 0, 0, - 0, 1, 0, - 0, 0, 1.0/depth - ); - - // 转地理系 - Mat result = M_het_inv * R * K.inv() * point; - - - // 说明ceres::AngleAxisRotatePoint有效，结果等效于cv::Rodrigues - //double resP[3] = { 0 }; - //cv::Mat pt = (K.inv() * point); - //ceres::AngleAxisRotatePoint((double*)rvec.data, (double*)pt.data, (double*)resP); - //result = M_het_inv * cv::Mat(3, 1, CV_64FC1, resP); - - - // 转局部地理系 - double warpedX = result.at(0, 0) / result.at(2, 0); - double warpedY = result.at(1, 0) / result.at(2, 0); - - warpedX += tvec.at(0, 0); - warpedY += tvec.at(1, 0); - - - return cv::Point2f(warpedX,warpedY); -} +#include "Arith_GeoSolver.h" +#include "Arith_UnderStitch.h" +#include "Arith_Utils.h" +#include "Arith_CoordModule.h" +#include "Arith_SysStruct.h" +//#define GLOG_USE_GLOG_EXPORT + +using namespace cv; + +GeoSolver::GeoSolver() +{ +} + +GeoSolver::~GeoSolver() +{ +} + + + +//cv::Point2f GeoSolver::project(cv::Point2f pos_pan, Proj m_Proj, PanInfo pan) +//{ +// cv::Point2f pos_geo = Trans_pan2Geo(pos_pan, pan); +// cv::Point2f pos_frame = Trans_Geo2uv(pos_geo, m_Proj.tf_g2p); +// return pos_frame; +//} +// +//cv::Point2f GeoSolver::back_project(cv::Point2f pos_frame, Proj m_Proj, PanInfo pan) +//{ +// cv::Point2f pos_geo = Trans_uv2Geo(pos_frame, m_Proj.tf_p2g); +// +// cv::Point2f pos_pan = Trans_Geo2pan(pos_geo, pan); +// +// return pos_pan; +//} + +void GeoSolver::SetOriginPoint(FrameInfo info) +{ + originCGCSPoint = getXYZFromBLH(info.craft.stPos); + origininfo = info; + return; +} + +PointXYZ GeoSolver::GetOriginPoint() +{ + return originCGCSPoint; +} + +int GeoSolver::GetOriginDHeight() +{ + return origininfo.nEvHeight; +} + + +PointBLH GeoSolver::getBLHFromFrame(cv::Mat H, cv::Point2f pt) +{ + cv::Point2f pt_Geo = warpPointWithH(H, pt); + return getBLHFromGeo(pt_Geo); +} + + +PointBLH GeoSolver::getBLHFromGeo(cv::Point2f ptInGeo) +{ + PointXYZ ptNUE = { 0 }; + // 本模块使用的地理系是东(x)-北(y)-地(z),需要转换一下 + ptNUE.X = ptInGeo.y; + ptNUE.Y = -origininfo.nEvHeight; + ptNUE.Z = ptInGeo.x; + PointBLH res = getBLHFromXYZ(getCGCSXYZFromNUEXYZ(ptNUE, originCGCSPoint)); + return res; +} + +cv::Point2f GeoSolver::getGeoFromBLH(PointBLH ptPos) +{ + cv::Point2f ptInGeo = { 0 }; + PointXYZ ptNUE = { 0 }; + ptNUE = getNUEXYZFromCGCSXYZ(getXYZFromBLH(ptPos), originCGCSPoint); + // 本模块使用的地理系是东(x)-北(y)-地(z),需要转换一下 + ptInGeo.x = ptNUE.Z; + ptInGeo.y = ptNUE.X; + + return ptInGeo; +} + +// cv::Point2f GeoSolver::Trans_uv2Geo(cv::Point2f pos_frame, TForm form) +// { +// Mat point = (Mat_(3, 1) << pos_frame.x, pos_frame.y, 1); +// Mat result = form.R * point; +// // 转局部地理系 +// double warpedX = result.at(0, 0) / result.at(2, 0); +// double warpedY = result.at(1, 0) / result.at(2, 0); +// // 平移到原点地理系 +// warpedX += form.T.at(0, 0); +// warpedY += form.T.at(1, 0); + +// return cv::Point2f(warpedX, warpedY); +// } + +// cv::Point2f GeoSolver::Trans_Geo2uv(cv::Point2f pos_geo, TForm form_inv) +// { +// // 先平移到当前相机位置 +// cv::Point2f pos_cam = pos_geo; +// pos_cam.x = pos_geo.x + form_inv.T.at(0, 0); +// pos_cam.y = pos_geo.y + form_inv.T.at(1, 0); + +// Mat point = (Mat_(3, 1) << pos_cam.x, pos_cam.y, 1); +// Mat result = form_inv.R * point; + +// // 转像方 +// double warpedX = result.at(0, 0) / result.at(2, 0); +// double warpedY = result.at(1, 0) / result.at(2, 0); + +// return cv::Point2f(warpedX, warpedY); +// } + + +Mat GeoSolver::Mat_TransENGMove(FrameInfo info) +{ + PointXYZ ptCurr = getXYZFromBLH(info.craft.stPos); + PointXYZ diff = getNUEXYZFromCGCSXYZ(ptCurr, originCGCSPoint); + + Mat move = Mat::zeros(3, 1, CV_64F); + + move.at(0, 0) = diff.Z; + move.at(1, 0) = diff.X; + move.at(2, 0) = -diff.Y; + return move; +} + +Mat GeoSolver::Mat_TransENG2uv(FrameInfo info) +{ + //从地理坐标系转像素坐标 + //[u,v,1]'=Z*M*[X,Y,DH]' = Z*M*[1,0,0;0,1,0;0,0,DH]'*[X,Y,1]' + //[u,v,1]'=Z*M(内参)*M(alaph)*M(beta)*M(roll)*M(pitch)*M(yaw)*[X,Y,DH] + + // 深度矩阵 + Mat M_het = (Mat_(3, 3) << 1, 0, 0, + 0, 1, 0, + 0, 0, info.nEvHeight + ); + + Mat M = Mat_GetCamK(info) * Mat_TransENG2Cam(info) * M_het; + + return M; +} + +using namespace std; +cv::Mat GeoSolver::Mat_TransENG2Cam(FrameInfo info) +{ + + float yaw = info.craft.stAtt.fYaw; + Mat M_yaw = (Mat_(3, 3) << cosd(yaw), -sind(yaw), 0, + sind(yaw), cosd(yaw), 0, + 0, 0, 1 + ); + + float pit = info.craft.stAtt.fPitch; + Mat M_pitch = (Mat_(3, 3) << 1, 0, 0, + 0, cosd(pit), -sind(pit), + 0, sind(pit), cosd(pit) + ); + + /* 1 0 0 + 0 cos sin + 0 -sin cos + */ + + float roll = info.craft.stAtt.fRoll; + Mat M_roll = (Mat_(3, 3) << cosd(roll), 0, sind(roll), + 0, 1, 0, + -sind(roll), 0, cosd(roll) + ); + + + float beta = info.servoInfo.fServoAz; + + Mat M_beta = (Mat_(3, 3) << cosd(beta), -sind(beta), 0, + sind(beta), cosd(beta), 0, + 0, 0, 1 + ); + + + + + float alaph = info.servoInfo.fServoPt; + + Mat M_alaph = (Mat_(3, 3) << 1, 0, 0, + 0, cosd(alaph), -sind(alaph), + 0, sind(alaph), cosd(alaph) + + ); + + Mat M =M_alaph * M_beta * M_roll * M_pitch * M_yaw; + + return M; +} + +cv::Mat GeoSolver::Mat_GetCamK(FrameInfo info) +{ + // 内参 + FLOAT32 fd = info.camInfo.nFocus / info.camInfo.fPixelSize * 1000; + + Mat M_cam = (Mat_(3, 3) << fd, 0, info.nWidth / 2, + 0, -fd, info.nHeight / 2, + 0, 0, 1 + ); + + return M_cam; +} + +RtK GeoSolver::AnlayseRtK(FrameInfo info) +{ + RtK rtk; + rtk.R = Mat_TransENG2Cam(info); + rtk.T = Mat_TransENGMove(info); + rtk.K = Mat_GetCamK(info); + return rtk; +} + +cv::Mat TransRtKToH(RtK rtk, double Depth) +{ + // 深度矩阵 + Mat M_het = (Mat_(3, 3) << 1, 0, 0, + 0, 1, 0, + 0, 0, Depth + ); + + // cv::Mat R_p2g = (rtk.K * rtk.R * M_het).inv(); + // //cv::Mat R_p2g = R_g2p.inv(); + // cv::Mat t_MAT = rtk.T; + + // double R[9] = { 0 }; + // double T[3] = { 0 }; + + // memcpy(R, R_p2g.data, sizeof(double) * 9); + // memcpy(T, t_MAT.data, sizeof(double) * 3); + + // Mat H = (Mat_(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]), + // (R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]), + // R[6], R[7], R[8]); + + // // 归一化 + // H = H / H.at(2, 2); + + // 地面法向量 n_w（向上方向，Z轴正方向，在地理坐标系中） + cv::Mat n_w = (cv::Mat_(3, 1) << 0, 0, 1); + + // 平面到相机的距离（有效高度） + double d = Depth; + + cv::Mat I = cv::Mat::eye(3, 3, CV_64F); + + // 计算 (T_geo * n_w^T) / d + cv::Mat TnT = (rtk.R * rtk.T *n_w.t()) / d; + + + + + // 计算从地理坐标到像素坐标的单应性矩阵 + cv::Mat H_geo_to_pixel = rtk.K * (rtk.R - TnT) * M_het; + + cv::Mat H = H_geo_to_pixel.inv(); + + // 归一化（使 H[2,2] = 1） + H = H / H.at(2, 2); + + return H.clone(); +} + +cv::Mat Transe3ToH(double* se3, cv::Mat K, double Depth) +{ + RtK rtk; + cv::Rodrigues(cv::Mat(3, 1, CV_64FC1, se3), rtk.R); + + + //cv::Mat w, u, vt; + //cv::SVD::compute(rtk.R, w, u, vt); + + //// 核心：确保 u * vt 是精确的正交矩阵 + //// 而不是直接对R进行尺度归一化 + //rtk.R = u * vt; + + rtk.T = cv::Mat(3, 1, CV_64FC1, se3 + 3); + + rtk.K = K; + + return TransRtKToH(rtk, Depth); +} + + + +// 根据R和t，解析H +// cv::Mat GeoSolver::findHomography(FrameInfo info) +// { +// cv::Mat Rmat = Mat_TransENG2uv(info).inv(); +// cv::Mat Tmat = Mat_TransENGMove(info); + +// double R[9] = { 0 }; +// double T[3] = { 0 }; + +// memcpy(R, Rmat.data, sizeof(double) * 9); +// memcpy(T, Tmat.data, sizeof(double) * 3); + +// Mat H = (Mat_(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]), +// (R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]), +// R[6], R[7], R[8]); + + +// // 归一化 +// H = H / H.at(2, 2); + + +// return H; +// } + +cv::Mat GeoSolver::findHomography(FrameInfo info) +{ + // 获取相机内参矩阵 K + cv::Mat Kmat = Mat_GetCamK(info); + + // 获取从地理坐标系到相机坐标系的旋转矩阵 R_geo_to_cam + cv::Mat R_geo_to_cam = Mat_TransENG2Cam(info); + + // 获取平移向量 T（从原点地理坐标系到当前相机位置地理坐标系） + cv::Mat T_geo = Mat_TransENGMove(info); + + // 深度矩阵 + Mat M_het = (Mat_(3, 3) << 1, 0, 0, + 0, 1, 0, + 0, 0, info.nEvHeight + ); + + // 地面法向量 n_w（向上方向，Z轴正方向，在地理坐标系中） + cv::Mat n_w = (cv::Mat_(3, 1) << 0, 0, 1); + + // 平面到相机的距离（有效高度） + double d = info.nEvHeight; + + cv::Mat I = cv::Mat::eye(3, 3, CV_64F); + + // 计算 (T_geo * n_w^T) / d + cv::Mat TnT = (R_geo_to_cam * T_geo *n_w.t()) / d; + + + // 计算从地理坐标到像素坐标的单应性矩阵 + cv::Mat H_geo_to_pixel = Kmat * (R_geo_to_cam - TnT) * M_het; + + cv::Mat H = H_geo_to_pixel.inv(); + + // 归一化（使 H[2,2] = 1） + H = H / H.at(2, 2); + + return H.clone(); +} + + +cv::Mat GeoSolver::findHomography(FrameInfo info, RtK& rtk) +{ + rtk = AnlayseRtK(info); + + cv::Mat H = TransRtKToH(rtk,info.nEvHeight); + + return H; +} + + +// 计算多边形的面积 +double polygonArea(const vector& points) +{ + double area = 0.0; + int n = points.size(); + for (int i = 0; i < n; i++) { + int j = (i + 1) % n; + area += points[i].x * points[j].y - points[j].x * points[i].y; + } + return abs(area) / 2.0; +} + +// 计算两个四边形的 IOU +double computeQuadrilateralIOU(const vector& quad1, const vector& quad2) +{ + // 将四边形转换为多边形 + vector poly1 = quad1; + vector poly2 = quad2; + + // 计算交集多边形 + vector intersectionPolygon; + intersectConvexConvex(poly1, poly2, intersectionPolygon); + + // 计算面积 + double area1 = polygonArea(poly1); + double area2 = polygonArea(poly2); + double intersectionArea = polygonArea(intersectionPolygon); + double unionArea = area1 + area2 - intersectionArea; + + // 计算 IOU + if (unionArea == 0) return 0.0; // 避免除零错误 + return intersectionArea / unionArea; +} + +cv::Point2f warpPointWithH(cv::Mat H, cv::Point2f srcPt) +{ + Mat point = (Mat_(3, 1) << srcPt.x, srcPt.y, 1); + Mat result = H * point; + // 转局部地理系 + double warpedX = result.at(0, 0) / result.at(2, 0); + double warpedY = result.at(1, 0) / result.at(2, 0); + + return cv::Point2f(warpedX,warpedY); +} + + + +vector warpRectWithH(cv::Mat H,cv::Size size) +{ + vector _res; + _res.push_back(warpPointWithH(H, cv::Point2f(0,0))); + _res.push_back(warpPointWithH(H, cv::Point2f(size.width,0))); + _res.push_back(warpPointWithH(H, cv::Point2f(size.width,size.height))); + _res.push_back(warpPointWithH(H, cv::Point2f(0,size.height))); + return _res; +} + +cv::Rect2f warpRectWithH_2Rect(cv::Mat H, cv::Size size) +{ + vector dstCorners = warpRectWithH(H, size); + + // 找到变换后图像的边界 + float minX = MIN(MIN(dstCorners[0].x, dstCorners[1].x), MIN(dstCorners[2].x, dstCorners[3].x)); + float minY = MIN(MIN(dstCorners[0].y, dstCorners[1].y), MIN(dstCorners[2].y, dstCorners[3].y)); + float maxX = MAX(MAX(dstCorners[0].x, dstCorners[1].x), MAX(dstCorners[2].x, dstCorners[3].x)); + float maxY = MAX(MAX(dstCorners[0].y, dstCorners[1].y), MAX(dstCorners[2].y, dstCorners[3].y)); + + + // 变换后结果的rect + cv::Rect2f roi(minX, minY, maxX - minX, maxY - minY); + + return roi; +} + + +cv::Point2f warpPointWithse3(const double* se3, double depth, cv::Mat K, cv::Point2f srcPt) +{ + Mat point = (Mat_(3, 1) << srcPt.x, srcPt.y, 1.0); + + // 轴角转R + cv::Mat rvec = (Mat_(3, 1) << -se3[0], -se3[1], -se3[2]); + cv::Mat tvec = (Mat_(3, 1) << se3[3], se3[4], se3[5]); + cv::Mat R; + cv::Rodrigues(rvec, R); + + + // 深度矩阵 + Mat M_het_inv = (Mat_(3, 3) << 1, 0, 0, + 0, 1, 0, + 0, 0, 1.0/depth + ); + + // 转地理系 + Mat result = M_het_inv * R * K.inv() * point; + + + // 说明ceres::AngleAxisRotatePoint有效，结果等效于cv::Rodrigues + //double resP[3] = { 0 }; + //cv::Mat pt = (K.inv() * point); + //ceres::AngleAxisRotatePoint((double*)rvec.data, (double*)pt.data, (double*)resP); + //result = M_het_inv * cv::Mat(3, 1, CV_64FC1, resP); + + + // 转局部地理系 + double warpedX = result.at(0, 0) / result.at(2, 0); + double warpedY = result.at(1, 0) / result.at(2, 0); + + warpedX += tvec.at(0, 0); + warpedY += tvec.at(1, 0); + + + return cv::Point2f(warpedX,warpedY); +} diff --git a/stitch/src/Arith_GeoSolver.h b/stitch/src/Arith_GeoSolver.h old mode 100644 new mode 100755 index c945cdcc..f48bdeef --- a/stitch/src/Arith_GeoSolver.h +++ b/stitch/src/Arith_GeoSolver.h @@ -1,121 +1,111 @@ - -/*********版权所有（C）2025，武汉高德红外股份有限公司*************************************** -* 文件名称： Arith_GeoStitcher.h -* 文件标识：吊舱视频帧外参计算 -* 内容摘要：基于机载下视外部参数的几何校正,支持独立工作,也可以作为特征拼接的外参初始值 -* 其它说明：【注意：为了便于推导，局部地理坐标系采用的是东-北-地坐标系，使用大地坐标转换工具时注意转换。】 -* 当前版本：V0.5 -* 创建作者：04046wcw -* 创建日期：2025/01/15 -* 包含关系： -*****************************************************************************************/ -#pragma once -#include "StitchStruct.h" - -// 像方-物方转换关系 -struct TForm -{ - cv::Mat R; - cv::Mat T; -}; - - -// 李群+K -struct RtK -{ - cv::Mat R; // world->pix - cv::Mat T; // world - cv::Mat K; // cam -}; - -class GeoSolver -{ -public: - // 构造函数 - GeoSolver(); - ~GeoSolver(); - - // 建立像方与归一化地理系的H矩阵，解析出H - cv::Mat findHomography(FrameInfo info); - - // 计算标准李群,并返回H矩阵 - cv::Mat findHomography(FrameInfo info, RtK& se3Para); - - // 测试 - cv::Mat findHomography2(FrameInfo info); - - cv::Mat findHomography_GPT(FrameInfo info); - - // 设置起始拼接点外参，返回当前光轴大地指向 - void SetOriginPoint(FrameInfo info); - - // 返回当前拼接起始大地原点 - PointXYZ GetOriginPoint(); - - // 返回起始点高差 - int GetOriginDHeight(); - - // 根据H计算原始像方的经纬度 - PointBLH getBLHFromFrame(cv::Mat H, cv::Point2f ptInFrame); - - // 计算局部地理系下目标的经纬度 - PointBLH getBLHFromGeo(cv::Point2f ptInGeo); - - // 经纬度转换为局部地理系坐标 - cv::Point2f getGeoFromBLH(PointBLH ptPos); - -public: - - // 计算当前帧李群（SE3）和投影K - RtK AnlayseRtK(FrameInfo info); - - // 帧像方-地理坐标 - cv::Point2f Trans_uv2Geo(cv::Point2f pos_frame, TForm form); - - // 地理坐标-帧像方 - cv::Point2f Trans_Geo2uv(cv::Point2f pos_geo, TForm form); - - // 机体ENG(东北地)到像方的 旋转矩阵 - cv::Mat Mat_TransENG2uv(FrameInfo info); - -public: - - // 平移矩阵，以初始化点为基准，计算当前位置在初始点的地理坐标，那么当前帧所有点的坐标做此平移 - cv::Mat Mat_TransENGMove(FrameInfo info); - - // 机体ENG(东北地)到相机系 旋转矩阵 - cv::Mat Mat_TransENG2Cam(FrameInfo info); - - // 内参 - cv::Mat Mat_GetCamK(FrameInfo info); - - PointXYZ originCGCSPoint;//成图初始点的地心地固坐标，作为拼接参考 - - FrameInfo origininfo; -}; - - - -// 李群转H矩阵，地平假设,利用深度 -cv::Mat TransRtKToH(RtK se3, double Depth); - -// 李代数转H矩阵，参见上面 李群转H矩阵 -cv::Mat Transe3ToH(double* se3, cv::Mat K, double Depth); - -// 多边形面积 -double polygonArea(const vector& points); - -// 计算两个四边形的 IOU -double computeQuadrilateralIOU(const vector& quad1, const vector& quad2); - -// H映射点 -cv::Point2f warpPointWithH(cv::Mat H,cv::Point2f srcPt); - -// H映射多边形，按照顺时针 -vector warpRectWithH(cv::Mat H,cv::Size size); - -// H映射多边形，转rect -cv::Rect2f warpRectWithH_2Rect(cv::Mat H, cv::Size size); - -// 用SE3映射 -cv::Point2f warpPointWithse3(const double* se3, double depth, cv::Mat K, cv::Point2f srcPt); + +/*********版权所有（C）2025，武汉高德红外股份有限公司*************************************** +* 文件名称： Arith_GeoStitcher.h +* 文件标识：吊舱视频帧外参计算 +* 内容摘要：基于机载下视外部参数的几何校正,支持独立工作,也可以作为特征拼接的外参初始值 +* 其它说明：【注意：为了便于推导，局部地理坐标系采用的是东-北-地坐标系，使用大地坐标转换工具时注意转换。】 +* 当前版本：V0.5 +* 创建作者：04046wcw +* 创建日期：2025/01/15 +* 包含关系： +*****************************************************************************************/ +#pragma once +#include "StitchStruct.h" + + + +// 李群+K +struct RtK +{ + cv::Mat R; // world->pix + cv::Mat T; // world + cv::Mat K; // cam +}; + +class GeoSolver +{ +public: + // 构造函数 + GeoSolver(); + ~GeoSolver(); + + // 建立像方与归一化地理系的H矩阵，解析出H (H用于从像方投影到地理，定位矩阵) + cv::Mat findHomography(FrameInfo info); + + // 计算标准李群,并返回H矩阵 + cv::Mat findHomography(FrameInfo info, RtK& se3Para); + + + // 设置起始拼接点外参，返回当前光轴大地指向 + void SetOriginPoint(FrameInfo info); + + // 返回当前拼接起始大地原点 + PointXYZ GetOriginPoint(); + + // 返回起始点高差 + int GetOriginDHeight(); + + // 根据H计算原始像方的经纬度 + PointBLH getBLHFromFrame(cv::Mat H, cv::Point2f ptInFrame); + + // 计算局部地理系下目标的经纬度 + PointBLH getBLHFromGeo(cv::Point2f ptInGeo); + + // 经纬度转换为局部地理系坐标 + cv::Point2f getGeoFromBLH(PointBLH ptPos); + +public: + + // 计算当前帧李群（SE3）和投影K + RtK AnlayseRtK(FrameInfo info); + + // // 帧像方-地理坐标 + // cv::Point2f Trans_uv2Geo(cv::Point2f pos_frame, TForm form); + + // // 地理坐标-帧像方 + // cv::Point2f Trans_Geo2uv(cv::Point2f pos_geo, TForm form); + + // 机体ENG(东北地)到像方的 旋转矩阵 + cv::Mat Mat_TransENG2uv(FrameInfo info); + +public: + + // 平移矩阵t，以初始化点为基准，计算当前位置在初始点的地理坐标，那么当前帧所有点的坐标做此平移 + cv::Mat Mat_TransENGMove(FrameInfo info); + + // 机体ENG(东北地)到相机系 旋转矩阵R + cv::Mat Mat_TransENG2Cam(FrameInfo info); + + // 内参 K + cv::Mat Mat_GetCamK(FrameInfo info); + + PointXYZ originCGCSPoint;//成图初始点的地心地固坐标，作为拼接参考 + + FrameInfo origininfo; +}; + + + +// 李群转H矩阵，地平假设,利用深度 +cv::Mat TransRtKToH(RtK se3, double Depth); + +// 李代数转H矩阵，参见上面 李群转H矩阵 +cv::Mat Transe3ToH(double* se3, cv::Mat K, double Depth); + +// 多边形面积 +double polygonArea(const vector& points); + +// 计算两个四边形的 IOU +double computeQuadrilateralIOU(const vector& quad1, const vector& quad2); + +// H映射点 +cv::Point2f warpPointWithH(cv::Mat H,cv::Point2f srcPt); + +// H映射多边形，按照顺时针 +vector warpRectWithH(cv::Mat H,cv::Size size); + +// H映射多边形，转rect +cv::Rect2f warpRectWithH_2Rect(cv::Mat H, cv::Size size); + +// 用SE3映射 +cv::Point2f warpPointWithse3(const double* se3, double depth, cv::Mat K, cv::Point2f srcPt); diff --git a/stitch/src/Arith_UnderStitch.cpp b/stitch/src/Arith_UnderStitch.cpp index 6f9fe615..10728a27 100644 --- a/stitch/src/Arith_UnderStitch.cpp +++ b/stitch/src/Arith_UnderStitch.cpp @@ -377,8 +377,6 @@ void UnderStitch::run_test(FrameInfo para) RtK rtk; cv::Mat H2 = _GeoSolver->findHomography(para, rtk); - - cv::Vec3d rvec; cv::Rodrigues(rtk.R, rvec); @@ -538,7 +536,7 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para) para.craft.stPos.B, para.craft.stPos.L, para.craft.stPos.H, para.craft.stAtt.fYaw, para.craft.stAtt.fPitch, para.craft.stAtt.fRoll, para.servoInfo.fServoAz, para.servoInfo.fServoPt,para.camInfo.nFocus,para.camInfo.fPixelSize); - //run_test(para); + run_test(para); // 统计扫描方向 int dir = StatScanDir(para); @@ -697,15 +695,15 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para) if (_config.bUseBA && TriggerBA(para)) { - // 异步执行BA优化 - _pThreadPool->commit( - [this]() { - OptAndOutCurrPan(_baWindowKey); - }, - "BA_Optimization" - ); - + // // 异步执行BA优化 + // _pThreadPool->commit( + // [this]() { + // OptAndOutCurrPan(_baWindowKey); + // }, + // "BA_Optimization" + // ); +OptAndOutCurrPan(_baWindowKey); } @@ -1079,7 +1077,7 @@ SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para) // 保持滑动窗口大小 - const size_t WINDOW_SIZE = 10; // 滑动窗口大小 + const size_t WINDOW_SIZE = 6; // 滑动窗口大小 if (_baWindowKey.size() > WINDOW_SIZE) { _baWindowKey.erase(_baWindowKey.begin()); diff --git a/stitch/src/Arith_UnderStitch.h b/stitch/src/Arith_UnderStitch.h index 4a49d19a..a1221d2e 100644 --- a/stitch/src/Arith_UnderStitch.h +++ b/stitch/src/Arith_UnderStitch.h @@ -14,7 +14,7 @@ // 定义扫描模式，使用扫描专用的拼接地图策略 -//#define SCAN_MODE +#define SCAN_MODE // 关键帧间邻接信息 diff --git a/stitch/src/Version.h b/stitch/src/Version.h index 7d7e762d..b6eed073 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_12_07-12.02.16"; +std::string BUILD_TIME = "BUILD_TIME 2025_12_12-09.36.33"; std::string VERSION = "BUILD_VERSION 1.0.1"; diff --git a/tests/cpp/S7215/TsDecoder.hpp b/tests/cpp/S7215/TsDecoder.hpp index eaff7c80..a73c83f7 100644 --- a/tests/cpp/S7215/TsDecoder.hpp +++ b/tests/cpp/S7215/TsDecoder.hpp @@ -151,8 +151,8 @@ public: else { UPanConfig config = {0}; - config.bUseBA = 1; - config.bOutGoogleTile = 1; + config.bUseBA = 0; + config.bOutGoogleTile = 0; config.bOutLocalPanGeoTIFF = 1; pUnderStitch->SetConfig(config); @@ -179,14 +179,14 @@ public: cv::imshow("",src); cv::waitKey(1); - if (info.nFrmID % 100 == 0) - { + // if (info.nFrmID % 100 == 0) + // { - char filename[1024] = {0}; - sprintf(filename, "%d_%02d_%02d_%02d_%04d.tif", aTelem.mSensorID, aTelem.mHour, aTelem.mMinute, aTelem.mSecond, aTelem.mMillSecond); - exportFrameGeoTIFF(src, info, filename); + // char filename[1024] = {0}; + // sprintf(filename, "%d_%02d_%02d_%02d_%04d.tif", aTelem.mSensorID, aTelem.mHour, aTelem.mMinute, aTelem.mSecond, aTelem.mMillSecond); + // exportFrameGeoTIFF(src, info, filename); - } + // } } diff --git a/tests/cpp/S7215/stitch_S7215.cpp b/tests/cpp/S7215/stitch_S7215.cpp index 91819d0c..0b29ac67 100644 --- a/tests/cpp/S7215/stitch_S7215.cpp +++ b/tests/cpp/S7215/stitch_S7215.cpp @@ -6,7 +6,9 @@ int main(int, char**) { - std::string file = std::string("F:/20251105140846_chn_10_25Hz.ts"); + //std::string file = std::string("F:/20251105140846_chn_10_25Hz.ts"); + + std::string file = std::string("/media/wang/data/20251105140846_chn_10_25Hz.ts"); TsDecoder* dec = new TsDecoder(file); dec->decoder(); diff --git a/tests/python/1.py b/tests/python/1.py deleted file mode 100644 index b6725ae0..00000000 --- a/tests/python/1.py +++ /dev/null @@ -1,78 +0,0 @@ -import os -import sys -import ctypes -from pathlib import Path -import numpy as np - -# 获取项目根目录（当前文件所在目录的父目录） -project_root = Path(__file__).parent.parent.parent -bin_dir = project_root / "Bin" - -# 添加Bin目录到Python模块搜索路径 -if str(bin_dir) not in sys.path: - sys.path.insert(0, str(bin_dir)) - -# 预加载依赖库，确保动态链接器能找到它们 -lib_guide_stitch = bin_dir / "libGuideStitch.so" -if lib_guide_stitch.exists(): - try: - ctypes.CDLL(str(lib_guide_stitch), mode=ctypes.RTLD_GLOBAL) - except Exception as e: - print(f"警告: 预加载libGuideStitch.so失败: {e}") - -# 导入模块 -from UStitcher import API_UnderStitch, FrameInfo, getHomography - -import cv2 - - -frame_info = FrameInfo() - - - -frame_info.nFrmID = 1 -frame_info.craft.stPos.B = 39 -frame_info.craft.stPos.L = 120 -frame_info.craft.stPos.H = 1000 -frame_info.camInfo.nFocus = 40 -frame_info.camInfo.fPixelSize = 12 - -frame_info.servoInfo.fServoAz = 90 -frame_info.servoInfo.fServoPt = -45 - -frame_info.nEvHeight = 1200 - -frame_info.nWidth = 1280 -frame_info.nHeight = 1024 - - - -stitcher = API_UnderStitch.Create() - - -# 先初始化（设置原点） -pan_info = stitcher.Init(frame_info) -print(f"初始化成功，全景图尺寸: {pan_info.m_pan_width} x {pan_info.m_pan_height}") - -def warpPointWithH(H, pt): - wp = H @ np.array([pt[0],pt[1],1]).T - return wp / wp[2] - -for i in range(100): - frame_info.nFrmID = i - frame_info.craft.stPos.B = 39 - frame_info.craft.stPos.L = 120 - frame_info.craft.stPos.H = 1000 - frame_info.camInfo.nFocus = 40 - frame_info.camInfo.fPixelSize = 12 - - frame_info.servoInfo.fServoAz = 90 - frame_info.servoInfo.fServoPt = -45 - - H = getHomography(frame_info) - print(f"单应性矩阵 H:\n{H}") - - wp = warpPointWithH(H, np.array([100,111])) - print(f"物方坐标:\n{wp}") - -print("done") diff --git a/tests/python/ProcDJ.py b/tests/python/ProcDJ.py index f393ac37..94ff9577 100644 --- a/tests/python/ProcDJ.py +++ b/tests/python/ProcDJ.py @@ -8,10 +8,19 @@ import numpy as np from dataclasses import dataclass from typing import List, Tuple +import os -sys.path.append("/media/wang/WORK/wangchongwu_gitea_2023/StitchVideo/Bin") +sys.path.append("../../Bin/Release") +# CUDA_BIN_DIR = r"C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v12.4\bin" +# CERES_DEP_DIR = r"D:\wangchongwu_gitea_2023\StitchVideo\3rdParty\ceres_2.2\bin" +# OPEN_CV_DEP_DIR = r"C:\Lib\opencv455\build\x64\vc14\bin" +# os.add_dll_directory(CUDA_BIN_DIR) +# os.add_dll_directory(CERES_DEP_DIR) +# os.add_dll_directory(OPEN_CV_DEP_DIR) +# os.add_dll_directory("D:/wangchongwu_gitea_2023/StitchVideo/Bin/Release") +#os.add_dll_directory("../../Bin/Release") # 导入模块 from UStitcher import API_UnderStitch, FrameInfo, UPanInfo, UPanConfig @@ -411,7 +420,7 @@ def main(): srt_path_list = [] # 修改为你的数据路径 - folder = "/media/wang/data/K2D_data/" + folder = "F:/K2D_data/" video_path_list.append(folder + "DJI_20250418153043_0006_W.MP4") srt_path_list.append(folder + "DJI_20250418153043_0006_W.srt") diff --git a/tests/python/sim_scan.py b/tests/python/sim_scan.py index 8315ad86..611eff0c 100644 --- a/tests/python/sim_scan.py +++ b/tests/python/sim_scan.py @@ -15,6 +15,10 @@ import time from dataclasses import dataclass from pathlib import Path from typing import Iterable, List, Optional, Tuple +import pprint + +pprint.pprint(sys.path) + import numpy as np @@ -23,17 +27,31 @@ import numpy as np # Environment bootstrap (reuse logic from tests/python/1.py) # ----------------------------------------------------------------------------- PROJECT_ROOT = Path(__file__).resolve().parents[2] -BIN_DIR = PROJECT_ROOT / "Bin" +# BIN_DIR = PROJECT_ROOT / "Bin/Release" + +# if str(BIN_DIR) not in sys.path: +# sys.path.insert(0, str(BIN_DIR)) + +# LIB_GUIDE = BIN_DIR / "GuideStitch.dll" +# if LIB_GUIDE.exists(): +# try: +# ctypes.CDLL(str(LIB_GUIDE), mode=ctypes.RTLD_GLOBAL) +# except OSError as exc: +# print(f"[WARN] Failed to preload {LIB_GUIDE.name}: {exc}") + +import os +sys.path.append("D:/wangchongwu_gitea_2023/StitchVideo/Bin/Release") + +CUDA_BIN_DIR = r"C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v12.4\bin" +CERES_DEP_DIR = r"D:\wangchongwu_gitea_2023\StitchVideo\3rdParty\ceres_2.2\bin" +OPEN_CV_DEP_DIR = r"C:\Lib\opencv455\build\x64\vc14\bin" + +os.add_dll_directory(CUDA_BIN_DIR) +os.add_dll_directory(CERES_DEP_DIR) +os.add_dll_directory(OPEN_CV_DEP_DIR) +os.add_dll_directory("D:/wangchongwu_gitea_2023/StitchVideo/Bin/Release") -if str(BIN_DIR) not in sys.path: - sys.path.insert(0, str(BIN_DIR)) -LIB_GUIDE = BIN_DIR / "libGuideStitch.so" -if LIB_GUIDE.exists(): - try: - ctypes.CDLL(str(LIB_GUIDE), mode=ctypes.RTLD_GLOBAL) - except OSError as exc: - print(f"[WARN] Failed to preload {LIB_GUIDE.name}: {exc}") from UStitcher import API_UnderStitch, FrameInfo, UPanConfig import cv2