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

BA误差改为综合误差，不使用xy误差

Browse Source
main
wangchongwu 2 weeks ago
parent 75b63e41e8
commit d434eda3de
5 changed files with 39 additions and 42 deletions
Show Stats Download Patch File Download Diff File

46
stitch/src/Arith_BATask.cpp
Unescape View File

@ -19,6 +19,8 @@ using namespace ceres;
#define STABLE_Y3 1080
//// H投影残差：将左图投影到右图，最小化同名点全景图上的投影误差。
//// 问题：缺少尺度约束，H矩阵优化后失去正交性 优点：形式简单
struct HomographyResidual
@ -106,8 +108,8 @@ private:
// 问题：不是真正三角化，无深度优化。 优点：有尺度约束，标准做法，可对21个参数优化。
struct SE3Residual
{
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)
SE3Residual(cv::KeyPoint keypoint_i, cv::KeyPoint keypoint_j,cv::Mat _K, double _fEvHeight_i, double _fEvHeight_j, const cv::Mat H1, const cv::Mat H2)
: keypoint_i_(keypoint_i), keypoint_j_(keypoint_j),K(_K),EvHeight_i(_fEvHeight_i), EvHeight_j(_fEvHeight_j), Hi0_(H1), Hj0_(H2)
{
}
@ -115,18 +117,20 @@ struct SE3Residual
template <typename T>
bool operator()(const T* const se3_i, const T* const se3_j, T* residual) const
{
typedef Eigen::Matrix<T, 3, 1> EPoint;
typedef Eigen::Matrix<T, 3, 3> EMat;
// 左图投影到物方
// T ptL[3] = {T(keypoint_i_.pt.x), T(keypoint_i_.pt.y)}; // 原始点
// 深度矩阵
Eigen::Matrix<T, 3, 3> M_het_inv;
EMat 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_i);
// K-1
Eigen::Matrix<T, 3, 3> K_1;
Eigen::Matrix<T, 3, 3> K_inv;
EMat K_1;
EMat K_inv;
double fx = K.at<double>(0, 0);
double fy = K.at<double>(1, 1);
@ -141,17 +145,18 @@ struct SE3Residual
T(0), T(1) / T(fy), -T(cy) / T(fy),
T(0), T(0), T(1);
Eigen::Matrix<T, 3, 1> ptL(T(keypoint_i_.pt.x), T(keypoint_i_.pt.y), T(1.0));
EPoint ptL(T(keypoint_i_.pt.x), T(keypoint_i_.pt.y), T(1.0));
EPoint ptR(T(keypoint_j_.pt.x), T(keypoint_j_.pt.y), T(1.0));
// 投影也可以用这个
//cv::Point2f warpPL = warpPointWithse3((const double*)se3_i, (const double)EvHeight, K, ptL);
Eigen::Matrix<T, 3, 1> pt = K_inv * ptL;
Eigen::Matrix<T, 3, 1> LeftRota;
EPoint pt = K_inv * ptL;
EPoint LeftRota;
T rvec_i[3] = { -se3_i[0], -se3_i[1], -se3_i[2] };
ceres::AngleAxisRotatePoint(rvec_i, pt.data(), LeftRota.data());
Eigen::Matrix<T, 3, 1> result = M_het_inv * LeftRota;
EPoint result = M_het_inv * LeftRota;
// 恢复尺度
T GeoX = result[0] / result[2];
T GeoY = result[1] / result[2];
@ -168,22 +173,20 @@ struct SE3Residual
T rvec_j[3] = { se3_j[0], se3_j[1], se3_j[2] };
Eigen::Matrix<T, 3, 1> pGeoR(GeoX, GeoY, GeoZ);
Eigen::Matrix<T, 3, 1> RightRota;
EPoint pGeoR(GeoX, GeoY, GeoZ);
EPoint RightRota;
ceres::AngleAxisRotatePoint(rvec_j, pGeoR.data(), RightRota.data());
// 投影到右图像方
Eigen::Matrix<T, 3, 1> pUVR = K_1* RightRota;
// 压缩尺度
T uR = pUVR[0] / pUVR[2];
T vR = pUVR[1] / pUVR[2];
EPoint pUVR = K_1* RightRota;
EPoint pUVR_1 = pUVR / pUVR[2];
// 计算重投影误差
residual[0] = uR - T(keypoint_j_.pt.x);
residual[1] = vR - T(keypoint_j_.pt.y);
residual[0] = (pUVR_1 - ptR).squaredNorm();
@ -195,6 +198,9 @@ private:
cv::Mat_<double> K; //内参
double EvHeight_i;//高差，用于三角化
double EvHeight_j;//高差，用于三角化
const cv::Mat Hi0_;//初始定位H，用于坐标控制
const cv::Mat Hj0_;//初始定位H，用于坐标控制
};
@ -332,8 +338,8 @@ void BA_Task::OptFrame(vector<KeyType> frameInd,cv::Mat H_map, std::unordered_ma
#ifdef OPT_SE3
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction<SE3Residual,2,6,6>(
new SE3Residual(keypoint_i, keypoint_j, constK, _depth[i], _depth[j]));
new ceres::AutoDiffCostFunction<SE3Residual,1,6,6>(
new SE3Residual(keypoint_i, keypoint_j, constK, _depth[i], _depth[j], Hi0, Hj0));
problemSE3.AddResidualBlock(cost_function, scale_loss, se3list[i], se3list[j]);
#endif

15
stitch/src/Arith_UnderStitch.cpp
Unescape View File

@ -91,7 +91,7 @@ UPanInfo UnderStitch::InitMap(FrameInfo para)
double yaw_angle = para.craft.stAtt.fYaw;
// 根据飞行方向调整全景图尺寸
int base_size = MIN(para.nWidth * 10, 10000);
int base_size = MIN(para.nWidth * 10, 12000);
UPanInfo panPara = { 0 };
// 根据偏航角计算宽高比
@ -649,7 +649,7 @@ bool UnderStitch::FilterFrame(FrameInfo para)
bool UnderStitch::TriggerBA(FrameInfo para)
{
// 1. 固定帧间隔触发
const int FIXED_FRAME_INTERVAL = 30;
const int FIXED_FRAME_INTERVAL = 15;
if (_totalFrameCnt % FIXED_FRAME_INTERVAL == 0)
{
LOG_INFO("TriggerBA: Fixed frame interval {} reached",FIXED_FRAME_INTERVAL);
@ -876,13 +876,6 @@ SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
memcpy(_t_frame_cache->se3, rvec.val, sizeof(double) * 3);
memcpy(_t_frame_cache->se3 + 3, rtk.T.data, sizeof(double) * 3);
//#define TEST
#ifdef TEST
auto wpt1 = warpPointWithH(H_cur, cv::Point2f(100, 100));
auto wpt2 = warpPointWithse3(_t_frame_cache->se3, para.nEvHeight, rtk.K, cv::Point2f(100, 100));
#endif
// 预处理结果加入文件缓存
_cache->set(para.nFrmID,_t_frame_cache);
@ -901,10 +894,6 @@ SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
// 优化表初始化
_recvFrameOptFlag[para.nFrmID] = 0;
_totalFrameCnt++;
return _totalFrameCnt;

2
stitch/src/Version.h
Unescape View File

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

9
tests/ProcDJ.cpp
Unescape View File

@ -89,7 +89,8 @@ void inferCameraParams_H30(FrameData& frame, const std::string& filename)
frame.pixel_size_um = 2.4;
}
else if (filename.find("_Z") != std::string::npos) {
frame.real_focal_mm = 6.72*2;
frame.pixel_size_um = 2.4;
}
else if (filename.find("_T") != std::string::npos) {
frame.real_focal_mm = 24;
@ -216,7 +217,7 @@ void ProcDJVideo(vector<std::string> videoPathList, vector<std::string> srtPathL
UPanConfig cfg = { 0 };
cfg.bOutFrameTile = 0;
cfg.bOutGoogleTile = 1;
cfg.bOutGoogleTile = 0;
cfg.bUseBA = 1;
stitcher->SetConfig(cfg);
@ -373,8 +374,8 @@ int main()
string folder = "F:/K2D_data/";
videoPathList.push_back(folder + "DJI_20250418153043_0006_W.MP4");
srtPathList.push_back(folder + "DJI_20250418153043_0006_W.srt");
videoPathList.push_back(folder + "DJI_20251024144932_0001_Z.MP4");
srtPathList.push_back(folder + "DJI_20251024144932_0001_Z.srt");
//videoPathList.push_back(folder + "DJI_20250418153043_0006_W.MP4");
//srtPathList.push_back(folder + "DJI_20250418153043_0006_W.srt");

7
tests/stitch_S732.cpp
Unescape View File

@ -123,9 +123,9 @@ void ProcessVL(string filePath)
stitcher->Init(info);
UPanConfig cfg = { 0 };
cfg.bOutGoogleTile = 1;
cfg.bOutGoogleTile = 0;
cfg.bOutFrameTile = 0;
cfg.bUseBA = 1;
cfg.bUseBA = 0;
stitcher->SetConfig(cfg);
stitcher->SetOutput("baotou","F:/google_tiles");
@ -205,6 +205,7 @@ void ProcessVL(string filePath)
int main()
{
ProcessVL("F:/包头/20251101/20251101_091550742_2.yuv");
ProcessVL("F:/包头/20251101/20251101_092143724_3.yuv");
return 0;
}

Write Preview
Loading…
Cancel
Save