wangchongwu
/
StitchVideo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

多线程读写锁，BA模块和blend模块均不支持重入

Browse Source
main
wangchongwu 3 weeks ago
parent 9387fc3586
commit 75b63e41e8
15 changed files with 249 additions and 201 deletions
Show Stats Download Patch File Download Diff File

11
CMakeLists.txt
Unescape View File

@ -5,6 +5,7 @@ set(CMAKE_CXX_STANDARD 17) # 或更高版本
set(CMAKE_CXX_STANDARD_REQUIRED ON)
IF(WIN32)
# ======== OpenCV 配置 ========
set(OpenCV_DIR "C:/lib/opencv455/build/x64/vc14/lib")
@ -24,12 +25,22 @@ IF(WIN32)
${Ceres_DIR}/include/glog)
include_directories(${Ceres_INCLUDE_DIR})
set(CERES_LIBRARIES
"${Ceres_DIR}/lib/ceres.lib"
"${Ceres_DIR}/lib/gflags.lib"
"${Ceres_DIR}/lib/glog.lib"
)
if(CMAKE_BUILD_TYPE STREQUAL "Debug")
set(CERES_LIBRARIES
"${Ceres_DIR}/lib/ceres-debug.lib"
"${Ceres_DIR}/lib/gflags_debug.lib"
"${Ceres_DIR}/lib/glogd.lib"
)
endif()
ELSE()
# ======== OpenCV 配置 ========
find_package(OpenCV REQUIRED)

223
stitch/src/Arith_BATask.cpp
Unescape View File

