自动驾驶AVM环视算法--鱼眼相机的畸变矫正原理

测试工程和视频：

链接：https://pan.baidu.com/s/11GNLuIxcONGCeobp0MbXFQ?pwd=0z6l 提取码：0z6l

1、平面相机的成像和坐标系如下所示

说明

1、f（ud，vd）就是以图像中心为原点坐标(和p(x，y)坐标相对，就是坐表原点不同)。

2、p（x，y）就是在图像坐标系下的坐标点，坐标点的为图像的左上角点，这个和世界图像的保存数据的坐标一直。

3、假设xy坐标系的原点是uv坐标系的中点，相对的偏移量为cx和cy，就可以得出ud=x+cx,vd=y+cy。

4、P(Xc,Yc,Zc)就是相机坐标系下的坐标。

根据相似的三角形可以得出

AB/COuv=AOc/OcOuv=PB/pC=Xc/ud=Zc/f=Yc/vd

得出

Xc/ud=Zc/fx

Yc/vd=Zc/fy

得出

ud=fx(Xc/Zc)

vd=fy(Yc/Zc)

得出

x=fx(Xc/Zc)-cx

y=fy(Yc/Zc)-cy

2、鱼眼相机的成像坐标变换

参考：https://ww2.mathworks.cn/help/vision/ug/fisheye-calibration-basics.html

由上述的公式可以得出

上述公式中的ud和vd就是平面图像的坐标点，u和v 就是鱼眼图像坐标

根据上述的的计算公式就可以实现图像的畸变矫正

测试代码如下所示

void CjsAVMtoolDlg::OnBnClickedButton1()
{
	// TODO: 在此添加控件通知处理程序代码
		//MFC弹出命令窗体
	AllocConsole();
	freopen("CONOUT$", "w", stdout);
	//MFC弹出命令窗体
	// TODO: 在此添加控件通知处理程序代码
	float  a[4] = { 160,-0.0005,0,0 };
	int cx = 640, cy = 360;//图像的中心点
	float fx = 100, fy = 100;//虚拟焦距
	float p = 0.0;
	Mat src = imread("test.jpg");
	Mat dst(src.rows, src.cols, CV_8UC3, Scalar(0, 0, 0));
	if (src.empty()) {
		printf("不能加载图像！\n");
	}
	namedWindow("原图", WINDOW_AUTOSIZE);
	imshow("原图", src);
 
	//像素读取方式：直接读取
	int height = src.rows;
	int width = src.cols;
	int ch = src.channels();
	js_Transplane2Fisheye_V1(&dst, src, a, fx, fy, cx, cy);
	//js_Transplane2Fisheye_V2(&dst, src, a, fx, fy, cx, cy);
 
 
 
	imshow("直接读取像素图", dst);
}
 
int js_Transplane2Fisheye_V1(Mat *Dst,Mat Src,float *a,float fx,float fy,int cx,int cy)
{
	for (int row = 0; row < Src.rows ; row++) {//h
		for (int col = 0; col < Src.cols; col++) {//w
 
			float x1 = ((float)(col - cx));
			float y1 = ((float)(row - cy));
			float p = sqrtf(x1 * x1 + y1 * y1);
			float x2 = x1 / (a[0] + a[1] * p * p + a[2] * p * p * p + a[3] * p * p * p * p);
			float y2 = y1 / (a[0] + a[1] * p * p + a[2] * p * p * p + a[3] * p * p * p * p);
			int ud = fx * x2 + cx + 0.5;
			int vd = fy * y2 + cy + 0.5;
			if (((ud >= 0) && (ud < Src.cols)) && ((vd >= 0) && (vd < Src.rows)))
			{
				Dst->at<Vec3b>(vd, ud) = Src.at<Vec3b>(row, col);
			}
		}
	}
	return 0;
}

测试图片和测试结果（测试结果图像存在无效的计算点）

输入图像