#include "stdafx.h"
#include "superResolutionImg.h"

// openCV Library
#include <opencv2/opencv.hpp>  
#include "opencv2/nonfree/features2d.hpp"
using namespace cv;

#include <deque>

#include "fuseImg.h"
#include "histProject.h"


// 移除NN比率大于阈值的匹配
int ClearMatchByKnnRatio(std::vector<std::vector<cv::DMatch>> &matches, float ratio)
{
	try
	{
		// 移除匹配的数目
		int removedCount = 0;

		// 初始化迭代器
		std::vector<std::vector<cv::DMatch>>::iterator DMatchIterator = matches.begin();

		// 遍历所有匹配
		for (; DMatchIterator != matches.end(); ++DMatchIterator)
		{
			// 如果识别到两个最近邻
			if (DMatchIterator->size() > 1)
			{
				// 次近距离
				if ((*DMatchIterator)[1].distance > 0)
				{
					if ((*DMatchIterator)[0].distance / (*DMatchIterator)[1].distance > ratio)
					{
						DMatchIterator->clear();
						removedCount++;
					}
				}
				else
				{
					DMatchIterator->clear();
					removedCount++;
				}
			}
			else // 不包括两个最近邻
			{
				DMatchIterator->clear();
				removedCount++;
			}
		} // end of for

		return removedCount;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return 0;
	}
	catch(...)
	{
		return 0;
	}
}

// 获取对称匹配
int GetSysmmetryMatch(std::vector<cv::DMatch> &sysmMatches,
	const std::vector<std::vector<cv::DMatch>> &preMatches, 
	const std::vector<std::vector<cv::DMatch>> &nextMatches)                 
{
	try
	{
		sysmMatches.clear();

		// 对称匹配数目
		int sysmMatchNum = 0;

		// 遍历matches1的所有匹配
		std::vector<std::vector<cv::DMatch>>::const_iterator preMatchesIterator = preMatches.begin();
		for(; preMatchesIterator != preMatches.end(); preMatchesIterator++)
		{
			// 跳过被删除的匹配
			if (preMatchesIterator->size() < 2)
			{
				continue;
			}
			// 遍历matches2的所有匹配
			std::vector<std::vector<cv::DMatch>>::const_iterator nextMatchesIterator = nextMatches.begin();
			for(; nextMatchesIterator != nextMatches.end(); nextMatchesIterator++)
			{
				// 跳过被删除的匹配
				if (nextMatchesIterator->size() < 2)
				{
					continue;
				}

				if ((*preMatchesIterator)[0].queryIdx == (*nextMatchesIterator)[0].trainIdx 
					&& (*preMatchesIterator)[0].trainIdx == (*nextMatchesIterator)[0].queryIdx)
				{
					// 添加对称匹配
					sysmMatches.push_back(cv::DMatch((*preMatchesIterator)[0].queryIdx, (*preMatchesIterator)[0].trainIdx, (*preMatchesIterator)[0].distance));
					sysmMatchNum++;
					break;
				}
			}
		}

		// 返回对称匹配数目
		return sysmMatchNum;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return 0;
	}
	catch(...)
	{
		return 0;
	}
}


