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

初始BA完成，全局尺度约束存在问题

Browse Source
main
wangchongwu 6 months ago
parent 50929e6945
commit 4bdec33be3
5 changed files with 132 additions and 168 deletions
Show Stats Download Patch File Download Diff File

4
main.cpp
Unescape View File

@ -96,7 +96,7 @@ int main(int, char**)
FILE* file = fopen("../20241219152917_4.xraw","rb");
//FILE* file = fopen("/home/wang/data62/729dataset/ir_1280_1024_para40_y16/5.xraw","rb");
for (size_t i = 0; i < 900; i++)
for (size_t i = 0; i < 600; i++)
{
fread(pFrameIR,2,IMAGE_WIDTH_IR*(IMAGE_HEIGHT_IR + PARA_IR_LINE),file);
@ -152,7 +152,7 @@ int main(int, char**)
}
else
{
if(i % 20 != 0)
if(i % 10 != 0)
{
continue;
}

284
stitch/src/Arith_BATask.cpp
Unescape View File

@ -5,11 +5,11 @@ using namespace ceres;
struct HomographyResidual
{
HomographyResidual(const cv::KeyPoint& keypoint_i, const cv::KeyPoint& keypoint_j)
: keypoint_i_(keypoint_i), keypoint_j_(keypoint_j)
{
}
: keypoint_i_(keypoint_i), keypoint_j_(keypoint_j) {}
template <typename T>
bool operator()(const T* const h_i, const T* const h_j, T* residual) const {
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],
@ -25,8 +25,10 @@ struct HomographyResidual
T P_i[3] = { T(0), T(0), T(0) };
T P_j[3] = { T(0), T(0), T(0) };
for (int row = 0; row < 3; row++) {
for (int col = 0; col < 3; col++) {
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];
}
@ -77,8 +79,8 @@ SINT32 BA_Task::addFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
// 缓存初始H
Proj t_Proj = _GeoStitcher->AnlayseTform(para);
auto H = _GeoStitcher->findHomography(t_Proj,_panPara);
_origMatrix.push_back(H);
_currMatrix.push_back(H);
_origMatrix.push_back(H.clone());
_currMatrix.push_back(H.clone());
// 提取特征点
@ -102,43 +104,14 @@ void BA_Task::setPanPara(PanInfo info)
void BA_Task::Test()
{
// google::InitGoogleLogging("ceres");
// // The variable to solve for with its initial value. It will be
// // mutated in place by the solver.
// double x = 0.5;
// const double initial_x = x;
// // Build the problem.
// ceres::Problem problem;
// // Set up the only cost function (also known as residual). This uses
// // auto-differentiation to obtain the derivative (jacobian).
// ceres::CostFunction* cost_function =
// new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
// problem.AddResidualBlock(cost_function, nullptr, &x);
// // Run the solver!
// ceres::Solver::Options options;
// options.minimizer_progress_to_stdout = true;
// ceres::Solver::Summary summary;
// ceres::Solve(options, &problem, &summary);
// std::cout << summary.BriefReport() << "\n";
// std::cout << "x : " << initial_x << " -> " << x << "\n";
}
//#define SHOW_MATCH
void BA_Task::optimizeBA()
{
//for (int i = 0; i < _currMatrix.size(); i++)
//{
// std::cout << "------------" << std::endl;
// std::cout << _origMatrix[i] << std::endl;
// std::cout << _currMatrix[i] << std::endl;
// std::cout << "------------" << std::endl;
//}
remap(_origMatrix, "_origMatrix");
// 计算匹配性矩阵
CalMatchMat(0.3);
@ -146,137 +119,25 @@ void BA_Task::optimizeBA()
google::InitGoogleLogging("ceres");
// 将 cv::Mat 转换为 Ceres 需要的数组形式
std::vector<double*> h_list(_origMatrix.size());
std::vector<double*> h_list(_currMatrix.size());
for (int i = 0; i < _origMatrix.size(); i++)
for (int i = 0; i < _currMatrix.size(); i++)
{
h_list[i] = (double*)_origMatrix[i].data;
h_list[i] = (double*)_currMatrix[i].data;
}
// 创建 Ceres 问题
ceres::Problem problem;
// 添加残差块
int nParaCnt = 0;//参数组数
for (int i = 0; i < _MatchMat.cols; i++)
{
for (int j = i + 1; j < _MatchMat.rows; j++)
{
int M = _MatchMat.at<int>(i, j); // 获取匹配点对的数量
if (M > 0)
{
// 添加匹配点对的残差块
for (int m = 0; m < M; m++)
{
// 注意：这里不对，应该找匹配点！！ todo 节后完成
cv::KeyPoint keypoint_i = _FeaPtVec[i][m];
cv::KeyPoint keypoint_j = _FeaPtVec[j][m];
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction<HomographyResidual, 2, 8, 8>(
new HomographyResidual(keypoint_i, keypoint_j));
problem.AddResidualBlock(cost_function, nullptr, h_list[i], h_list[j]);
}
}
}
}
// 配置求解器
ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;
//options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 使用稀疏求解器
options.minimizer_progress_to_stdout = true;
options.num_threads = 1; // 使用多线程
ceres::Solver::Summary summary;
// 求解
ceres::Solve(options, &problem, &summary);
//// 将优化后的参数转换回 cv::Mat
for (int i = 0; i < _currMatrix.size(); i++)
{
std::cout << "------------" << std::endl;
std::cout << _origMatrix[i] << std::endl;
std::cout << _currMatrix[i] << std::endl;
std::cout << "------------" << std::endl;
_currMatrix[i].at<double>(0, 0) = h_list[i][0];
_currMatrix[i].at<double>(0, 1) = h_list[i][1];
_currMatrix[i].at<double>(0, 2) = h_list[i][2];
_currMatrix[i].at<double>(1, 0) = h_list[i][3];
_currMatrix[i].at<double>(1, 1) = h_list[i][4];
_currMatrix[i].at<double>(1, 2) = h_list[i][5];
_currMatrix[i].at<double>(2, 0) = h_list[i][6];
_currMatrix[i].at<double>(2, 1) = h_list[i][7];
_currMatrix[i].at<double>(2, 2) = 1.0; // 固定 h_33 = 1
}
// 输出结果
std::cout << summary.BriefReport() << std::endl;
remap();
}
void BA_Task::remap()
{
cv::Mat image(1000, 1000, CV_8UC3, cv::Scalar(0, 0, 0));
for (size_t i = 0; i < _imgVec.size(); i++)
{
cv::Mat imagetmp(1000, 1000, CV_8UC3, cv::Scalar(0, 0, 0));
cv::warpPerspective(_imgVec[i], imagetmp, _currMatrix[i], imagetmp.size());
cv::Mat mask = cv::Mat::ones(_imgVec[i].size(), CV_8UC1) * 255;
cv::Mat warped_mask;
cv::warpPerspective(mask, warped_mask, _currMatrix[i], image.size());
imagetmp.copyTo(image, warped_mask);
}
imshow("remap", image);
cv::waitKey(0);
}
//#define SHOW_MATCH
SINT32 BA_Task::CalMatchMat(float fiou_thre)
{
_IOUMat = cv::Mat::zeros(_polygon.size(),_polygon.size(),CV_32FC1);
_MatchMat = cv::Mat::zeros(_polygon.size(), _polygon.size(), CV_32SC1);
// 先计算IOU矩阵
for (size_t i = 0; i < _polygon.size(); i++)
{
vector<cv::Point2f> poly_i = _polygon[i];
for (size_t j = i + 1; j < _polygon.size(); j++)
{
if (i == j)
{
_IOUMat.at<float>(i,j) = 1;
continue;
}
vector<cv::Point2f> poly_j = _polygon[j];
float fiou = computeQuadrilateralIOU(poly_i,poly_j);
_IOUMat.at<float>(i,j) = fiou;
_IOUMat.at<float>(j,i) = fiou;//是对称矩阵
}
}
// IOU大于0.3以上才进行特征点匹配
for (size_t i = 0; i < _polygon.size(); i++)
{
for (size_t j = i+1; j < _polygon.size(); j++)
{
if (i == j)
{
continue;
}
// IOU满足条件才匹配特征点
if (_IOUMat.at<float>(i,j) < fiou_thre)
if (_IOUMat.at<float>(i, j) < 0.3)
{
continue;
}
@ -312,7 +173,7 @@ SINT32 BA_Task::CalMatchMat(float fiou_thre)
#endif
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);
_FeaMatcher->matchFeatures_WithH(_FeaPtVec[i], _FeaDespVec[i], _FeaPtVec[j], _FeaDespVec[j], _origMatrix[i], _origMatrix[j], matches);
// 图像特征匹配点对超过N对才认为有效
if (matches.size() > 50)
@ -345,12 +206,115 @@ SINT32 BA_Task::CalMatchMat(float fiou_thre)
cv::imshow("Feature Matches", img_matches);
cv::waitKey(0);
#endif
// 添加匹配点对的残差块
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];
ceres::LossFunction* loss_function = new ceres::HuberLoss(15);
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction<HomographyResidual, 2, 8, 8>(
new HomographyResidual(keypoint_i, keypoint_j));
problem.AddResidualBlock(cost_function, loss_function, h_list[i], h_list[j]);
}
nParaCnt += matches.size();
}
}
// 配置求解器
ceres::Solver::Options options;
options.max_num_iterations = 2; // 增加最大迭代次数
options.function_tolerance = 1e-8; // 设置更严格的函数值容忍度
options.gradient_tolerance = 1e-10; // 设置更严格的梯度容忍度
options.parameter_tolerance = 1e-10; // 设置更严格的参数容忍度
options.minimizer_progress_to_stdout = true;
//options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 使用稀疏求解器
options.num_threads = 16; // 使用多线程
ceres::Solver::Summary summary;
// 求解
ceres::Solve(options, &problem, &summary);
//// 将优化后的参数转换回 cv::Mat
for (int i = 0; i < _currMatrix.size(); i++)
{
std::cout << "------------" << std::endl;
std::cout << _origMatrix[i] << std::endl;
std::cout << _currMatrix[i] << std::endl;
std::cout << "------------" << std::endl;
}
// 输出结果
std::cout << summary.BriefReport() << std::endl;
remap(_currMatrix, "_currMatrix");
cv::waitKey(0);
}
void BA_Task::remap(vector<cv::Mat_<double>> H_Vec, std::string winname)
{
cv::Mat image(1500, 1500, CV_8UC3, cv::Scalar(0, 0, 0));
for (size_t i = 0; i < _imgVec.size(); i++)
{
cv::Mat imagetmp(1500, 1500, CV_8UC3, cv::Scalar(0, 0, 0));
cv::warpPerspective(_imgVec[i], imagetmp, H_Vec[i], imagetmp.size());
cv::Mat mask = cv::Mat::ones(_imgVec[i].size(), CV_8UC1) * 255;
cv::Mat warped_mask;
cv::warpPerspective(mask, warped_mask, H_Vec[i], image.size());
imagetmp.copyTo(image, warped_mask);
}
imshow(winname, image);
cv::waitKey(1);
}
//#define SHOW_MATCH
SINT32 BA_Task::CalMatchMat(float fiou_thre)
{
_IOUMat = cv::Mat::zeros(_polygon.size(),_polygon.size(),CV_32FC1);
_MatchMat = cv::Mat::zeros(_polygon.size(), _polygon.size(), CV_32SC1);
// 先计算IOU矩阵
for (size_t i = 0; i < _polygon.size(); i++)
{
vector<cv::Point2f> poly_i = _polygon[i];
for (size_t j = i + 1; j < _polygon.size(); j++)
{
if (i == j)
{
_IOUMat.at<float>(i,j) = 1;
continue;
}
vector<cv::Point2f> poly_j = _polygon[j];
float fiou = computeQuadrilateralIOU(poly_i,poly_j);
_IOUMat.at<float>(i,j) = fiou;
_IOUMat.at<float>(j,i) = fiou;//是对称矩阵
}
}
return 0;
}

