|
|
|
@ -10,9 +10,12 @@ struct CostFunctor {
|
|
|
|
|
}
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
BA_Task::BA_Task()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
BA_Task::BA_Task(GeoStitcher * pGeoTrans)
|
|
|
|
|
{
|
|
|
|
|
// 来个100帧
|
|
|
|
|
_GeoStitcher = pGeoTrans;
|
|
|
|
|
_FeaMatcher = new FeatureMatcher(DetectorType::ORB, MatcherType::BF);
|
|
|
|
|
_imgVec.reserve(100);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
@ -22,17 +25,44 @@ BA_Task::~BA_Task()
|
|
|
|
|
|
|
|
|
|
SINT32 BA_Task::addFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
|
|
|
|
|
{
|
|
|
|
|
cv::Mat frame = Mat(img.u32Height, img.u32Width, CV_8UC1, (void*)img.u64VirAddr[0]);
|
|
|
|
|
|
|
|
|
|
// 缓存图像帧
|
|
|
|
|
if (img.enPixelFormat == GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y8)
|
|
|
|
|
{
|
|
|
|
|
_imgVec.emplace_back(Mat(img.u32Height, img.u32Width, CV_8UC1, img.u64VirAddr[0]));
|
|
|
|
|
_imgVec.emplace_back(frame.clone());
|
|
|
|
|
}
|
|
|
|
|
// 缓存外参
|
|
|
|
|
_paraVec.push_back(para);
|
|
|
|
|
|
|
|
|
|
// 缓存初始H
|
|
|
|
|
Proj t_Proj = _GeoStitcher->AnlayseTform(para);
|
|
|
|
|
auto H = _GeoStitcher->findHomography(t_Proj,_panPara);
|
|
|
|
|
_origMatrix.push_back(H);
|
|
|
|
|
_currMatrix.push_back(H);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 提取特征点
|
|
|
|
|
std::vector<cv::KeyPoint> keypoints;
|
|
|
|
|
cv::Mat descriptors;
|
|
|
|
|
_FeaMatcher->extractFeatures(frame,keypoints,descriptors);
|
|
|
|
|
_FeaPtVec.push_back(keypoints);
|
|
|
|
|
_FeaDespVec.push_back(descriptors);
|
|
|
|
|
|
|
|
|
|
// 缓存包围多边形
|
|
|
|
|
_polygon.push_back(warpRectWithH(H,cv::Size(img.u32Width,img.u32Height)));
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return _imgVec.size();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void BA_Task::setPanPara(PanInfo info)
|
|
|
|
|
{
|
|
|
|
|
_panPara = info;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@ -40,28 +70,151 @@ SINT32 BA_Task::addFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
|
|
|
|
|
|
|
|
|
|
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";
|
|
|
|
|
// 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";
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void BA_Task::optimizeBA()
|
|
|
|
|
{
|
|
|
|
|
// 计算匹配性矩阵
|
|
|
|
|
CalMatchMat(0.3);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
SINT32 BA_Task::CalMatchMat(float fiou_thre)
|
|
|
|
|
{
|
|
|
|
|
_IOUMat = cv::Mat::zeros(_polygon.size(),_polygon.size(),CV_32FC1);
|
|
|
|
|
// 先计算IOU矩阵
|
|
|
|
|
for (size_t i = 0; i < _polygon.size(); i++)
|
|
|
|
|
{
|
|
|
|
|
vector<cv::Point2f> poly_i = _polygon[i];
|
|
|
|
|
|
|
|
|
|
for (size_t j = 0; 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 = 0; j < _polygon.size(); j++)
|
|
|
|
|
{
|
|
|
|
|
if (i == j)
|
|
|
|
|
{
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (_IOUMat.at<float>(i,j) < fiou_thre)
|
|
|
|
|
{
|
|
|
|
|
continue;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// IOU满足条件才匹配特征点
|
|
|
|
|
cv::Mat image(1000, 1000, CV_8UC3, cv::Scalar(255, 255, 255));
|
|
|
|
|
vector<vector<cv::Point2f>> tmpPoly;
|
|
|
|
|
tmpPoly.push_back(_polygon[i]);
|
|
|
|
|
tmpPoly.push_back(_polygon[j]);
|
|
|
|
|
drawPolygons(image, tmpPoly);
|
|
|
|
|
|
|
|
|
|
// 显示绘制结果
|
|
|
|
|
cv::imshow("Polygons", image);
|
|
|
|
|
cv::waitKey(1);
|
|
|
|
|
std::vector<cv::DMatch> matches;
|
|
|
|
|
_FeaMatcher->matchFeatures(_FeaDespVec[i],_FeaDespVec[j],matches);
|
|
|
|
|
|
|
|
|
|
// 可视化匹配结果
|
|
|
|
|
cv::Mat img_matches;
|
|
|
|
|
cv::drawMatches(
|
|
|
|
|
_imgVec[i], _FeaPtVec[i], // 第一幅图像及其特征点
|
|
|
|
|
_imgVec[j], _FeaPtVec[j], // 第二幅图像及其特征点
|
|
|
|
|
matches, // 匹配结果
|
|
|
|
|
img_matches, // 输出图像
|
|
|
|
|
cv::Scalar::all(-1), // 匹配线颜色(默认随机颜色)
|
|
|
|
|
cv::Scalar::all(-1), // 未匹配点颜色(默认不绘制)
|
|
|
|
|
std::vector<char>(), // 掩码(可选,用于筛选匹配)
|
|
|
|
|
cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS // 不绘制未匹配的点
|
|
|
|
|
);
|
|
|
|
|
|
|
|
|
|
cv::resize(img_matches,img_matches,cv::Size(1280,512));
|
|
|
|
|
|
|
|
|
|
// 显示匹配结果
|
|
|
|
|
cv::imshow("Feature Matches", img_matches);
|
|
|
|
|
cv::waitKey(0);
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 函数:绘制多边形
|
|
|
|
|
void drawPolygons(cv::Mat& image, const std::vector<std::vector<cv::Point2f>>& polygons)
|
|
|
|
|
{
|
|
|
|
|
// 定义颜色列表
|
|
|
|
|
std::vector<cv::Scalar> colors = {
|
|
|
|
|
cv::Scalar(255, 0, 0), // 蓝色
|
|
|
|
|
cv::Scalar(0, 255, 0), // 绿色
|
|
|
|
|
cv::Scalar(0, 0, 255), // 红色
|
|
|
|
|
cv::Scalar(255, 255, 0), // 青色
|
|
|
|
|
cv::Scalar(255, 0, 255), // 品红
|
|
|
|
|
cv::Scalar(0, 255, 255) // 黄色
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
// 绘制每个多边形
|
|
|
|
|
for (size_t i = 0; i < polygons.size(); ++i) {
|
|
|
|
|
const auto& polygon = polygons[i];
|
|
|
|
|
cv::Scalar color = colors[i % colors.size()]; // 循环使用颜色
|
|
|
|
|
|
|
|
|
|
// 将多边形点转换为整数类型
|
|
|
|
|
std::vector<cv::Point> intPolygon;
|
|
|
|
|
for (const auto& pt : polygon) {
|
|
|
|
|
intPolygon.push_back(cv::Point(static_cast<int>(pt.x), static_cast<int>(pt.y)));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 绘制多边形
|
|
|
|
|
cv::polylines(image, intPolygon, true, color, 2);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|