鱼眼图像转经纬图代码

#include <iostream>
#include <string>
#include <opencv2\opencv.hpp>

using namespace std;
using namespace cv;

const float PI = 3.1415926;

void rectifyMap(Mat &mapImg, const int inWidth, const int inHeight,const float* rot, const int outWidth, const int outHeight, const float FOV, const float radius)
{
	float cx = inWidth/2.0;
	float cy = inHeight/2.0;

	float* pMapData = (float*)mapImg.data;
	for (int j = 0; j < outHeight; j++)
	{
		float theta1 = j*PI / outHeight;
		float sinTheta1 = sin(theta1);
		float z1 = cos(theta1);

		for (int i = 0; i < outWidth; i++)
		{
			float fi1 = 2 * PI - i* 2*PI / outWidth;
			float x1 = sinTheta1*cos(fi1);
			float y1 = sinTheta1*sin(fi1);

			//归一化三维坐标
			float x2 = rot[0] * x1 + rot[1] * y1 + rot[2] * z1;
			float y2 = rot[3] * x1 + rot[4] * y1 + rot[5] * z1;
			float z2 = rot[6] * x1 + rot[7] * y1 + rot[8] * z1;
			float norm = sqrt(x2*x2 + y2*y2 + z2*z2);
			x2 /= norm;
			y2 /= norm;
			z2 /= norm;

			//球面坐标系转换
			float theta2 = acos(z2)*180/PI;
			float fi2 = atan2(y2, x2);

			if (theta2 <= (FOV / 2) && theta2 >= 0)
			{
				//球面到鱼眼
				float radius2 =radius* theta2 / (FOV / 2);
				float u = (radius2*cos(fi2) + cx);
				float v = (radius2*sin(fi2) + cy);
				if (u >= 0 && u < inWidth - 1 && v >= 0 && v < inHeight - 1)
				{
					pMapData[j*outWidth * 2 + 2 * i + 0] = u;
					pMapData[j*outWidth * 2 + 2 * i + 1] = v;
				}
				else
				{
					pMapData[j*outWidth * 2 + 2 * i + 0] = 0;
					pMapData[j*outWidth * 2 + 2 * i + 1] = 0;
				}
			}
			else
			{
				pMapData[j*outWidth * 2 + 2 * i + 0] = 0;
				pMapData[j*outWidth * 2 + 2 * i + 1] = 0;
			}
		}
	}
}

void remap(const cv::Mat& srcImg, cv::Mat& dstImg, const cv::Mat& mapImg, int inHeight, int inWidth, int outHeight, int outWidth)
{
	uchar* pSrcData = (uchar*)srcImg.data;
	uchar* pDstData = (uchar*)dstImg.data;
	float* pMapData = (float*)mapImg.data;

	for (int j = 0; j < outHeight; j++)
	{
		for (int i = 0; i < outWidth; i++)
		{
			int idx = j*outWidth * 2 + i * 2;
			float u = pMapData[idx + 0];
			float v = pMapData[idx + 1];

			int u0 = floor(u);
			int v0 = floor(v);
			float dx = u - u0;
			float dy = v - v0;
			float weight1 = (1 - dx)*(1 - dy);
			float weight2 = dx*(1 - dy);
			float weight3 = (1 - dx)*dy;
			float weight4 = dx*dy;

			if (u0 >= 0 && v0 >= 0 && (u0 + 1) < inWidth && (v0 + 1) < inHeight)
			{
				float B = weight1*pSrcData[v0*inWidth * 3 + u0 * 3 + 0] + weight2*pSrcData[v0*inWidth * 3 + (u0 + 1) * 3 + 0] +
					weight3*pSrcData[(v0 + 1)*inWidth * 3 + u0 * 3 + 0] + weight4*pSrcData[(v0 + 1)*inWidth * 3 + (u0 + 1) * 3 + 0];

				float G = weight1*pSrcData[v0*inWidth * 3 + u0 * 3 + 1] + weight2*pSrcData[v0*inWidth * 3 + (u0 + 1) * 3 + 1] +
					weight3*pSrcData[(v0 + 1)*inWidth * 3 + u0 * 3 + 1] + weight4*pSrcData[(v0 + 1)*inWidth * 3 + (u0 + 1) * 3 + 1];

				float R = weight1*pSrcData[v0*inWidth * 3 + u0 * 3 + 2] + weight2*pSrcData[v0*inWidth * 3 + (u0 + 1) * 3 + 2] +
					weight3*pSrcData[(v0 + 1)*inWidth * 3 + u0 * 3 + 2] + weight4*pSrcData[(v0 + 1)*inWidth * 3 + (u0 + 1) * 3 + 2];

				int idxResult = j*outWidth * 3 + i * 3;
				pDstData[idxResult + 0] = uchar(B);
				pDstData[idxResult + 1] = uchar(G);
				pDstData[idxResult + 2] = uchar(R);
			}
		}
	}
}

void main()
{
	string imgPath = "data/source_images/";
	Mat srcImg = imread(imgPath + "animal.png");
    //输入鱼眼图像尺寸
	int inHeight = srcImg.rows;
	int inWidth = srcImg.cols;
    //输出经纬度图像尺寸
	int outHeight = 1000;
	int outWidth = 2000;
    //视场角
	float FOV = 210;
    //鱼眼半径
	float radius = inWidth / 2.0;

    //以图像中心为赤道
    float rot[9] = { 1,0,0,0,1,0,0,0,1 };
	float angle = PI/2;
	rot[0] = cos(angle);
	rot[2] = sin(angle);
	rot[6] = -sin(angle);
	rot[8] = cos(angle);

    //求映射Map
	cv::Mat mapImg = cv::Mat::zeros(outHeight, outWidth, CV_32FC2);
	rectifyMap(mapImg, inWidth, inHeight,rot, outWidth, outHeight, FOV, radius);
    //remap得到经纬度图像
	Mat dstImg = Mat::zeros(outHeight, outWidth, CV_8UC3);
	remap(srcImg, dstImg, mapImg, inHeight, inWidth, outHeight, outWidth);

	imwrite("dstImg.jpg", dstImg);
}