﻿#include "EvisionUtils.h"
#include <sstream>
#include <QFileInfo>

EvisionUtils::EvisionUtils()
{
}


EvisionUtils::~EvisionUtils()
{
}
/*
 * 获取当前路径
 */
std::string EvisionUtils::getCurrentPath()
{
	std::ostringstream ss;
	ss << _pgmptr;
	std::string abspath = ss.str();
	int last_slash_pos = abspath.rfind("\\");
	std::string path = abspath.substr(0, last_slash_pos);
	return path;
}
/*
 * 路径整理
 */
std::string EvisionUtils::pathPurify(std::string& src)
{
	QString str1 = QString::fromStdString(src);
	QString str2 = str1.replace(QRegExp("\\\\"), "/");
	QFileInfo* fileinfo = new QFileInfo(str2);
	if (fileinfo->exists())
	{
		return fileinfo->absoluteFilePath().toStdString();
	}
	return str2.toStdString();
}

std::string EvisionUtils::getDataPath()
{
	return EvisionUtils::pathPurify(EvisionUtils::getCurrentPath() + "\\..\\..\\..\\data");
}

/*
 * cv::Mat转换为QImage
 */
QImage EvisionUtils::cvMat2QImage(const cv::Mat& mat)
{
	cv::Mat mat_8;
	switch (mat.channels())
	{
	case 1:
		mat.convertTo(mat_8, CV_8UC1);
		break;
	case 2:
		mat.convertTo(mat_8, CV_8UC2);
		break;
	case 3:
		mat.convertTo(mat_8, CV_8UC3);
		break;
	case 4:
		mat.convertTo(mat_8, CV_8UC4);
		break;
	}
	// 8-bits unsigned, NO. OF CHANNELS = 1
	if (mat_8.type() == CV_8UC1)
	{
		QImage image(mat_8.cols, mat_8.rows, QImage::Format_Indexed8);
		// Set the color table (used to translate colour indexes to qRgb values)
		image.setColorCount(256);
		for (int i = 0; i < 256; i++)
		{
			image.setColor(i, qRgb(i, i, i));
		}
		// Copy input Mat
		uchar *pSrc = mat_8.data;
		for (int row = 0; row < mat_8.rows; row++)
		{
			uchar *pDest = image.scanLine(row);
			memcpy(pDest, pSrc, mat_8.cols);
			pSrc += mat_8.step;
		}
		return image;
	}
	// 8-bits unsigned, NO. OF CHANNELS = 3
	else if (mat_8.type() == CV_8UC3)
	{
		// Copy input Mat
		const uchar *pSrc = (const uchar*)mat_8.data;
		// Create QImage with same dimensions as input Mat
		QImage image(pSrc, mat_8.cols, mat_8.rows, mat_8.step, QImage::Format_RGB888);
		return image.rgbSwapped();
	}
	else if (mat_8.type() == CV_8UC4)
	{
		// Copy input Mat
		const uchar *pSrc = (const uchar*)mat_8.data;
		// Create QImage with same dimensions as input Mat
		QImage image(pSrc, mat_8.cols, mat_8.rows, mat_8.step, QImage::Format_ARGB32);
		return image.copy();
	}
	else
	{
		return getDefaultImage();
	}
}

/*
 * QImage转换为cv::Mat
 */
cv::Mat EvisionUtils::QImage2cvMat(QImage image)
{
	cv::Mat mat;
	//qDebug() << image.format();
	switch (image.format())
	{
	case QImage::Format_ARGB32:
	case QImage::Format_RGB32:
	case QImage::Format_ARGB32_Premultiplied:
		mat = cv::Mat(image.height(), image.width(), CV_8UC4, (void*)image.constBits(), image.bytesPerLine());
		break;
	case QImage::Format_RGB888:
		mat = cv::Mat(image.height(), image.width(), CV_8UC3, (void*)image.constBits(), image.bytesPerLine());
		cv::cvtColor(mat, mat, cv::COLOR_BGR2RGB);
		break;
	case QImage::Format_Indexed8:
		mat = cv::Mat(image.height(), image.width(), CV_8UC1, (void*)image.constBits(), image.bytesPerLine());
		break;
	}
	return mat;
}

/*
* 灰度图转彩虹图
*/

