You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
556 lines
14 KiB
556 lines
14 KiB
#include "API_UnderStitch.h"
|
|
#include "Arith_UnderStitch.h"
|
|
#include "Arith_Utils.h"
|
|
#include "Arith_CoordModule.h"
|
|
#include "Arith_FeaMatch.h"
|
|
#include <opencv2/opencv.hpp>
|
|
#include <omp.h>
|
|
|
|
#ifdef _WIN32
|
|
#include <direct.h>
|
|
#include <io.h>
|
|
#define MKDIR(dir) _mkdir(dir)
|
|
#define ACCESS _access
|
|
#else
|
|
#include <sys/stat.h>
|
|
#include <unistd.h>
|
|
#define MKDIR(dir) mkdir(dir, 0777)
|
|
#define ACCESS access
|
|
#endif
|
|
|
|
using namespace std;
|
|
using namespace cv;
|
|
|
|
API_UnderStitch* API_UnderStitch::Create(std::string cachedir)
|
|
{
|
|
return new UnderStitch(cachedir);
|
|
}
|
|
|
|
|
|
void API_UnderStitch::Destroy(API_UnderStitch* obj)
|
|
{
|
|
delete obj;
|
|
}
|
|
|
|
|
|
UnderStitch::UnderStitch(std::string cachedir)
|
|
{
|
|
// 创建缓存对象
|
|
_cache = new FileCache<FrameCache>(50, cachedir);
|
|
|
|
_GeoSolver = new GeoSolver();
|
|
|
|
_FeaMatcher = new FeatureMatcher(DetectorType::SIFT, MatcherType::FLANN);
|
|
|
|
_BATask = new BA_Task(_cache);
|
|
|
|
_BlendTask = new MapBlend(_cache);
|
|
|
|
_panPara = { 0 };
|
|
|
|
_totalFrameCnt = 0;
|
|
}
|
|
|
|
UnderStitch::~UnderStitch()
|
|
{
|
|
delete _GeoSolver;
|
|
delete _cache;
|
|
}
|
|
|
|
|
|
|
|
|
|
UPanInfo UnderStitch::InitMap(FrameInfo info)
|
|
{
|
|
// 设置拼接原点
|
|
_GeoSolver->SetOriginPoint(info);
|
|
|
|
cv::Mat H0 = _GeoSolver->findHomography(info);
|
|
|
|
cv::Point2f ct_geo = warpPointWithH(H0, cv::Point2f(info.nWidth / 2, info.nHeight / 2));// 视场中心的地理坐标
|
|
|
|
// 中心原始空间分辨率 M/p
|
|
double gsd = (info.nEvHeight * info.camInfo.fPixelSize) / (info.camInfo.nFocus * 1000);
|
|
|
|
// 全景图初始化
|
|
UPanInfo panPara = { 0 };
|
|
panPara.m_pan_width = MIN(info.nWidth * 5,8000);//全景宽
|
|
panPara.m_pan_height = MIN(info.nWidth * 5, 8000);//全景高
|
|
panPara.scale = 6;//比例尺,1m = ?pix
|
|
|
|
|
|
// 直接无平移解算
|
|
cv::Mat H_0 = getAffineFromGeo2Pan(panPara);
|
|
auto cur = warpPointWithH(H_0, ct_geo);
|
|
|
|
// 计算平移到全景图固定点的平移量，从此处开始拼接
|
|
int planX = panPara.m_pan_width/2 + 400 ;
|
|
int planY = panPara.m_pan_height;
|
|
|
|
|
|
panPara.map_shiftX = planX - (cur.x);//平移X
|
|
panPara.map_shiftY = planY - (cur.y);//平移Y
|
|
|
|
// 获取全景图的投影矩阵，全景图配置更新前保持不变
|
|
_H_pan = getAffineFromGeo2Pan(panPara);
|
|
|
|
|
|
// 计算全景图瓦片相关
|
|
auto P1 = getBLHFromPan(cv::Point2f(0, 0), _H_pan);
|
|
auto P2 = getBLHFromPan(cv::Point2f(panPara.m_pan_width, 0), _H_pan);
|
|
|
|
auto P3 = getBLHFromPan(cv::Point2f(panPara.m_pan_width, panPara.m_pan_height), _H_pan);
|
|
auto P4 = getBLHFromPan(cv::Point2f(0, panPara.m_pan_height), _H_pan);
|
|
|
|
auto minL = min(min(min(P1.L, P2.L), P3.L), P4.L);
|
|
auto maxL = max(max(max(P1.L, P2.L), P3.L), P4.L);
|
|
|
|
auto minB = min(min(min(P1.B, P2.B), P3.B), P4.B);
|
|
auto maxB = max(max(max(P1.B, P2.B), P3.B), P4.B);
|
|
|
|
_panTile.boxLatLon.north = maxB;
|
|
_panTile.boxLatLon.south = minB;
|
|
_panTile.boxLatLon.west = minL;
|
|
_panTile.boxLatLon.east = maxL;
|
|
_panTile.ind.z = _googleProduct.ZoomLevel(panPara.scale);
|
|
|
|
// 生成瓦片划分
|
|
_taskTilesVec = _googleProduct.CalcTileOverlayVec(_panTile, panPara.m_pan_width, panPara.m_pan_height);
|
|
|
|
|
|
return panPara;
|
|
}
|
|
|
|
|
|
UPanInfo UnderStitch::Init(FrameInfo info)
|
|
{
|
|
_panPara = InitMap(info);
|
|
|
|
_panImage = cv::Mat(_panPara.m_pan_height, _panPara.m_pan_width, CV_8UC4, cv::Scalar(255, 255, 255, 255));
|
|
|
|
_panMask = cv::Mat::zeros(_panPara.m_pan_height, _panPara.m_pan_width, CV_8UC1);
|
|
|
|
// 默认配置参数
|
|
_config.bOutGoogleTile = true;
|
|
_config.bUseBA = true;
|
|
_config.bOutFrameTile = true;
|
|
|
|
return _panPara;
|
|
}
|
|
|
|
void UnderStitch::SetOutput(std::string filename, std::string outdir)
|
|
{
|
|
_filename = filename;
|
|
_outDir = outdir;
|
|
_kmlPath = _outDir + "/" + _filename + ".kml";
|
|
|
|
// 创建输出目录
|
|
if (ACCESS(_outDir.c_str(), 0) != 0)
|
|
{
|
|
MKDIR(_outDir.c_str());
|
|
}
|
|
}
|
|
|
|
void UnderStitch::SetConfig(UPanConfig config)
|
|
{
|
|
_config = config;
|
|
}
|
|
|
|
SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S img, FrameInfo para)
|
|
{
|
|
// 快拼
|
|
GeoStitch(img, para);
|
|
|
|
|
|
if (_config.bUseBA)
|
|
{
|
|
// 预处理+缓存
|
|
ReceiveFrame(img, para);
|
|
|
|
// 按照需要执行优化
|
|
//if (0)
|
|
//{
|
|
// ProcessFrame(_recvFrameKey);
|
|
//}
|
|
|
|
}
|
|
|
|
// todo
|
|
// 推扫出视场，重新初始化全景图
|
|
return 0;
|
|
}
|
|
|
|
SINT32 UnderStitch::OptAndOutCurrPan()
|
|
{
|
|
if (_config.bUseBA)
|
|
{
|
|
// 优化所有帧
|
|
ProcessFrame(_recvFrameKey);
|
|
}
|
|
|
|
// 输出当前所有瓦片
|
|
_googleProduct.ExportTileSet(_panImage, _taskTilesVec, _outDir, _filename);
|
|
|
|
return _recvFrameKey.size();
|
|
}
|
|
|
|
|
|
|
|
//BYTE8 UnderStitch::GeoStitch(GD_VIDEO_FRAME_S img, FrameInfo para)
|
|
//{
|
|
// Proj t_Proj = _GeoStitcher->AnlayseTform(para);
|
|
//
|
|
// // 计算帧的map四至
|
|
// cv::Point2f leftTop_map = _GeoStitcher->back_project(cv::Point2f(0,0), t_Proj, _panPara);
|
|
// cv::Point2f rightTop_map = _GeoStitcher->back_project(cv::Point2f(img.u32Width,0), t_Proj, _panPara);
|
|
// cv::Point2f rightBottom_map = _GeoStitcher->back_project(cv::Point2f(img.u32Width,img.u32Height), t_Proj, _panPara);
|
|
// cv::Point2f leftBottom_map = _GeoStitcher->back_project(cv::Point2f(0,img.u32Height), t_Proj, _panPara);
|
|
//
|
|
//
|
|
// auto H = _GeoStitcher->findHomography(t_Proj,_panPara);
|
|
// leftTop_map = warpPointWithH(H,cv::Point2f(0,0));
|
|
// rightTop_map = warpPointWithH(H,cv::Point2f(img.u32Width,0));
|
|
// rightBottom_map = warpPointWithH(H,cv::Point2f(img.u32Width,img.u32Height));
|
|
// leftBottom_map = warpPointWithH(H,cv::Point2f(0,img.u32Height));
|
|
//
|
|
// // 计算全景图的范围
|
|
// int right = max(max(max(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x);
|
|
// int left = min(min(min(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x);
|
|
// int top = min(min(min(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y);
|
|
// int bottom = max(max(max(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y);
|
|
//
|
|
//
|
|
// int xRange = right - left;
|
|
// int yRnage = bottom - top;
|
|
//
|
|
// //反映射到像素坐标
|
|
// int valid_top = std::max(0, top);
|
|
// int valid_bottom = std::min(_pan.cols, bottom);
|
|
// int valid_left = std::max(0, left);
|
|
// int valid_right = std::min(_pan.rows, right);
|
|
//
|
|
//#pragma omp parallel for
|
|
// for (int i = valid_top; i < valid_bottom; i++)
|
|
// {
|
|
// for (int j = valid_left; j < valid_right; j++)
|
|
// {
|
|
// //转换为pixel坐标
|
|
// cv::Point2f p_img = _GeoStitcher->project(Point2f(j, i), t_Proj, _panPara);
|
|
//
|
|
// p_img = warpPointWithH(H.inv(),Point2f(j, i));
|
|
// if (p_img.x >= 0 && p_img.y >= 0 && p_img.x < img.u32Width && p_img.y < img.u32Height)
|
|
// {
|
|
// _pan.data[i * _pan.rows + j] =
|
|
// FourPointInterpolation(img.u64VirAddr[0], img.u32Width, img.u32Height, p_img.x, p_img.y);
|
|
// }
|
|
// }
|
|
// }
|
|
//
|
|
//
|
|
// return 0;
|
|
//}
|
|
|
|
SINT32 UnderStitch::GeoStitch(GD_VIDEO_FRAME_S img, FrameInfo para)
|
|
{
|
|
// 获取RGBA图像
|
|
cv::Mat src = getRGBAMatFromGDFrame(img,img.u64VirAddr[0]);
|
|
|
|
// 地理投影H1
|
|
cv::Mat H_geo = _GeoSolver->findHomography(para);
|
|
|
|
// 结合全景图投影H
|
|
cv::Mat H = _H_pan * H_geo;
|
|
|
|
TileInfo info = { 0 };
|
|
cv::Mat Orth_Image = getOrthImage(src, para, info);
|
|
|
|
if (_config.bOutFrameTile)
|
|
{
|
|
// 输出单帧谷歌瓦片
|
|
_googleProduct.ExportGeoPng(Orth_Image, info, _outDir + "/FrameTile");
|
|
}
|
|
|
|
//src = src.colRange(200, src.cols);
|
|
|
|
// 利用H投影当前帧到全景
|
|
cv::Mat imagetmp(_panImage.size(), CV_8UC3, cv::Scalar(0, 0, 0));
|
|
//cv::Mat imagetmp(_panImage.size(), CV_8UC3);
|
|
cv::warpPerspective(src, imagetmp, H, imagetmp.size(), cv::INTER_LINEAR, cv::BORDER_TRANSPARENT);
|
|
|
|
|
|
cv::Mat mask = cv::Mat::ones(src.size(), CV_8UC1);
|
|
|
|
cv::Mat warped_mask;
|
|
cv::warpPerspective(mask, warped_mask, H, imagetmp.size(), cv::INTER_LINEAR);
|
|
imagetmp.copyTo(_panImage, warped_mask);
|
|
|
|
// 覆盖区域掩码更新
|
|
_panMask = _panMask | warped_mask;
|
|
|
|
|
|
return 0;
|
|
}
|
|
|
|
// 废弃，改为输出单帧正射后的瓦片
|
|
void UnderStitch::CutTileRealTime()
|
|
{
|
|
// 在4倍降采样图上计算瓦片覆盖情况
|
|
int downSampleR = 4;
|
|
|
|
cv::Mat panMasksmall;
|
|
cv::resize(_panMask, panMasksmall, cv::Size(_panMask.cols / downSampleR, _panMask.rows / downSampleR));
|
|
|
|
// 计算覆盖图的积分图
|
|
cv::Mat integralMask;
|
|
cv::integral(panMasksmall, integralMask, CV_32S);
|
|
|
|
// 遍历已经在初始化阶段得到的瓦片划分
|
|
auto tiles = _taskTilesVec;
|
|
|
|
int cnt = 0;
|
|
for (size_t i = 0; i < tiles.size(); i++)
|
|
{
|
|
TileInfo* ptile = &_taskTilesVec[i];
|
|
RECT32S box = ptile->boxInPan;
|
|
|
|
// 跳过已输出块
|
|
if (ptile->nStatus > 0)
|
|
{
|
|
//cnt++;
|
|
continue;
|
|
}
|
|
|
|
SINT32 x1 = box.x/ downSampleR;
|
|
SINT32 y1 = box.y/ downSampleR;
|
|
SINT32 x2 = box.x/ downSampleR + box.w/ downSampleR;
|
|
SINT32 y2 = box.y/ downSampleR + box.h/ downSampleR;
|
|
|
|
// 使用积分图快速计算区域和
|
|
float maskSum = integralMask.at<int>(y2, x2)
|
|
- integralMask.at<int>(y1, x2)
|
|
- integralMask.at<int>(y2, x1)
|
|
+ integralMask.at<int>(y1, x1);
|
|
|
|
// 计算瓦片影像覆盖率
|
|
float Coverage = maskSum * downSampleR * downSampleR / (box.w * box.h);
|
|
|
|
if (Coverage > 0.95)
|
|
{
|
|
// 输出并标记
|
|
ptile->nStatus = 1;
|
|
_googleProduct.ExportOneTile(_panImage, *ptile, _outDir, _filename);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
|
|
{
|
|
// 构造单帧缓存
|
|
auto _t_frame_cache = std::make_shared<FrameCache>();
|
|
|
|
_t_frame_cache->_para = para;
|
|
_t_frame_cache->_frame_info = img;
|
|
|
|
int imgSize = 0;
|
|
|
|
if(img.enPixelFormat == GD_PIXEL_FORMAT_GRAY_Y8)
|
|
{
|
|
imgSize = img.u32Height * img.u32Width;
|
|
}
|
|
if(img.enPixelFormat == GD_PIXEL_FORMAT_RGB_PACKED)
|
|
{
|
|
imgSize = img.u32Height * img.u32Width * 3;
|
|
}
|
|
if (img.enPixelFormat == GD_PIXEL_FORMAT_NV12)
|
|
{
|
|
imgSize = img.u32Height * img.u32Width * 1.5;
|
|
}
|
|
|
|
// 深拷贝图像数据
|
|
memcpy(_t_frame_cache->_data,img.u64VirAddr[0],imgSize);
|
|
|
|
// 提取特征点
|
|
std::vector<cv::KeyPoint> keypoints;
|
|
cv::Mat descriptors;
|
|
|
|
|
|
cv::Mat src = getRGBAMatFromGDFrame(img, img.u64VirAddr[0]);
|
|
|
|
_FeaMatcher->extractFeatures(src, keypoints, descriptors);
|
|
|
|
size_t keyNum = MIN(keypoints.size(), FEA_NUM_MAX);
|
|
|
|
_t_frame_cache->ptNum = keyNum;
|
|
|
|
// 保存特征点
|
|
memcpy(_t_frame_cache->_pt, keypoints.data(), sizeof(cv::KeyPoint) * keyNum);
|
|
|
|
// 保存描述子
|
|
memcpy(_t_frame_cache->_desp, descriptors.data, sizeof(float) * keyNum * FEA_DES_SIZE);
|
|
|
|
cv::Mat_<double> H0 = _GeoSolver->findHomography(para);
|
|
|
|
// 保存初始H
|
|
memcpy(_t_frame_cache->H, H0.data, sizeof(double) * 9);
|
|
|
|
|
|
// 预处理结果加入文件缓存
|
|
_cache->set(para.nFrmID,_t_frame_cache);
|
|
|
|
_recvFrameKey.push_back(para.nFrmID);
|
|
|
|
// 优化表初始化
|
|
_recvFrameOptFlag[para.nFrmID] = 0;
|
|
|
|
_totalFrameCnt++;
|
|
|
|
return _totalFrameCnt;
|
|
}
|
|
|
|
SINT32 UnderStitch::ProcessFrame(vector<KeyType> keys)
|
|
{
|
|
if (keys.size() == 0)
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
// 优化所有帧
|
|
//_BATask->OptFrame(keys, _H_pan);
|
|
|
|
// 重投影所有帧到全景
|
|
_BlendTask->DirectMap(keys, _H_pan, _panImage,_panMask);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
bool UnderStitch::ExportGeoPng()
|
|
{
|
|
// 计算全景图的坐标范围
|
|
auto P1 = getBLHFromPan(cv::Point2f(0, 0), _H_pan);
|
|
auto P2 = getBLHFromPan(cv::Point2f(_panImage.cols, 0), _H_pan);
|
|
|
|
auto P3 = getBLHFromPan(cv::Point2f(_panImage.cols, _panImage.rows), _H_pan);
|
|
auto P4 = getBLHFromPan(cv::Point2f(0, _panImage.rows), _H_pan);
|
|
|
|
auto minL = min(min(min(P1.L, P2.L), P3.L), P4.L);
|
|
auto maxL = max(max(max(P1.L, P2.L), P3.L), P4.L);
|
|
|
|
auto minB = min(min(min(P1.B, P2.B), P3.B), P4.B);
|
|
auto maxB = max(max(max(P1.B, P2.B), P3.B), P4.B);
|
|
|
|
TileInfo info = { 0 };
|
|
|
|
info.boxLatLon.north = maxB;
|
|
info.boxLatLon.south = minB;
|
|
info.boxLatLon.west = minL;
|
|
info.boxLatLon.east = maxL;
|
|
info.tileName = _filename;
|
|
info.href = _filename + ".png";
|
|
|
|
// 输出谷歌地图产品
|
|
_googleProduct.ExportGeoPng(_panImage, info, _outDir);
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
cv::Mat UnderStitch::getOrthImage(cv::Mat src, FrameInfo para,TileInfo& tile)
|
|
{
|
|
cv::Mat H = _GeoSolver->findHomography(para);
|
|
|
|
cv::Rect2f roi = warpRectWithH_2Rect(H, src.size());
|
|
|
|
float scale = src.cols / MAX(roi.width, roi.height);
|
|
float shiftX = -roi.x * scale;
|
|
float shiftY = (roi.y + roi.height)* scale;
|
|
|
|
Mat H_proj = (Mat_<double>(3, 3) << scale, 0, shiftX,
|
|
0, -scale, shiftY,
|
|
0, 0, 1
|
|
);
|
|
cv::Rect2f roi2 = warpRectWithH_2Rect(H_proj * H, src.size());
|
|
|
|
cv::Mat dst;
|
|
warpPerspective(src, dst, H_proj * H, cv::Size(roi2.width, roi2.height));
|
|
|
|
|
|
// 计算全景图的坐标范围
|
|
auto P1 = _GeoSolver->getBLHFromFrame(H,cv::Point2f(0, 0));
|
|
auto P2 = _GeoSolver->getBLHFromFrame(H, cv::Point2f(src.cols, 0));
|
|
|
|
auto P3 = _GeoSolver->getBLHFromFrame(H, cv::Point2f(src.cols, src.rows));
|
|
auto P4 = _GeoSolver->getBLHFromFrame(H, cv::Point2f(0, src.rows));
|
|
|
|
auto minL = min(min(min(P1.L, P2.L), P3.L), P4.L);
|
|
auto maxL = max(max(max(P1.L, P2.L), P3.L), P4.L);
|
|
|
|
auto minB = min(min(min(P1.B, P2.B), P3.B), P4.B);
|
|
auto maxB = max(max(max(P1.B, P2.B), P3.B), P4.B);
|
|
|
|
|
|
|
|
tile.boxLatLon.north = maxB;
|
|
tile.boxLatLon.south = minB;
|
|
tile.boxLatLon.west = minL;
|
|
tile.boxLatLon.east = maxL;
|
|
tile.tileName = _filename + std::to_string(para.nFrmID);
|
|
tile.href = _filename + std::to_string(para.nFrmID) + ".png";
|
|
|
|
|
|
|
|
return dst;
|
|
}
|
|
|
|
GD_VIDEO_FRAME_S UnderStitch::ExportPanAddr()
|
|
{
|
|
GD_VIDEO_FRAME_S pan_out;
|
|
|
|
pan_out.enPixelFormat = GD_PIXEL_FORMAT_RGB_PACKED;
|
|
pan_out.u32Width = _panPara.m_pan_width;
|
|
pan_out.u32Height = _panPara.m_pan_height;
|
|
pan_out.u64VirAddr[0] = _panImage.data;
|
|
|
|
return pan_out;
|
|
}
|
|
|
|
cv::Mat UnderStitch::ExportPanMat()
|
|
{
|
|
return _panImage;
|
|
}
|
|
|
|
PointBLH UnderStitch::getBLHFromPan(cv::Point2f ptInPan, cv::Mat _H_panPara)
|
|
{
|
|
cv::Mat H_inv = _H_panPara.inv();
|
|
|
|
// 解算到局部地理系
|
|
cv::Point2f ptInGeo = warpPointWithH(H_inv,ptInPan);
|
|
|
|
return _GeoSolver->getBLHFromGeo(ptInGeo);
|
|
}
|
|
|
|
cv::Point2f UnderStitch::getPanXYFromBLH(PointBLH ptInBLH, cv::Mat _H_panPara)
|
|
{
|
|
// 经纬度转局部地理系
|
|
cv::Point2f ptGeo = _GeoSolver->getGeoFromBLH(ptInBLH);
|
|
|
|
// 投影到全景图
|
|
return warpPointWithH(_H_panPara, ptGeo);;
|
|
}
|
|
|
|
cv::Mat UnderStitch::getAffineFromGeo2Pan(UPanInfo _pan)
|
|
{
|
|
Mat H = (Mat_<double>(3, 3) << _pan.scale, 0, _pan.map_shiftX,
|
|
0, -_pan.scale, _pan.m_pan_height + _pan.map_shiftY,
|
|
0, 0, 1
|
|
);
|
|
return H;
|
|
}
|
|
|
|
|