diff --git a/main.cpp b/main.cpp index 5df1d3d..b824cdb 100644 --- a/main.cpp +++ b/main.cpp @@ -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; } diff --git a/stitch/src/Arith_BATask.cpp b/stitch/src/Arith_BATask.cpp index 80f26f8..3132ab2 100644 --- a/stitch/src/Arith_BATask.cpp +++ b/stitch/src/Arith_BATask.cpp @@ -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 - 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(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 h_list(_origMatrix.size()); + std::vector 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(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( - 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(0, 0) = h_list[i][0]; - _currMatrix[i].at(0, 1) = h_list[i][1]; - _currMatrix[i].at(0, 2) = h_list[i][2]; - _currMatrix[i].at(1, 0) = h_list[i][3]; - _currMatrix[i].at(1, 1) = h_list[i][4]; - _currMatrix[i].at(1, 2) = h_list[i][5]; - _currMatrix[i].at(2, 0) = h_list[i][6]; - _currMatrix[i].at(2, 1) = h_list[i][7]; - _currMatrix[i].at(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 poly_i = _polygon[i]; - - for (size_t j = i + 1; j < _polygon.size(); j++) - { - if (i == j) - { - _IOUMat.at(i,j) = 1; - continue; - } - vector poly_j = _polygon[j]; - - float fiou = computeQuadrilateralIOU(poly_i,poly_j); - _IOUMat.at(i,j) = fiou; - _IOUMat.at(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(i,j) < fiou_thre) + if (_IOUMat.at(i, j) < 0.3) { continue; } @@ -312,7 +173,7 @@ SINT32 BA_Task::CalMatchMat(float fiou_thre) #endif std::vector 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( + 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> 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 poly_i = _polygon[i]; + + for (size_t j = i + 1; j < _polygon.size(); j++) + { + if (i == j) + { + _IOUMat.at(i,j) = 1; + continue; + } + vector poly_j = _polygon[j]; + + float fiou = computeQuadrilateralIOU(poly_i,poly_j); + _IOUMat.at(i,j) = fiou; + _IOUMat.at(j,i) = fiou;//是对称矩阵 + + } + } + + return 0; } diff --git a/stitch/src/Arith_BATask.h b/stitch/src/Arith_BATask.h index e087ee0..18ecce7 100644 --- a/stitch/src/Arith_BATask.h +++ b/stitch/src/Arith_BATask.h @@ -39,7 +39,7 @@ public: void optimizeBA(); // 使用H矩阵重投影所有帧 - void remap(); + void remap(vector>H_Vec,std::string winname); private: SINT32 CalMatchMat(float fiou_thre);//计算匹配性矩阵(以IOU约束) diff --git a/stitch/src/Arith_FeaMatch.cpp b/stitch/src/Arith_FeaMatch.cpp index 471d6ed..3a6e0fb 100644 --- a/stitch/src/Arith_FeaMatch.cpp +++ b/stitch/src/Arith_FeaMatch.cpp @@ -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"); diff --git a/stitch/src/Arith_GeoStitcher.cpp b/stitch/src/Arith_GeoStitcher.cpp index 356d028..8c4aee3 100644 --- a/stitch/src/Arith_GeoStitcher.cpp +++ b/stitch/src/Arith_GeoStitcher.cpp @@ -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