cv::Mat EvisionUtils::gray2rainbowcolor(cv::Mat& img) {
	cv::Mat img_color = cv::Mat::zeros(img.rows, img.cols, CV_8UC3);
#define IMG_B(img,y,x) img.at<cv::Vec3b>(y,x)[0]
#define IMG_G(img,y,x) img.at<cv::Vec3b>(y,x)[1]
#define IMG_R(img,y,x) img.at<cv::Vec3b>(y,x)[2]
	uchar tmp2 = 0;
	//转为彩虹图的具体算法，主要思路是把灰度图对
	//应的0～255的数值分别转换成彩虹色：红、橙、黄、绿、青、蓝。
	for (int y = 0; y < img.rows; y++)
	{
		for (int x = 0; x < img.cols; x++)
		{
			tmp2 = img.at<uchar>(y, x);
			if (tmp2 == 0)
				continue;
			if (tmp2 <= 51)
			{
				IMG_B(img_color, y, x) = 255;
				IMG_G(img_color, y, x) = tmp2 * 5;
				IMG_R(img_color, y, x) = 0;
			}
			else if (tmp2 <= 102)
			{
				tmp2 -= 51;
				IMG_B(img_color, y, x) = 255 - tmp2 * 5;
				IMG_G(img_color, y, x) = 255;
				IMG_R(img_color, y, x) = 0;
			}
			else if (tmp2 <= 153)
			{
				tmp2 -= 102;
				IMG_B(img_color, y, x) = 0;
				IMG_G(img_color, y, x) = 255;
				IMG_R(img_color, y, x) = tmp2 * 5;
			}
			else if (tmp2 <= 204)
			{
				tmp2 -= 153;
				IMG_B(img_color, y, x) = 0;
				IMG_G(img_color, y, x) = 255 - uchar(128.0 * tmp2 / 51.0 + 0.5);
				IMG_R(img_color, y, x) = 255;
			}
			else
			{
				tmp2 -= 204;
				IMG_B(img_color, y, x) = 0;
				IMG_G(img_color, y, x) = 127 - uchar(127.0 * tmp2 / 51.0 + 0.5);
				IMG_R(img_color, y, x) = 255;
			}
		}
	}
	return img_color;
}
/*
 * 创建并返回idle image
 */
QImage EvisionUtils::getDefaultImage()
{
	//图片上的字符串，例如 HELLO
	QString imageText = "Evision";
	QFont font;
	
	font.setPixelSize(13);//设置显示字体的大小


	QFontMetrics fm(font);
	int charWidth = fm.width(imageText);
	charWidth = fm.boundingRect(imageText).width();
	
	QSize size(charWidth + 8, 15);//指定图片大小为字体的大小
	//QSize size(1920, 1080);//指定图片大小为字体的大小


	//以ARGB32格式构造一个QImage
	QImage image(size, QImage::Format_ARGB32);
	//填充图片背景,120/250为透明度
	image.fill(qRgba(255, 255, 255, 0));


	//为这个QImage构造一个QPainter
	QPainter painter(&image);
	//设置画刷的组合模式CompositionMode_SourceOut这个模式为目标图像在上。
	//改变组合模式和上面的填充方式可以画出透明的图片。
	painter.setCompositionMode(QPainter::CompositionMode_DestinationOver);


	//改变画笔和字体
	QPen pen = painter.pen();
	pen.setColor(QColor(0, 141, 239));


	painter.setPen(pen);
	painter.setFont(font);


	//将Hello写在Image的中心
	painter.drawText(image.rect(), Qt::AlignCenter, imageText);
	return image;
}

void EvisionUtils::ShowImageOnUi(cv::Mat& img, QGraphicsScene* sense, QGraphicsView* view)
{
	sense->clear();
	sense->addPixmap(QPixmap::fromImage(cvMat2QImage(img)));
	view->setScene(sense);
	view->fitInView(sense->itemsBoundingRect(), Qt::KeepAspectRatio);
	view->centerOn(0, 0);
	view->show();
	view->update();
}

/*
 * 把双目标定得到的所有参数写入文件
 * 1.文件名							std::string filename
 * 2.相机矩阵1(相对焦距和主点坐标)	cv::Mat& cameraMatrix1
 * 3.畸变系数1						cv::Mat& distCoeffs1
 * 4.相机矩阵2						cv::Mat& cameraMatrix2
 * 5.畸变系数2						cv::Mat& distCoeffs2
 * 6.两个相机之间的旋转				cv::Mat& R
 * 7.两个相机之间的平移				cv::Mat& T
 * 8.本质矩阵						cv::Mat& E
 * 9.基本矩阵						cv::Mat& F
 * 10.图片尺寸						cv::Size& imageSize,
 * 11.旋转映射矩阵1                 cv::Mat& R1,
 * 12.投影映射矩阵1                 cv::Mat& P1,
 * 13.旋转映射矩阵2                 cv::Mat& R2,
 * 14.投影映射矩阵2                 cv::Mat& P2,
 * 15.三维映射矩阵                  cv::Mat& Q,
 * 16.矫正后的roi1                  cv::Rect& roi1,
 * 17.矫正后的roi2                  cv::Rect& roi2
 * 返回值:
 *	成功:true,失败和出错:false
 */