2
stitch/src/Arith_BATask.h
Unescape View File

@ -39,7 +39,7 @@ public:
void optimizeBA();
// 使用H矩阵重投影所有帧
void remap();
void remap(vector<cv::Mat_<double>>H_Vec,std::string winname);
private:
SINT32 CalMatchMat(float fiou_thre);//计算匹配性矩阵(以IOU约束)

4
stitch/src/Arith_FeaMatch.cpp
Unescape View File

@ -92,7 +92,7 @@ void FeatureMatcher::initDetector()
switch (detectorType_)
{
case SIFT:
detector_ = cv::SIFT::create(500);
detector_ = cv::SIFT::create(2000);
break;
case SURF:
//detector_ = cv::xfeatures2d::SURF::create();
@ -103,7 +103,7 @@ void FeatureMatcher::initDetector()
#endif
break;
case ORB:
detector_ = cv::ORB::create(500);
detector_ = cv::ORB::create(2000);
break;
default:
throw std::invalid_argument("Unsupported feature detector type");

6
stitch/src/Arith_GeoStitcher.cpp
Unescape View File

@ -28,14 +28,14 @@ PanInfo GeoStitcher::InitMap(FrameInfo info)
PanInfo panPara = {0};
panPara.m_pan_width = 1000;//全景宽
panPara.m_pan_height = 1000;//全景高
panPara.scale = 0.3;//比例尺,1m = ?pix
panPara.scale = 0.5;//比例尺,1m = ?pix
// 直接无平移解算
auto cur = Trans_Geo2pan(ct_geo, panPara);
// 计算平移到全景图固定点的平移量，从此处开始拼接
int planX = 500;
int planY = 500;
int planX = 200;
int planY = 200;
panPara.map_shiftX = planX - (cur.x);//平移X
panPara.map_shiftY = planY - (cur.y);//平移Y

Write Preview
Loading…
Cancel
Save