// 基于【对极约束】条件下的优质匹配识别（用到RANSAC估计方法）
bool GetRansacBaseMatch(std::vector<cv::DMatch>  &ransacMatches,    
	std::vector<cv::Point2f> &prePts,                      
	std::vector<cv::Point2f> &nextPts,
	const std::vector<cv::DMatch>   &matches,    
	const std::vector<cv::KeyPoint> &preKeyPts, 
	const std::vector<cv::KeyPoint> &nextKeyPts)           
{
	try
	{
		// 输入判断 对极约束至少需要8个点对, 
		if (matches.size() > preKeyPts.size() || matches.size() > nextKeyPts.size() || matches.size() < 12)
		{
			return false;
		}

		// 清理输出
		ransacMatches.clear();
		prePts.clear();
		nextPts.clear();

		// 转换KeyPoint类型到Point2f
		std::vector<DMatch>::const_iterator it = matches.begin();
		for(; it != matches.end(); it++)
		{
			// 得到左边特征点的坐标
			prePts.push_back(preKeyPts[it->queryIdx].pt);

			// 得到右边特征点的坐标
			nextPts.push_back(nextKeyPts[it->trainIdx].pt);
		}

		// 基于RANSAC计算基础矩阵
		std::vector<unsigned char> inliers(prePts.size(), 0);
		cv::Mat fundementalMat; // 基础矩阵
		fundementalMat = cv::findFundamentalMat(cv::Mat(prePts), // 输入匹配点集1
			cv::Mat(nextPts), // 输入匹配点集2
			inliers,		  // 输出匹配状态 outlier(0) inlier(1)
			CV_FM_RANSAC,	  // RANSAC方法
			3.0,			  // 到极线的距离
			0.99);			  // 置信概率

		// 提取通过的匹配
		std::vector<unsigned char>::const_iterator itInliers = inliers.begin();
		std::vector<cv::DMatch>::const_iterator itMatches = matches.begin();
		for (; itInliers != inliers.end(); ++itInliers,++itMatches)
		{
			if (*itInliers) // 有效匹配
			{
				ransacMatches.push_back((*itMatches));
			}
		}

		// 根据接收的匹配，重新计算基础矩阵：fundementalMat
		prePts.clear();
		nextPts.clear();

		std::vector<DMatch>::const_iterator itRansacMatches = ransacMatches.begin();
		for(; itRansacMatches != ransacMatches.end(); ++itRansacMatches)
		{
			// 得到左边特征点的坐标
			prePts.push_back(preKeyPts[itRansacMatches->queryIdx].pt);

			// 得到右边特征点的坐标
			nextPts.push_back(nextKeyPts[itRansacMatches->trainIdx].pt);
		}

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}


// 基于最小二乘计算仿射变换矩阵
bool GetAffineTransMat(cv::Mat &result, 
	const std::vector<Point2f> &prePoints, 
	const std::vector<Point2f> &nextPoints)
{
	try
	{
		int preSize = prePoints.size();
		int nextSize = nextPoints.size();

		if (preSize < 3 || nextSize < 3 || preSize != nextSize)
		{
			return false;
		}

		// 重新分配输出矩阵
		result.create(3, 3, CV_64FC1);
		result = cv::Scalar(0);

		// 匹配数目
		int MatchNum = preSize;

		/* set up matrices so we can unstack homography into X;  AX = B  */
		cv::Mat A(2 * MatchNum, 6, CV_64FC1);
		cv::Mat B(2 * MatchNum, 1, CV_64FC1);
		cv::Mat X(6,            1, CV_64FC1);

		// 置零
		A = cv::Scalar(0);
		B = cv::Scalar(0);
		X = cv::Scalar(0);

		// 初始化矩阵
		for(int i = 0; i < MatchNum; i++ )
		{
			A.at<double>(i, 0) = prePoints[i].x;
			A.at<double>(i, 1) = prePoints[i].y;
			A.at<double>(i, 2) = 1.0f;

			A.at<double>(i + MatchNum, 3) = prePoints[i].x;
			A.at<double>(i + MatchNum, 4) = prePoints[i].y;
			A.at<double>(i + MatchNum, 5) = 1.0f;

			B.at<double>(i,            0) = nextPoints[i].x;
			B.at<double>(i + MatchNum, 0) = nextPoints[i].y;
		}

		// 解方程
		cv::solve(A, B, X, CV_SVD);

		// 赋值
		result.at<double>(0, 0) = X.at<double>(0, 0); 
		result.at<double>(0, 1) = X.at<double>(1, 0); 
		result.at<double>(0, 2) = X.at<double>(2, 0); 
		result.at<double>(1, 0) = X.at<double>(3, 0); 
		result.at<double>(1, 1) = X.at<double>(4, 0); 
		result.at<double>(1, 2) = X.at<double>(5, 0); 
		result.at<double>(2, 0) = 0.0; 
		result.at<double>(2, 1) = 0.0; 
		result.at<double>(2, 2) = 1.0; 

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}


// 基于最小二乘计算透视变换矩阵
bool GetPerpectTransMat(cv::Mat &result, 
	const std::vector<Point2f> &prePoints, 
	const std::vector<Point2f> &nextPoints)
{
	try
	{
		int preSize = prePoints.size();
		int nextSize = nextPoints.size();

		if (preSize < 4 || nextSize < 4 || preSize != nextSize)
		{
			return false;
		}

		// 重新分配输出矩阵
		result.create(3, 3, CV_64FC1);
		result = cv::Scalar(0);

		// 匹配数目
		int MatchNum = nextSize;

		/* set up matrices so we can unstack homography into X;  AX = B  */
		cv::Mat A(2 * MatchNum, 8, CV_64FC1);
		cv::Mat B(2 * MatchNum, 1, CV_64FC1);
		cv::Mat X(8,            1, CV_64FC1);

		// 置零
		A = cv::Scalar(0);
		B = cv::Scalar(0);
		X = cv::Scalar(0);

		// 初始化矩阵
		for(int i = 0; i < MatchNum; i++ )
		{
			A.at<double>(i, 0) = prePoints[i].x;
			A.at<double>(i, 1) = prePoints[i].y;
			A.at<double>(i, 2) = 1.0f;
			A.at<double>(i, 6) = -prePoints[i].x * nextPoints[i].x;
			A.at<double>(i, 7) = -prePoints[i].y * nextPoints[i].x;

			A.at<double>(i + MatchNum, 3) = prePoints[i].x;
			A.at<double>(i + MatchNum, 4) = prePoints[i].y;
			A.at<double>(i + MatchNum, 5) = 1.0f;
			A.at<double>(i + MatchNum, 6) = -prePoints[i].x * nextPoints[i].y;
			A.at<double>(i + MatchNum, 7) = -prePoints[i].y * nextPoints[i].y;

			B.at<double>(i,            0) = nextPoints[i].x;
			B.at<double>(i + MatchNum, 0) = nextPoints[i].y;
		}

		// 解方程
		cv::solve(A, B, X, CV_SVD);

		// 赋值
		result.at<double>(0, 0) = X.at<double>(0, 0); 
		result.at<double>(0, 1) = X.at<double>(1, 0); 
		result.at<double>(0, 2) = X.at<double>(2, 0); 
		result.at<double>(1, 0) = X.at<double>(3, 0); 
		result.at<double>(1, 1) = X.at<double>(4, 0); 
		result.at<double>(1, 2) = X.at<double>(5, 0); 
		result.at<double>(2, 0) = X.at<double>(6, 0); 
		result.at<double>(2, 1) = X.at<double>(7, 0); 
		result.at<double>(2, 2) = 1.0; 

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return false;
	}
	catch(...)
	{
		return false;
	}
}


// 相邻图像间特征匹配，并计算关系
void CalImgAdjacentMat(std::deque<cv::Mat> &adjacentMatDeque, 
	std::deque<int> &successTagDeque, 
	const std::deque<std::vector<cv::KeyPoint>> &imgKeyptsDeque,
	const std::deque<cv::Mat> &imgDescriptorsDeque)
{
	if (imgKeyptsDeque.empty() == true 
		|| imgDescriptorsDeque.empty() == true 
		|| imgKeyptsDeque.size() != imgDescriptorsDeque.size())
	{
		return;
	}

	adjacentMatDeque.clear();
	successTagDeque.clear();

	// 总数目
	size_t count = imgKeyptsDeque.size();

	for (size_t i = 0; i < (count - 1); i++)
	{
		// Flann匹配器
		cv::FlannBasedMatcher matcher; 

		std::vector<cv::KeyPoint> preImgKeypts = imgKeyptsDeque.at(i);
		std::vector<cv::KeyPoint> nextImgKeypts = imgKeyptsDeque.at(i + 1);

		cv::Mat preImgDescriptors = imgDescriptorsDeque.at(i);
		cv::Mat nextImgDescriptors = imgDescriptorsDeque.at(i + 1);

		// 初步匹配结果
		std::vector<std::vector<cv::DMatch>> Pre2NextMatches;
		std::vector<std::vector<cv::DMatch>> Next2PreMatches;

		// 最近邻/次近邻特征匹配
		matcher.knnMatch(preImgDescriptors, nextImgDescriptors, Pre2NextMatches, 2);  // 前向匹配
		matcher.knnMatch(nextImgDescriptors, preImgDescriptors, Next2PreMatches, 2);  // 后向匹配

		// 剔除部分误匹配
		ClearMatchByKnnRatio(Pre2NextMatches, 0.65f);
		ClearMatchByKnnRatio(Next2PreMatches, 0.65f);

		// 对称匹配
		vector<DMatch> SysmMatches;
		GetSysmmetryMatch(SysmMatches, 
			Pre2NextMatches, 
			Next2PreMatches);

		// Ransac：对极约束提出误匹配
		std::vector<cv::DMatch> RansacMatches;
		std::vector<Point2f> PrePoints;
		std::vector<Point2f> NextPoints;
		bool MatchResult = false;
		MatchResult = GetRansacBaseMatch(RansacMatches,    // Ransac匹配
			                             PrePoints, 
			                             NextPoints,
			                             SysmMatches,      // 对称匹配
			                             preImgKeypts, 
			                             nextImgKeypts);

		if (MatchResult == false)
		{
			adjacentMatDeque.push_back(cv::Mat());
			successTagDeque.push_back(0);
			continue;
		}

		// 两帧图像的连接矩阵
		bool TransResult = false;
		cv::Mat AdjacentMat;
		TransResult = GetPerpectTransMat(AdjacentMat, PrePoints, NextPoints);

		if (TransResult == false)
		{
			adjacentMatDeque.push_back(cv::Mat());
			successTagDeque.push_back(0);
			continue;
		}
		else
		{
			adjacentMatDeque.push_back(AdjacentMat);
			successTagDeque.push_back(1);
		}
	}
}

// 灰度图像（红外图像）超分辨率构建
int SuperResolutionImg(cv::Mat &dst, const std::deque<cv::Mat> &imgDeque, double multiple)
{
	if (imgDeque.size() < 2)
	{
		return 0;
	}

	// 超分倍数
	if (multiple < 1)
	{
		multiple = 1;
	}
	else
		if (multiple > 2)
		{
			multiple = 2;
		}

	// 基准 第0张图像
	cv::Size imgSize = imgDeque[0].size();
	for (size_t i = 0; i < imgDeque.size(); i++)
	{
		if (imgDeque[i].type() != CV_8UC1 || imgDeque[i].size() != imgSize)  // 尺寸和类型判断
		{
			return 0;
		}
	}

	// 图像增大一倍后的集合
	std::deque<cv::Mat> imgUpSampleDeque;
	for (size_t i = 0; i < imgDeque.size(); i++)
	{
		// 尺寸加倍
		cv::Mat upSampleImg;
		cv::resize(imgDeque.at(i), upSampleImg, cv::Size(int(imgDeque.at(i).cols * multiple), int(imgDeque.at(i).rows * multiple)), 0, 0, 1);
		imgUpSampleDeque.push_back(upSampleImg);
	}

	// 特征集合
	std::deque<std::vector<cv::KeyPoint>> imgKeyptsDeque;
	std::deque<cv::Mat> imgDescriptorsDeque;
	cv::SIFT sift(0, 3, 0.004); // 小阈值
	for (size_t i = 0; i < imgUpSampleDeque.size(); i++)
	{
		// 计算特征点 特征向量
		std::vector<cv::KeyPoint> imgKeypts;
		cv::Mat imgDescriptors;
		sift(imgUpSampleDeque.at(i), noArray(), imgKeypts, imgDescriptors, false);

		// 保存
		imgKeyptsDeque.push_back(imgKeypts);
		imgDescriptorsDeque.push_back(imgDescriptors);
	}

	// 图像特征匹配
	std::deque<cv::Mat> adjacentMatDeque;
	std::deque<int>    successTagDeque;

	CalImgAdjacentMat(adjacentMatDeque, successTagDeque, imgKeyptsDeque, imgDescriptorsDeque);

	// 选出最佳的一段数据
	std::deque<int> segment;
	for (size_t i = 0; i < successTagDeque.size(); i++)
	{
		int count = 0;
		for (size_t t = i; t < successTagDeque.size(); t++)
		{
			if (successTagDeque.at(t) == 1)
			{
				count++;
			}
			else
			{
				break;
			}
		}

		segment.push_back(count);
	}

	// 找出最大值及其位置
	std::deque<int> segmentCopy = segment;
	std::sort(segmentCopy.begin(), segmentCopy.end());
	int maxVal = segmentCopy.at(segmentCopy.size() - 1);

	// 最少3张图像
	if (maxVal < 2)
	{
		return 0; 
	}

	int location = 0; // 位置
	for (size_t i = 0; i < segment.size(); i++)
	{
		if (maxVal == segment.at(i))
		{
			location = i;
			break;
		}
	}

	// 融合全部数据
	if (maxVal == static_cast<int>(successTagDeque.size()))
	{
		// 图像细节增强
		for (size_t i = 0; i < imgUpSampleDeque.size(); i++)
		{
			cv::Mat img;
			imgUpSampleDeque[i].copyTo(img);
			GNRHP(img, imgUpSampleDeque[i]);
		}

		// 逐步融合
		for (int i = imgUpSampleDeque.size() - 2; i > -1; i--)
		{
			cv::Mat img_pre = imgUpSampleDeque[i];
			cv::Mat img_next = imgUpSampleDeque[i+1];

			cv::Mat img_preNext;
			img_pre.copyTo(img_preNext);

			// 两帧图像的连接矩阵
			cv::Mat m_AdjacentMat = adjacentMatDeque[i];

			// 生成next 替换 pre 图像 
			double a00 = m_AdjacentMat.at<double>(0, 0);
			double a01 = m_AdjacentMat.at<double>(0, 1);
			double a02 = m_AdjacentMat.at<double>(0, 2);
			double a10 = m_AdjacentMat.at<double>(1, 0); 
			double a11 = m_AdjacentMat.at<double>(1, 1);
			double a12 = m_AdjacentMat.at<double>(1, 2);
			double a20 = m_AdjacentMat.at<double>(2, 0);
			double a21 = m_AdjacentMat.at<double>(2, 1);
			double a22 = m_AdjacentMat.at<double>(2, 2);

			for(int r = 0; r < img_preNext.rows; r++)
			{
				//	#pragma omp parallel for
				for (int c = 0; c < img_preNext.cols; c++)
				{
					double denominator = c * a20 + r * a21 + a22;			
					double src_x = (c * a00 + r * a01 + a02) / denominator;
					double src_y = (c * a10 + r * a11 + a12) / denominator;	

					// 最近邻，速度快
					int x_nearest = int(src_x + 0.5);
					int y_nearest = int(src_y + 0.5);

					if ( x_nearest >= 0 && y_nearest >= 0 && x_nearest < img_next.cols && y_nearest < img_next.rows)
					{
						img_preNext.at<unsigned char>(r, c) =  img_next.at<unsigned char>(y_nearest, x_nearest);
					}
				}
			} // 搬移像素结束

			// 融合 img_pre 和 img_preNext
			cv::Mat fusion;
			FuseImg(fusion, img_pre, img_preNext, "sym4", 10);

			// 用 fusion 替换 validImgUpSampleDeque[i]
			fusion.copyTo(imgUpSampleDeque[i]);
		}

		imgUpSampleDeque[0].copyTo(dst);

		return imgUpSampleDeque.size();
	} 
	else
	{
		// 选出有效的图像片段
		std::deque<cv::Mat> validImgUpSampleDeque(imgUpSampleDeque.begin() + location, imgUpSampleDeque.begin() + location + maxVal + 1);

		// 图像细节增强
		for (size_t i = 0; i < validImgUpSampleDeque.size(); i++)
		{
			cv::Mat img;
			validImgUpSampleDeque[i].copyTo(img);
			GNRHP(img, validImgUpSampleDeque[i]);
		}

		std::deque<cv::Mat> validAdjacentMatDeque(adjacentMatDeque.begin() + location, adjacentMatDeque.begin() + location + maxVal);

		imgUpSampleDeque.clear();
		adjacentMatDeque.clear();


		// 逐步融合
		for (int i = validImgUpSampleDeque.size() - 2; i > -1; i--)
		{
			cv::Mat img_pre = validImgUpSampleDeque[i];
			cv::Mat img_next = validImgUpSampleDeque[i+1];

			cv::Mat img_preNext;
			img_pre.copyTo(img_preNext);

			// 两帧图像的连接矩阵
			cv::Mat m_AdjacentMat = validAdjacentMatDeque[i];

			// 生成next 替换 pre 图像 
			double a00 = m_AdjacentMat.at<double>(0, 0);
			double a01 = m_AdjacentMat.at<double>(0, 1);
			double a02 = m_AdjacentMat.at<double>(0, 2);
			double a10 = m_AdjacentMat.at<double>(1, 0); 
			double a11 = m_AdjacentMat.at<double>(1, 1);
			double a12 = m_AdjacentMat.at<double>(1, 2);
			double a20 = m_AdjacentMat.at<double>(2, 0);
			double a21 = m_AdjacentMat.at<double>(2, 1);
			double a22 = m_AdjacentMat.at<double>(2, 2);

			for(int r = 0; r < img_preNext.rows; r++)
			{
				//	#pragma omp parallel for
				for (int c = 0; c < img_preNext.cols; c++)
				{
					double denominator = c * a20 + r * a21 + a22;			
					double src_x = (c * a00 + r * a01 + a02) / denominator;
					double src_y = (c * a10 + r * a11 + a12) / denominator;	

					// 最近邻，速度快
					int x_nearest = int(src_x + 0.5);
					int y_nearest = int(src_y + 0.5);

					if ( x_nearest >= 0 && y_nearest >= 0 && x_nearest < img_next.cols && y_nearest < img_next.rows)
					{
						img_preNext.at<unsigned char>(r, c) =  img_next.at<unsigned char>(y_nearest, x_nearest);
					}
				}
			} // 搬移像素结束

			// 融合 img_pre 和 img_preNext
			cv::Mat fusion;
			FuseImg(fusion, img_pre, img_preNext, "sym4", 10);

			// 用 fusion 替换 validImgUpSampleDeque[i]
			fusion.copyTo(validImgUpSampleDeque[i]);
		}

		validImgUpSampleDeque[0].copyTo(dst);

		return validImgUpSampleDeque.size();
	}
}