bool EvisionUtils::write_AllCameraParams(std::string& filename, cv::Mat& cameraMatrix1, cv::Mat& distCoeffs1,
	cv::Mat& cameraMatrix2, cv::Mat& distCoeffs2, cv::Mat& R, cv::Mat& T, cv::Size& imageSize,
	cv::Mat& R1, cv::Mat& P1, cv::Mat& R2, cv::Mat& P2, cv::Mat& Q, cv::Rect& roi1, cv::Rect& roi2)
{
	if (filename.empty() == true)
	{
		return false;
	}
	try
	{
		cv::FileStorage fs(filename, cv::FileStorage::WRITE);
		if (fs.isOpened())
		{
			fs << "cameraMatrix1" << cameraMatrix1;
			fs << "distCoeffs1" << distCoeffs1;
			fs << "cameraMatrix2" << cameraMatrix2;
			fs << "distCoeffs2" << distCoeffs2;
			fs << "R" << R;
			fs << "T" << T;
			fs << "imageSize" << imageSize;
			fs << "R1" << R1;
			fs << "P1" << P1;
			fs << "R2" << R2;
			fs << "P2" << P2;
			fs << "Q" << Q;
			fs << "roi1" << roi1;
			fs << "roi2" << roi2;
			fs.release();
			return true;
		}
		else
		{
			return false;
		}
	}
	catch (...)
	{
		return false;
	}
}

bool EvisionUtils::write_MonocularCameraParams(std::string& filename, cv::Mat& cameraMatrix1, cv::Mat& distCoeffs1, cv::Size& imageSize)
{
	if (filename.empty() == true)
	{
		return false;
	}
	try
	{
		cv::FileStorage fs(filename, cv::FileStorage::WRITE);
		if (fs.isOpened())
		{
			fs << "cameraMatrix1" << cameraMatrix1;
			fs << "distCoeffs1" << distCoeffs1;
			fs << "imageSize" << imageSize;
			fs.release();
			return true;
		}
		else
		{
			return false;
		}
	}
	catch (...)
	{
		return false;
	}
}


/*
 * 从文件中读取双目标定得到的参数
 * 1.文件名							std::string filename
 * 2.相机矩阵1(相对焦距和主点坐标)	cv::Mat* cameraMatrix1
 * 3.畸变系数1						cv::Mat* distCoeffs1
 * 4.相机矩阵2						cv::Mat* cameraMatrix2
 * 5.畸变系数2						cv::Mat* distCoeffs2
 * 6.两个相机之间的旋转				cv::Mat* R
 * 7.两个相机之间的平移				cv::Mat* T
 * 8.本质矩阵						cv::Mat* E
 * 9.基本矩阵						cv::Mat* F
 * 10.图片尺寸						cv::Size* imageSize,
 * 11.旋转映射矩阵1                 cv::Mat* R1,
 * 12.投影映射矩阵1                 cv::Mat* P1,
 * 13.旋转映射矩阵2                 cv::Mat* R2,
 * 14.投影映射矩阵2                 cv::Mat* P2,
 * 15.三维映射矩阵                  cv::Mat* Q,
 * 16.矫正后的roi1                  cv::Rect* roi1,
 * 17.矫正后的roi2                  cv::Rect* roi2
 * 返回值:
 *	成功:true,失败和出错:false
 */
bool EvisionUtils::read_AllCameraParams(std::string& filename, cv::Mat* cameraMatrix1, cv::Mat* distCoeffs1,
	cv::Mat* cameraMatrix2, cv::Mat* distCoeffs2, cv::Mat* R, cv::Mat* T, cv::Mat* E, cv::Mat* F, cv::Size* imageSize,
	cv::Mat* R1, cv::Mat* P1, cv::Mat* R2, cv::Mat* P2, cv::Mat* Q, cv::Rect* roi1, cv::Rect* roi2)
{
	if (filename.empty() == true)
	{
		return false;
	}
	try
	{
		cv::FileStorage fs(filename, cv::FileStorage::READ);
		if (fs.isOpened())
		{
			fs["cameraMatrix1"] >> *cameraMatrix1;
			fs["distCoeffs1"] >> *distCoeffs1;
			fs["cameraMatrix2"] >> *cameraMatrix2;
			fs["distCoeffs2"] >> *distCoeffs2;
			fs["R"] >> *R;
			fs["T"] >> *T;
			fs["E"] >> *E;
			fs["F"] >> *F;
			fs["imageSize"] >> *imageSize;
			fs["R1"] >> *R1;
			fs["P1"] >> *P1;
			fs["R2"] >> *R2;
			fs["P2"] >> *P2;
			fs["Q"] >> *Q;
			fs["roi1"] >> *roi1;
			fs["roi2"] >> *roi2;
			fs.release();
			return true;
		}
		else
		{
			return false;
		}
	}
	catch (...)
	{
		return false;
	}
}
bool EvisionUtils::read_MonocularParams(std::string& filename, cv::Mat* cameraMatrix1, cv::Mat* distCoeffs1, cv::Size* imageSize)
{
	if (filename.empty() == true)
	{
		return false;
	}
	try
	{
		cv::FileStorage fs(filename, cv::FileStorage::READ);
		if (fs.isOpened())
		{
			fs["cameraMatrix1"] >> *cameraMatrix1;
			fs["distCoeffs1"] >> *distCoeffs1;
			fs["imageSize"] >> *imageSize;
			fs.release();
			return true;
		}
		else
		{
			return false;
		}
	}
	catch (...)
	{
		return false;
	}
}
/*
 * 读取匹配所需的参数
 */