@ -7,6 +7,8 @@
using namespace ceres;
#include "Logger.h"
#define STABLE_X1 320
#define STABLE_Y1 256
@ -17,52 +19,42 @@ using namespace ceres;
#define STABLE_Y3 1080
// H投影残差：将左右图全都投影到全景图，最小化同名点全景图上的投影误差。
// 问题：缺少尺度约束，H矩阵优化后失去正交性 优点：形式简单
//// H投影残差：将左图投影到右图，最小化同名点全景图上的投影误差。
//// 问题：缺少尺度约束，H矩阵优化后失去正交性 优点：形式简单
struct HomographyResidual
{
HomographyResidual(const cv::KeyPoint& keypoint_i, const cv::KeyPoint& keypoint_j, const cv::Mat H1, const cv::Mat H2)
: keypoint_i_(keypoint_i), keypoint_j_(keypoint_j), Hi0_(H1), Hj0_(H2)
: keypoint_i_(keypoint_i), keypoint_j_(keypoint_j),Hi0_(H1), Hj0_(H2)
{
}
template <typename T>
bool operator()(const T* const h_i, const T* const h_j, T* residual) const
{
// 残差计算逻辑
T H_i[9] = { h_i[0], h_i[1], h_i[2],
h_i[3], h_i[4], h_i[5],
h_i[6], h_i[7], T(1.0) };
typedef Eigen::Matrix<T, 3, 1> EPoint;
typedef Eigen::Matrix<T, 3, 3> EMat;
T H_j[9] = { h_j[0], h_j[1], h_j[2],
h_j[3], h_j[4], h_j[5],
h_j[6], h_j[7], T(1.0) };
EMat Mat_H_i;
Mat_H_i << T(h_i[0]), T(h_i[1]), T(h_i[2]),
T(h_i[3]), T(h_i[4]), T(h_i[5]),
T(h_i[6]), T(h_i[7]), T(1.0);
T p_i[3] = { T(keypoint_i_.pt.x), T(keypoint_i_.pt.y), T(1.0) };
T p_j[3] = { T(keypoint_j_.pt.x), T(keypoint_j_.pt.y), T(1.0) };
EMat Mat_H_j;
Mat_H_j << T(h_j[0]), T(h_j[1]), T(h_j[2]),
T(h_j[3]), T(h_j[4]), T(h_j[5]),
T(h_j[6]), T(h_j[7]), T(1.0);
T P_i[3] = { T(0), T(0), T(0) };
T P_j[3] = { T(0), T(0), T(0) };
EPoint p_i(T(keypoint_i_.pt.x), T(keypoint_i_.pt.y), T(1.0));
EPoint p_j(T(keypoint_j_.pt.x), T(keypoint_j_.pt.y), T(1.0));
for (int row = 0; row < 3; row++)
{
for (int col = 0; col < 3; col++)
{
P_i[row] += H_i[row * 3 + col] * p_i[col];
P_j[row] += H_j[row * 3 + col] * p_j[col];
}
}
P_i[0] /= P_i[2];
P_i[1] /= P_i[2];
P_j[0] /= P_j[2];
P_j[1] /= P_j[2];
EPoint warp_i = Mat_H_i * p_i;
EPoint img_j = Mat_H_j.inverse() * warp_i;
EPoint img_j_1 = img_j / img_j[2];
// 1.添加重投影误差
residual[0] = P_i[0] - P_j[0];
residual[1] = P_i[1] - P_j[1];
// 计算残差向量
residual[0] = (img_j_1 - p_j).squaredNorm();
// 2. 不动点位置约束
@ -70,42 +62,28 @@ struct HomographyResidual
cv::Point2f pS2 = warpPointWithH(Hi0_, cv::Point2f(STABLE_X2, STABLE_Y2));
cv::Point2f pS3 = warpPointWithH(Hi0_, cv::Point2f(STABLE_X3, STABLE_Y3));
T p_1[3] = { T(STABLE_X1), T(STABLE_Y1), T(1.0) };
T p_2[3] = { T(STABLE_X2), T(STABLE_Y2), T(1.0) };
T p_3[3] = { T(STABLE_X3), T(STABLE_Y3), T(1.0) };
T P_r1[3] = { T(0), T(0), T(0) };
T P_r2[3] = { T(0), T(0), T(0) };
T P_r3[3] = { T(0), T(0), T(0) };
EPoint pS1_E{ (T)pS1.x,(T)pS1.y ,T(1.0) };
EPoint pS2_E{ (T)pS2.x,(T)pS2.y,T(1.0) };
EPoint pS3_E{ (T)pS3.x,(T)pS3.y ,T(1.0) };
for (int row = 0; row < 3; row++)
{
for (int col = 0; col < 3; col++)
{
P_r1[row] += H_i[row * 3 + col] * p_1[col];
P_r2[row] += H_i[row * 3 + col] * p_2[col];
P_r3[row] += H_i[row * 3 + col] * p_3[col];
}
}
EPoint p_1{ T(STABLE_X1), T(STABLE_Y1), T(1.0) };
EPoint p_2{ T(STABLE_X2), T(STABLE_Y2), T(1.0) };
EPoint p_3{ T(STABLE_X3), T(STABLE_Y3), T(1.0) };
P_r1[0] /= P_r1[2];
P_r1[1] /= P_r1[2];
P_r2[0] /= P_r2[2];
P_r2[1] /= P_r2[2];
EPoint P_r1 = Mat_H_i * p_1;
EPoint P_r2 = Mat_H_i * p_2;
EPoint P_r3 = Mat_H_i * p_3;
P_r3[0] /= P_r3[2];
P_r3[1] /= P_r3[2];
P_r1 /= P_r1[2];
P_r2 /= P_r2[2];
P_r3 /= P_r3[2];
// 约束中心点投影位置不变
residual[2] = (P_r1[0] - (T)pS1.x);
residual[3] = P_r1[1] - (T)pS1.y;
residual[4] = (P_r2[0] - (T)pS2.x);
residual[5] = P_r2[1] - (T)pS2.y;
residual[6] = (P_r3[0] - (T)pS3.x);
residual[7] = P_r3[1] - (T)pS3.y;
// 约束投影位置不变
residual[2] = (P_r1 - pS1_E).squaredNorm();
residual[3] = (P_r2 - pS2_E).squaredNorm();
residual[4] = (P_r3 - pS3_E).squaredNorm();
return true;
@ -121,13 +99,15 @@ private:
};
// SE3群优化
// SE3群优化残差：将左图三角化后投影到右图，最小化相对投影误差
// 问题：不是真正三角化，无深度优化。 优点：有尺度约束，标准做法，可对21个参数优化。
struct SE3Residual
{
SE3Residual(cv::KeyPoint keypoint_i, cv::KeyPoint keypoint_j,cv::Mat _K, double _fEvHeight)
: keypoint_i_(keypoint_i), keypoint_j_(keypoint_j),K(_K),EvHeight(_fEvHeight)
SE3Residual(cv::KeyPoint keypoint_i, cv::KeyPoint keypoint_j,cv::Mat _K, double _fEvHeight_i, double _fEvHeight_j)
: keypoint_i_(keypoint_i), keypoint_j_(keypoint_j),K(_K),EvHeight_i(_fEvHeight_i), EvHeight_j(_fEvHeight_j)
{
}
@ -138,12 +118,11 @@ struct SE3Residual
// 左图投影到物方
// T ptL[3] = {T(keypoint_i_.pt.x), T(keypoint_i_.pt.y)}; // 原始点
// 深度矩阵
Eigen::Matrix<T, 3, 3> M_het_inv;
M_het_inv << T(1), T(0), T(0),
T(0), T(1), T(0),
T(0), T(0), T(1.0 / EvHeight);
T(0), T(0), T(1.0 / EvHeight_i);
// K-1
Eigen::Matrix<T, 3, 3> K_1;
@ -176,18 +155,20 @@ struct SE3Residual
// 恢复尺度
T GeoX = result[0] / result[2];
T GeoY = result[1] / result[2];
T GeoZ = T(EvHeight_i);
// 平移，获得地理坐标
GeoX += se3_i[3];
GeoY += se3_i[4];
GeoZ += se3_i[5];
// 正算，投影到右图
GeoX += -se3_j[3];
GeoY += -se3_j[4];
GeoZ += -se3_j[5];
T rvec_j[3] = { se3_j[0], se3_j[1], se3_j[2] };
Eigen::Matrix<T, 3, 1> pGeoR(GeoX, GeoY, T(EvHeight));
Eigen::Matrix<T, 3, 1> pGeoR(GeoX, GeoY, GeoZ);
Eigen::Matrix<T, 3, 1> RightRota;
ceres::AngleAxisRotatePoint(rvec_j, pGeoR.data(), RightRota.data());
@ -205,26 +186,23 @@ struct SE3Residual
residual[1] = vR - T(keypoint_j_.pt.y);
return true;
}
private:
const cv::KeyPoint keypoint_i_; // 第 i 帧图像中的特征点
const cv::KeyPoint keypoint_j_; // 第 j 帧图像中的特征点
cv::Mat_<double> K; //内参
double EvHeight;//高差，用于三角化
double EvHeight_i;//高差，用于三角化
double EvHeight_j;//高差，用于三角化
};
BA_Task::BA_Task(FileCache<FrameCache>* cache)
BA_Task::BA_Task(FileCache<FrameCache>* cache, FeatureMatcher* feaProcess)
{
_cache = cache;
_FeaMatcher = new FeatureMatcher(DetectorType::SIFT, MatcherType::FLANN);
_FeaMatcher = feaProcess;
//google::InitGoogleLogging("ceres");
}
@ -253,8 +231,10 @@ void BA_Task::InitTask()
void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map, std::unordered_map <KeyType, cv::Mat>& global_H)
{
std::lock_guard<std::mutex> lock(mutex_);
// 任务容器初始化
InitTask();
@ -276,10 +256,10 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
// 整合为6D参数
std::vector<double*> se3_list(_currse3.size());
std::vector<double*> se3list(_currse3.size());
for (int i = 0; i < _currse3.size(); i++)
{
se3_list[i] = (double*)_currse3[i].data;
se3list[i] = (double*)_currse3[i].data;
}
constK = cv::Mat(3, 3, CV_64FC1, ((double*)_origRtK[0].data) + 12);
@ -287,6 +267,7 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
// 创建 Ceres 问题
ceres::Problem problemH;
ceres::Problem problemSE3;
ceres::Problem problemH2;
// 添加残差块
int nParaCnt = 0;//参数组数
@ -301,6 +282,7 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
continue;
}
std::vector<cv::DMatch> matches;
//_FeaMatcher->matchFeatures(_FeaDespVec[i],_FeaDespVec[j],matches);
_FeaMatcher->matchFeatures_WithH(_FeaPtVec[i], _FeaDespVec[i], _FeaPtVec[j], _FeaDespVec[j], _origMatrix[i], _origMatrix[j], matches);
@ -324,6 +306,8 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
for (int m = 0; m < matches.size(); m++)
{
auto mc = matches[m];
// 注意：这里不对，应该找匹配点！！ todo 节后完成
cv::KeyPoint keypoint_i = _FeaPtVec[i][mc.queryIdx];
cv::KeyPoint keypoint_j = _FeaPtVec[j][mc.trainIdx];
@ -339,16 +323,18 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
cv::Mat Hj0 = _origMatrix[j];
#ifdef OPT_H
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction<HomographyResidual, 8, 8, 8>(
new ceres::AutoDiffCostFunction<HomographyResidual, 4, 8, 8>(
new HomographyResidual(keypoint_i, keypoint_j, Hi0, Hj0));
problemH.AddResidualBlock(cost_function, scale_loss, h_list[i], h_list[j]);
problemH2.AddResidualBlock(cost_function, scale_loss, h_list[i], h_list[j]);
#endif
#else
#ifdef OPT_SE3
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction<SE3Residual,2,6,6>(
new SE3Residual(keypoint_i, keypoint_j, constK, constHeight));
problemSE3.AddResidualBlock(cost_function, scale_loss, se3_list[i],se3_list[j]);
new SE3Residual(keypoint_i, keypoint_j, constK, _depth[i], _depth[j]));
problemSE3.AddResidualBlock(cost_function, scale_loss, se3list[i], se3list[j]);
#endif
}
@ -361,38 +347,37 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
// 配置求解器
ceres::Solver::Options options;
options.max_num_iterations = 10; // 增加最大迭代次数
options.function_tolerance = 1e-5; // 设置更严格的函数值容忍度
options.gradient_tolerance = 1e-5; // 设置更严格的梯度容忍度
options.parameter_tolerance = 1e-5; // 设置更严格的参数容忍度
options.function_tolerance = 1e-15; // 设置更严格的函数值容忍度
options.gradient_tolerance = 1e-15; // 设置更严格的梯度容忍度
options.parameter_tolerance = 1e-15; // 设置更严格的参数容忍度
options.minimizer_progress_to_stdout = true;
//options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 使用稀疏求解器
options.num_threads = 12; // 使用多线程
options.num_threads = 8; // 使用多线程
ceres::Solver::Summary summary;
// 求解
#ifdef OPT_H
ceres::Solve(options, &problemH, &summary);
#else
ceres::Solve(options, &problemSE3, &summary);
ceres::Solve(options, &problemH2, &summary);
#endif
//for (int i = 0; i < _origse3.size(); i++)
//{
// std::cout << "------------" << std::endl;
// std::cout << _origse3[i] << std::endl;
// std::cout << _currse3[i] << std::endl;
// std::cout << "------------" << std::endl;
//}
#ifdef OPT_SE3
ceres::Solve(options, &problemSE3, &summary);
#endif
// 输出结果
//std::cout << summary.BriefReport() << std::endl;
std::cout << summary.BriefReport() << std::endl;
for (int i = 0; i < _currse3.size(); i++)
{
std::cout << "-----SE3------" << std::endl;
std::cout << _origse3[i]- _currse3[i] << std::endl;
std::cout << "------------" << std::endl;
}
#ifndef OPT_H
#ifdef OPT_SE3
// 将优化se3写入H
updateSe3();
updateSe3toH();
#endif
//for (int i = 0; i < _origMatrix.size(); i++)
@ -406,22 +391,35 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map)
// 优化后H写入文件缓存
writeFrameInfo();
for (size_t i = 0; i < _currMatrix.size(); i++)
{
auto Key = _frameInd[i];
// 更新缓存
updateCacheH(Key, _currMatrix[i], global_H);
}
}
bool BA_Task::updateCacheH(KeyType Key, cv::Mat H)
bool BA_Task::updateCacheH(KeyType Key, cv::Mat H, std::unordered_map <KeyType, cv::Mat>& global_H)
{
auto _t_frame_cache = std::make_shared<FrameCache>();
if (_cache->get(Key, _t_frame_cache))
{
//cv::Mat old(3, 3, CV_64FC1, _t_frame_cache->H);
//std::cout << "before" << old << std::endl;
//std::cout << "after" << H << std::endl;
// 更新H
memcpy(_t_frame_cache->H, H.data, sizeof(double) * 9);
// 存储
_cache->set(Key, _t_frame_cache);
global_H[Key] = H.clone();
return true;
}
@ -432,7 +430,6 @@ int BA_Task::readFrameInfo(vector<KeyType> frameInd)
{
auto _t_frame_cache = std::make_shared<FrameCache>();
_cache->get(frameInd[0], _t_frame_cache);
constHeight = _t_frame_cache->_para.nEvHeight;
for (size_t i = 0; i < frameInd.size(); i++)
{
@ -442,6 +439,8 @@ int BA_Task::readFrameInfo(vector<KeyType> frameInd)
// 记录key
_frameInd.push_back(key);
_depth.push_back(_t_frame_cache->_para.nEvHeight);
// 注意Mat 浅拷贝陷阱！！！！！！
// 特征点
@ -482,27 +481,17 @@ int BA_Task::readFrameInfo(vector<KeyType> frameInd)
return 0;
}
#include <Arith_GeoSolver.h>
void BA_Task::updateSe3()
void BA_Task::updateSe3toH()
{
for (size_t i = 0; i < _origse3.size(); i++)
{
_currMatrix[i] = Transe3ToH((double*)_origse3[i].data, constK, constHeight);
_currMatrix[i] = Transe3ToH((double*)_currse3[i].data, constK, _depth[i]);
}
}
int BA_Task::writeFrameInfo()
{
for (size_t i = 0; i < _currMatrix.size(); i++)
{
auto Key = _frameInd[i];
// 更新缓存
updateCacheH(Key, _currMatrix[i]);
}
return 0;
}
SINT32 BA_Task::CalMatchMat()
{

24
stitch/src/Arith_BATask.h
Unescape View File

@ -17,23 +17,24 @@
//#define SHOW_MATCH
//#define OPT_H
//#define OPT_H
#define OPT_SE3
class BA_Task
{
public:
BA_Task(FileCache<FrameCache>* cache);
BA_Task(FileCache<FrameCache>* cache, FeatureMatcher* feaProcess);
~BA_Task();
// 初始化BA任务
void InitTask();
// 优化指定帧，并更新H到缓存中,H_map用于优化过程可视化
void OptFrame(vector<KeyType> frameInd, cv::Mat H_map);
void OptFrame(vector<KeyType> frameInd, cv::Mat H_map, std::unordered_map <KeyType, cv::Mat>& global_H);
// 更新缓存中的帧H矩阵，并存储
bool updateCacheH(KeyType Key, cv::Mat H);
private:
@ -41,10 +42,11 @@ private:
int readFrameInfo(vector<KeyType> frameInd);
// 更新se3到H
void updateSe3();
void updateSe3toH();
// 输出优化结果到cache
int writeFrameInfo();
// 更新缓存中的帧H矩阵，并存储到缓存
bool updateCacheH(KeyType Key, cv::Mat H, std::unordered_map <KeyType, cv::Mat>& global_H);
SINT32 CalMatchMat();//计算匹配性矩阵
@ -75,12 +77,14 @@ private:
vector<cv::Mat_<double>> _currse3; // 当前旋转向量
vector<double> _depth;
vector<vector<cv::Point2f>> _polygon;//帧包围四边形
vector<vector<cv::KeyPoint>> _FeaPtVec;//特征点缓存
vector<cv::Mat> _FeaDespVec;//特征点描述子
vector<FrameInfo> _paraVec;
double constHeight;
cv::Mat constK;
private:
@ -88,6 +92,8 @@ private:
// 调试：绘制匹配关系
void drawMatch(int i, int j, std::vector<cv::DMatch> match,cv::Mat H_map);
std::mutex mutex_;
};

4
stitch/src/Arith_BlendMap.cpp
Unescape View File

@ -35,6 +35,7 @@ cv::Mat MapBlend::getPanMaskMat()
void MapBlend::ClearPan()
{
std::lock_guard<std::mutex> lock(mutex_);
_panImage.setTo(0);
_panMask.setTo(0);
}
@ -245,7 +246,7 @@ void MapBlend::FrameMap_1(cv::Mat src, cv::Mat H_Geo, cv::Mat mapH)
void MapBlend::BatchMapWithH(const vector<KeyType> frameInd, const cv::Mat mapH, std::unordered_map <KeyType, cv::Mat>allH, BLEND_TYPE blendType)
{
std::lock_guard<std::mutex> lock(mutex_);
if (frameInd.empty())
{
return;
@ -324,6 +325,7 @@ void MapBlend::BatchMapWithH(const vector<KeyType> frameInd, const cv::Mat mapH,
void MapBlend::BatchMap(const vector<KeyType> frameInd, const cv::Mat mapH, BLEND_TYPE blendType)
{
std::lock_guard<std::mutex> lock(mutex_);
// 从文件缓存获取帧
auto _t_frame_cache = std::make_shared<FrameCache>();

2
stitch/src/Arith_BlendMap.h
Unescape View File

@ -69,6 +69,8 @@ private:
cv::Mat _panMask; //覆盖区域遮罩
std::mutex mutex_;
};

15
stitch/src/Arith_GeoSolver.cpp
Unescape View File

@ -133,7 +133,6 @@ 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_<double>(3, 3) << 1, 0, 0,
0, 1, 0,
@ -246,17 +245,25 @@ cv::Mat TransRtKToH(RtK rtk, double Depth)
(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<double>(2, 2);
return H;
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;
@ -304,8 +311,6 @@ cv::Mat GeoSolver::findHomography(FrameInfo info)
tf_p2g.R = Mat_TransENG2uv(info).inv();
tf_p2g.T = Mat_TransENGMove(info);
double R[9] = { 0 };
double T[3] = { 0 };

2
stitch/src/Arith_GeoSolver.h
Unescape View File

@ -68,8 +68,6 @@ private:
// 计算当前帧李群（SE3）和投影K
RtK AnlayseRtK(FrameInfo info);
// 帧像方-地理坐标
cv::Point2f Trans_uv2Geo(cv::Point2f pos_frame, TForm form);

76
stitch/src/Arith_UnderStitch.cpp
Unescape View File

@ -55,11 +55,9 @@ UnderStitch::UnderStitch(std::string cachedir)
_GeoSolver = new GeoSolver();
_FeaMatcher = new FeatureMatcher(DetectorType::ORB, MatcherType::FLANN);
_FeaMatcher = new FeatureMatcher(DetectorType::ORB, MatcherType::BF);
_BATask = new BA_Task(_cache);
/*_BlendTask = new MapBlend(_cache);*/
_BATask = new BA_Task(_cache, _FeaMatcher);
_panPara = { 0 };
@ -125,7 +123,7 @@ UPanInfo UnderStitch::InitMap(FrameInfo para)
auto geo_pro_rect = warpRectWithH_2Rect(H0,cv::Size(para.nWidth, para.nHeight));
// 计算目标比例（单帧投影后占全景图的0.25）
float target_ratio = 0.08;
float target_ratio = 0.25;
float current_ratio = MAX(geo_pro_rect.width / (target_ratio*panPara.m_pan_width), geo_pro_rect.height / (target_ratio*panPara.m_pan_height));
// 调整scale参数
@ -348,6 +346,31 @@ UPanInfo UnderStitch::ShiftMap(FrameInfo para,int& shift_x,int& shift_y)
}
void UnderStitch::run_test(FrameInfo para)
{
cv::Mat H1 = _GeoSolver->findHomography(para);
RtK rtk;
cv::Mat H2 = _GeoSolver->findHomography(para, rtk);
cv::Vec3d rvec;
cv::Rodrigues(rtk.R, rvec);
double se3[6] = { 0 };
memcpy(se3, rvec.val, sizeof(double) * 3);
memcpy(se3 + 3, rtk.T.data, sizeof(double) * 3);
auto H3 = Transe3ToH(se3,rtk.K, para.nEvHeight);
int a = 0;
}
UPanInfo UnderStitch::Init(FrameInfo info)
{
// 以向下零位初始化
@ -425,6 +448,8 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para)
LOG_INFO("Run Frame {}", para.nFrmID);
TransPitch(para);
run_test(para);
//// 视频帧筛选
if (!FilterFrame(para))
{
@ -447,6 +472,7 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para)
if (!isFrameInPanView(cv::Size(para.nWidth, para.nHeight), _H_pan * H_geo,
cv::Size(_panPara.m_pan_width, _panPara.m_pan_height)))
{
LOG_INFO("Frame {} is out of pan view, reinitializing projection parameters", para.nFrmID);
// 输出当前所有瓦片，再不输出就会丢失当前全景中的瓦片
@ -472,6 +498,9 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para)
// 清空全景视场
_MapBlender->ClearPan();
// 清空视场内帧
_baCurrViewKey.clear();
// 将BatchMap操作放入线程池中后台处理
_threadPool.commit([this, recvFrameKeys=_recvFrameKey, h_pan=_H_pan, Hgeos = _frameGeoHAll]() {
_MapBlender->BatchMapWithH(recvFrameKeys, h_pan, Hgeos);
@ -499,7 +528,6 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para)
// 投影帧到全景
_MapBlender->FrameMap(src, H_geo, _H_pan);
// 预处理+缓存
ReceiveFrame(frame, para);
@ -509,10 +537,11 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para)
// 异步执行BA优化
_threadPool.commit(
[this]() {
OptAndOutCurrPan();
OptAndOutCurrPan(_baWindowKey);
},
"BA_Optimization"
);
}
return 0;
@ -520,33 +549,50 @@ SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para)
}
void UnderStitch::Stop()
{
_recvFrameKey.clear();
_baWindowKey.clear();
_baCurrViewKey.clear();
_cache->clear();
}
SINT32 UnderStitch::OptAndOutCurrPan()
{
LOG_INFO("BA_Optimization start{}-{}",_baWindowKey[0],_baWindowKey[_baWindowKey.size()-1]);
return 0;
}
SINT32 UnderStitch::OptAndOutCurrPan(vector<KeyType> workFrame)
{
LOG_INFO("BA_Optimization start{}-{}", workFrame[0], workFrame[workFrame.size()-1]);
if (_config.bUseBA)
{
//BA_Task* task = new BA_Task(_cache, _FeaMatcher);
// 优化滑动窗口内的帧
_BATask->OptFrame(_baWindowKey, _H_pan);
_BATask->OptFrame(workFrame, _H_pan, _frameGeoHAll);
LOG_INFO("BA_Optimization finish");
}
// 重投影所有帧到全景，这里以文件缓存H为准，因为优化结果直接更新到cache中
_MapBlender->BatchMap(_baWindowKey, _H_pan);
_MapBlender->BatchMap(workFrame, _H_pan);
// 输出当前所有瓦片
LOG_INFO("ExportTileSet");
_googleProduct.ExportTileSet(_MapBlender->getPanImageMat(), _MapBlender->getPanMaskMat(),
_taskTilesVec, _outDir, _filename + std::to_string(_lastFrameKey));
return _baWindowKey.size();
return workFrame.size();
}
SINT32 UnderStitch::BatchAdjust(vector<KeyType> ids)
{
// 优化滑动窗口内的帧
_BATask->OptFrame(ids, _H_pan, _frameGeoHAll);
LOG_INFO("BA_Optimization finish");
return ids.size();
}
@ -603,7 +649,7 @@ bool UnderStitch::FilterFrame(FrameInfo para)
bool UnderStitch::TriggerBA(FrameInfo para)
{
// 1. 固定帧间隔触发
const int FIXED_FRAME_INTERVAL = 10;
const int FIXED_FRAME_INTERVAL = 30;
if (_totalFrameCnt % FIXED_FRAME_INTERVAL == 0)
{
LOG_INFO("TriggerBA: Fixed frame interval {} reached",FIXED_FRAME_INTERVAL);
@ -771,7 +817,7 @@ SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
// 深拷贝图像数据
memcpy(_t_frame_cache->_data,img.u64VirAddr[0],imgSize);
_t_frame_cache->nHeight = para.nEvHeight;
// 开启BA才提取特征
if (_config.bUseBA)
@ -842,6 +888,8 @@ SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
_recvFrameKey.push_back(para.nFrmID);
_baWindowKey.push_back(para.nFrmID);
_baCurrViewKey.push_back(para.nFrmID);
// 保持滑动窗口大小
const size_t WINDOW_SIZE = 30; // 滑动窗口大小

9
stitch/src/Arith_UnderStitch.h
Unescape View File

@ -43,6 +43,10 @@ public:
SINT32 OptAndOutCurrPan();
SINT32 OptAndOutCurrPan(vector<KeyType> workFrame);
SINT32 BatchAdjust(vector<KeyType> ids);
GD_VIDEO_FRAME_S ExportPanAddr();
cv::Mat ExportPanMat();
@ -56,7 +60,6 @@ private:
// 俯仰零位转换
void TransPitch(FrameInfo& para);
// 判断帧是否在全景图视场内
bool isFrameInPanView(const cv::Size& frameSize, const cv::Mat& H, const cv::Size& panSize);
@ -95,10 +98,14 @@ private:
// 重新定位全景图配置，用于帧超出全景图视场时平移调整
UPanInfo ShiftMap(FrameInfo para,int& shift_x,int& shift_y);
void run_test(FrameInfo para);
private:
vector<KeyType> _recvFrameKey;// 接收帧总表
vector<KeyType> _baWindowKey;// BA滑动窗口
vector<KeyType> _baCurrViewKey;//当前视口内帧
std::map<KeyType, int> _recvFrameOptFlag;//接收帧的优化标记
// 邻接关系表

14
stitch/src/FrontStitch/Arith_FrontStitch.cpp
Unescape View File

@ -36,7 +36,7 @@ FrontStitch::FrontStitch(std::string cachedir)
_FeaMatcher = new FeatureMatcher(DetectorType::SIFT, MatcherType::FLANN);
_BATask = new BA_Task(_cache);
_BATask = new BA_Task(_cache, _FeaMatcher);
_FBlendTask = new FrontMapBlend(_cache);
}
@ -380,18 +380,6 @@ SINT32 FrontStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
return _totalFrameCnt;
}
SINT32 FrontStitch::ProcessFrame(vector<KeyType> keys)
{
if (keys.size() == 0)
{
return -1;
}
// 优化所有帧
_BATask->OptFrame(keys, cv::Mat::zeros(cv::Size(3,3),CV_8UC1));
return 0;
}

2
stitch/src/FrontStitch/Arith_FrontStitch.h
Unescape View File

@ -133,8 +133,6 @@ private:
// 缓存接收帧
SINT32 ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para);
// 处理帧
SINT32 ProcessFrame(vector<KeyType> keys);
private:

2
stitch/src/StitchStruct.h
Unescape View File

@ -76,6 +76,8 @@ struct FrameCache
// SE3
DOUBLE64 RtK[21]; //R(9)+T(3)+K(9)
DOUBLE64 se3[6]; //旋转向量+平移向量，李代数（优化项）
DOUBLE64 nHeight;
};

2
stitch/src/Version.h
Unescape View File

@ -2,5 +2,5 @@
#pragma once
#include <string>
std::string BUILD_TIME = "BUILD_TIME 2025_11_11-09.47.45";
std::string BUILD_TIME = "BUILD_TIME 2025_11_14-15.38.58";
std::string VERSION = "BUILD_VERSION 1.0.1";

31
tests/ProcDJ.cpp
Unescape View File

@ -156,9 +156,9 @@ std::vector<FrameData> parseDJISRT(const std::string& filename) {
void ProcDJVideo(vector<std::string> videoPathList, vector<std::string> srtPathList)
{
auto stitcher = API_UnderStitch::Create();
auto stitcher = API_UnderStitch::Create("F:/cache");
stitcher->SetOutput("DJI", "D:/google_tiles");
stitcher->SetOutput("DJI", "F:/google_tiles");
GD_VIDEO_FRAME_S frame = { 0 };//输入帧
GD_VIDEO_FRAME_S pan = { 0 };//输出全景
@ -187,7 +187,7 @@ void ProcDJVideo(vector<std::string> videoPathList, vector<std::string> srtPathL
std::vector<FrameData> srt_init = parseDJISRT(srtPathList[0]);
int nStart = 3550;
int nStart = 2000;
FrameInfo info = { 0 };
info.nFrmID = nStart;
@ -195,7 +195,7 @@ void ProcDJVideo(vector<std::string> videoPathList, vector<std::string> srtPathL
info.camInfo.fPixelSize = srt_init[nStart].pixel_size_um * nDownSample;
info.craft.stAtt.fYaw = srt_init[nStart].gb_yaw;
info.craft.stAtt.fPitch = srt_init[nStart].gb_pitch;
info.craft.stAtt.fPitch = 0;
info.craft.stAtt.fRoll = srt_init[nStart].gb_roll;
info.craft.stPos.B = srt_init[nStart].latitude;
@ -206,7 +206,7 @@ void ProcDJVideo(vector<std::string> videoPathList, vector<std::string> srtPathL
info.nEvHeight = srt_init[nStart].rel_alt;
info.servoInfo.fServoAz = 0;
info.servoInfo.fServoPt = 0;
info.servoInfo.fServoPt = srt_init[nStart].gb_pitch;
info.nWidth = width / nDownSample;
info.nHeight = height / nDownSample;
@ -217,7 +217,7 @@ void ProcDJVideo(vector<std::string> videoPathList, vector<std::string> srtPathL
UPanConfig cfg = { 0 };
cfg.bOutFrameTile = 0;
cfg.bOutGoogleTile = 1;
cfg.bUseBA = 0;
cfg.bUseBA = 1;
stitcher->SetConfig(cfg);
mat_pan = stitcher->ExportPanMat();
@ -266,15 +266,13 @@ void ProcDJVideo(vector<std::string> videoPathList, vector<std::string> srtPathL
cv::resize(mat, mat_ds2, cv::Size(width / nDownSample, height / nDownSample));
FrameInfo info = { 0 };
info.nFrmID = frmID;
info.camInfo.nFocus = srt[frmID].real_focal_mm;
info.camInfo.fPixelSize = srt[frmID].pixel_size_um * nDownSample;
info.craft.stAtt.fYaw = srt[frmID].gb_yaw;
info.craft.stAtt.fPitch = srt[frmID].gb_pitch;
info.craft.stAtt.fPitch = 0;
info.craft.stAtt.fRoll = srt[frmID].gb_roll;
info.craft.stPos.B = srt[frmID].latitude;
@ -285,7 +283,7 @@ void ProcDJVideo(vector<std::string> videoPathList, vector<std::string> srtPathL
info.nEvHeight = srt[frmID].abs_alt;
info.servoInfo.fServoAz = 0;
info.servoInfo.fServoPt = 0;
info.servoInfo.fServoPt = srt[frmID].gb_pitch;
info.nWidth = mat_ds2.cols;
info.nHeight = mat_ds2.rows;
@ -372,18 +370,11 @@ int main()
vector<std::string> videoPathList;
vector<std::string> srtPathList;
string folder = "F:/DJI_202504181507_016/";
string folder = "F:/K2D_data/";
folder = "D:/H30T云台/DJI_202504181507_016/";
//string folder = "/media/wang/data/DJI_202504181507_016/";
//
//videoPathList.push_back(folder + "DJI_20250418152649_0005_W.MP4");
//srtPathList.push_back(folder + "DJI_20250418152649_0005_W.srt");
// videoPathList.push_back(folder + "DJI_20250418153043_0006_W.MP4");
//srtPathList.push_back(folder + "DJI_20250418153043_0006_W.srt");
videoPathList.push_back(folder + "DJI_20250418152649_0005_W.MP4");
srtPathList.push_back(folder + "DJI_20250418152649_0005_W.srt");
videoPathList.push_back(folder + "DJI_20250418153043_0006_W.MP4");
srtPathList.push_back(folder + "DJI_20250418153043_0006_W.srt");
//videoPathList.push_back(folder + "DJI_20250418153043_0006_W.MP4");
//srtPathList.push_back(folder + "DJI_20250418153043_0006_W.srt");

13
tests/stitch_S732.cpp
Unescape View File

@ -13,7 +13,7 @@ using namespace std;
void ProcessVL(string filePath)
{
auto stitcher = API_UnderStitch::Create("F:/cache");
auto stitcher = API_UnderStitch::Create("D:/cache");
stitcher->SetOutput("BT", "google_tiles");
@ -125,7 +125,7 @@ void ProcessVL(string filePath)
UPanConfig cfg = { 0 };
cfg.bOutGoogleTile = 1;
cfg.bOutFrameTile = 0;
cfg.bUseBA = 0;
cfg.bUseBA = 1;
stitcher->SetConfig(cfg);
stitcher->SetOutput("baotou","F:/google_tiles");
@ -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 / 16, pan.u32Height / 16), true);
output.open("output.mp4", VideoWriter::fourcc('M', 'P', '4', 'V'), 50, Size(pan.u32Width / 8, pan.u32Height / 8), true);
if (!output.isOpened())
{
cout << "打开视频失败" << endl;
@ -144,7 +144,7 @@ void ProcessVL(string filePath)
}
else
{
std::cout << info.craft.stPos.B << " " << info.craft.stPos.L << " " << info.craft.stPos.H << " "
std::cout << info.nFrmID << " " <<info.craft.stPos.B << " " << info.craft.stPos.L << " " << info.craft.stPos.H << " "
<< info.craft.stAtt.fYaw << " " << info.craft.stAtt.fPitch << " " << info.craft.stAtt.fRoll << " "
<< info.servoInfo.fServoAz << " " << info.servoInfo.fServoPt + 90
<< std::endl;
@ -160,8 +160,8 @@ void ProcessVL(string filePath)
}
Mat pan_ds;
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));
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));
cv::cvtColor(pan_ds, pan_rgb_ds, cv::COLOR_BGRA2BGR);
@ -169,6 +169,7 @@ void ProcessVL(string filePath)
output.write(pan_rgb_ds);
cv::namedWindow("pan_opt",0);
imshow("pan_opt", pan_rgb_ds);

Write Preview
Loading…
Cancel
Save