bool EvisionUtils::read_ParamsForStereoMatch(std::string& filename, cv::Mat* cameraMatrix1, cv::Mat* distCoeffs1,
	cv::Mat* cameraMatrix2, cv::Mat* distCoeffs2, cv::Mat* R1, cv::Mat* P1, cv::Mat* R2, cv::Mat* P2, cv::Mat* Q, cv::Rect* roi1,
	cv::Rect* roi2)
{
	if (filename.empty()==true)
	{
		return false;
	}
	try
	{
		cv::FileStorage fs(filename, cv::FileStorage::READ);
		if (fs.isOpened())
		{
			fs["cameraMatrix1"] >> *cameraMatrix1;
			fs["distCoeffs1"] >> *distCoeffs1;
			fs["cameraMatrix2"] >> *cameraMatrix2;
			fs["distCoeffs2"] >> *distCoeffs2;
			fs["R1"] >> *R1;
			fs["P1"] >> *P1;
			fs["R2"] >> *R2;
			fs["P2"] >> *P2;
			fs["Q"] >> *Q;
			fs["roi1"] >> *roi1;
			fs["roi2"] >> *roi2;
			fs.release();
			return true;
		}
		else
		{
			return false;
		}
	}
	catch (...)
	{
		return false;
	}
}

bool EvisionUtils::read_ParamsForStereoRectify(std::string& filename, cv::Mat* cameraMatrix1, cv::Mat* distCoeffs1,
	cv::Mat* cameraMatrix2, cv::Mat* distCoeffs2, cv::Mat* R1, cv::Mat* P1, cv::Mat* R2, cv::Mat* P2, cv::Rect* roi1,
	cv::Rect* roi2)
{
	if (filename.empty() == true)
	{
		return false;
	}
	try
	{
		cv::FileStorage fs(filename, cv::FileStorage::READ);
		if (fs.isOpened())
		{
			fs["cameraMatrix1"] >> *cameraMatrix1;
			fs["distCoeffs1"] >> *distCoeffs1;
			fs["cameraMatrix2"] >> *cameraMatrix2;
			fs["distCoeffs2"] >> *distCoeffs2;
			fs["R1"] >> *R1;
			fs["P1"] >> *P1;
			fs["R2"] >> *R2;
			fs["P2"] >> *P2;
			fs["roi1"] >> *roi1;
			fs["roi2"] >> *roi2;
			fs.release();
			return true;
		}
		else
		{
			return false;
		}
	}
	catch (...)
	{
		return false;
	}
}

void EvisionUtils::getGrayDisparity(const cv::Mat& disp, cv::Mat& grayDisp, bool stretch)
{
	cv::Size imgSize = disp.size();
	cv::Mat output(imgSize, CV_8UC3);
	float min, max;

	if (stretch)
	{
		min = (std::numeric_limits<float>::max());
		max = 0;
		for (size_t h = 0; h < imgSize.height; h++)
		{
			for (size_t w = 0; w < imgSize.width; w++)
			{
				float disparity = disp.at<float>(h, w);

				if (disparity < min && disparity >= 0)
					min = disparity;
				else if (disparity > max)
					max = disparity;
			}
		}
	}

	for (size_t h = 0; h < imgSize.height; h++)
	{
		for (size_t w = 0; w < imgSize.width; w++)
		{
			cv::Vec3b color;
			float disparity = disp.at<float>(h, w);

			if (disparity >= 0)
			{
				if (stretch)
					disparity = (255 / (max - min)) * (disparity - min);

				color[0] = (uchar)disparity;
				color[1] = (uchar)disparity;
				color[2] = (uchar)disparity;

			}
			else
			{
				color[0] = 0;
				color[1] = 0;
				color[2] = 0;
			}

			output.at<cv::Vec3b>(h, w) = color;
		}
	}

	grayDisp = output.clone();
}