AVM环视系统：鱼眼相机去畸变算法及实战-电子发烧友网

前言

在近一年的AVM算法开发工作中，鱼眼相机去畸变的玩法前前后后基本过了个遍。从最开始的调用Opencv API，到后来由于算法需要自己实现、正向的undis2fish、反向的fish2undis、鱼眼上检测、undis上标定，总之遇到很多坑，还好都解决了。正好最近有同学在AVM的帖子下面问这个东西的实现，今天在这里讨论一下。本帖从鱼眼相机模型开始讲起，包含Opencv API调参、基于畸变表的参数拟合、鱼眼相机去畸变算法原理和C++实现。

1. 鱼眼相机基础

1.1 鱼眼相机模型

鱼眼相机模型经过P点的入射光线没有透镜的话，本应交于相机成像平面的e点。然而，经过鱼眼相机的折射，光线会交于相机成像平面的d点，就产生了畸变，因此畸变图像整体上呈现出像素朝图像中心点聚集的态势。而去畸变，就是将折射到d点的点，重新映射回到e点，因此去畸变之后的图像与原始的鱼眼图像相比，仿佛是把向心聚集的像素又重新向四周铺展开来。下表中的两幅图分别为鱼眼图和去畸变之后的展开图：

鱼眼相机的投影方式有很多种假设，例如等距投影、等立体角投影、正交投影、体视投影、线性投影。但是真实的鱼眼相机镜头并不完全遵循上述的这些模型假设。因此Kannala-Brandt提出了一种一般形式的估计，适用于不同类型的鱼眼相机：

，这个也是纳入opencv中的鱼眼相机畸变模型。现在基本上默认鱼眼相机模型遵循上述公式。公式中的为光线入射角，r()为上图中 od 的长度。

1.2 基于畸变表的拟合方法

每个相机都有它固定的相机参数，包含内参、畸变系数。可以使用特定的相机标定方法，得到这些相机参数。通常，我们可以使用相机在不同位置、不同角度对着标定板拍摄几十张照片，然后用某种优化方法，计算出相机参数的最优解，例如张正友棋盘格标定法。然而，基于标定板的方法标定出的结果取决于光照是否充足、图像序列是否充足、全部的标定板序列是否能够充满整个图像幅面等因素。也就是说汽车标定的过程中需要人工摆放标定板的位置，指望算法工程师将产线上的工人培训得和他们一样专业显然不现实。大部分标定车间都是车开到标定场中间，十几秒标定出AVM系统所需要的参数，主要是4个鱼眼相机的外参，而非相机本身的参数（内参、畸变系数）。大概几十秒搞定一辆车，不可能有人工标定相机内参的过程。好在相机在出厂时厂家一般都会提供相机的必要参数

内参

{"focal_length": 950,"dx": 3,"dy": 3,"cx": 640,"cy": 480},通过这些参数可以计算出内参矩阵："intrinsic" : [316.66,0.0,640,0.0,316.66,480,0.0,0.0,1.0]内参计算公式：

中间矩阵为内参计算公式

畸变表：

畸变表通过畸变表可以得到畸变前后像素坐标的映射关系：其中angle表示光线的入射角，Real_Image_Height表示入射光线经过鱼眼相机透镜折射（出射角为）与成像平面的交点（畸变点）。opencv Kannala-Brandt模型与畸变表之间的差异为：

opencv Kannala-Brandt相机畸变模型描述的是光线的入射角与其经过折射后在相机归一化平面上的投影点距离归一化平面中心的距离r
畸变表描述的是光线的入射角与其经过折射后在相机的真实的成像平面上的投影点距离成像中心的距离r

两者之间相差一个系数focal_length。即，如果使用畸变表拟合opencv Kannala-Brandt数学公式中的畸变参数，必须已知相机焦距focal_length，注意：这个focal_length是实际的物理概念，正儿八经的相机焦距，而不是相机内参矩阵中的f/dx。寻找与r的关系，是一种曲线拟合的问题。畸变表中提供了数据和 r() ，拟合的多项式为：

具体的实现方法可以使用python的curve_fit函数，即可拟合出合适的k0,k1,k2,k3,k4系数。上面提到opencv Kannala-Brandt与厂家给的畸变表之间相差一个系数：focal_length，因此，在做曲线拟合的时候，要把这部分考虑进去：

theta_input=data[:,0]*3.14/180 theta_fit=np.arctan(self.data[:,1]/0.95)#focal_lenth=0.95 distort_data,_=curve_fit(func1,theta_input,theta_fit) 综上，我们通过曲线拟合的方法得到了畸变参数。

2 Opencv API 鱼眼图像去畸变方法

Opencv提供了基于Kannala-Brandt数学模型的鱼眼去畸变方法： cv::initUndistortRectifyMap，该函数使用相机的内参和畸变参数计算出映射图mapx和mapy。

2.1 基础鱼眼图像去畸变

其中入参K为鱼眼相机内参，D为,,, 畸变参数，R我们一般设置为单位阵，P为去畸变图像的相机内参，size为输出图像的大小；map1，map2为输出的映射图。

@paramKCameraintrinsicmatrixf$cameramatrix{K}f$. @paramDInputvectorofdistortioncoefficientsf$distcoeffsfisheyef$. @paramRRectificationtransformationintheobjectspace:3x31-channel,orvector:3x1/1x3 1-channelor1x13-channel @paramPNewcameraintrinsicmatrix(3x3)ornewprojectionmatrix(3x4) @paramsizeUndistortedimagesize. @paramm1typeTypeofthefirstoutputmapthatcanbeCV_32FC1orCV_16SC2.SeeconvertMaps() fordetails. @parammap1Thefirstoutputmap. @parammap2Thesecondoutputmap. */ CV_EXPORTS_WvoidinitUndistortRectifyMap(InputArrayK,InputArrayD,InputArrayR,InputArrayP, constcv::Size&size,intm1type,OutputArraymap1,OutputArraymap2); 相机内参矩阵表示如下，其中表示相机焦距 f 与相机cmos参数的比值，这个的物理意义为每个像素的实际长度，单位可以是mm/像素。表示相机主点，即光心与图像平面相交的坐标，单位为像素。

那么问题来了，为什么既需要鱼眼相机的内参，又需要输出图像的相机内参呢，它们之间是什么关系呢？最开始的时候，很多同学肯定是把这两个相机内参设置成一样的，即都设置成鱼眼相机的大小，如下图所示。代码中去畸变之后图像的内参是从鱼眼相机内参深拷贝过来的。

cv::MatR=cv::eye(3,3,CV_32F); cv::Matmapx_open,mapy_open; cv::Matintrinsic_undis; fish_intrinsic.copyTo(intrinsic_undis); //intrinsic_undis.at<float>(0,2)*=2; //intrinsic_undis.at<float>(1,2)*=2; cv::initUndistortRectifyMap( fish_intrinsic,m_undis2fish_params,R,intrinsic_undis, cv::Size(intrinsic_undis.at<float>(0,2)*2, intrinsic_undis.at<float>(1,2)*2), CV_32FC1,mapx_open,mapy_open); cv::Mattest; cv::remap(disImg[3],test,mapx_open,mapy_open,cv::INTER_LINEAR);

左侧为鱼眼图，右侧为去畸变图

2.2 相机主点参数调节

我们发现，上图中右侧去畸变之后虽然图像幅面大小与鱼眼图相同都是1280*960，但是可视范围变得很小。标定所需要的大方格没有包含进来。因此，需要进一步调参，下面代码中将去畸变之后图像相机参数中的主点 , 扩大为原来的两倍，且initUndistortRectifyMap函数输出的去畸变图像大小size是与去畸变之后图像相机参数主点相关的，也就是图像大小同样跟着放大了两倍。记住一点：initUndistortRectifyMap函数中的size参数一般都是与去畸变之后图像的相机参数中主点大小强相关的。这一点在后面C++代码手撕算法流程时候会提到。

cv::MatR=cv::eye(3,3,CV_32F); cv::Matmapx_open,mapy_open; cv::Matintrinsic_undis; fish_intrinsic.copyTo(intrinsic_undis); intrinsic_undis.at<float>(0,2)*=2; intrinsic_undis.at<float>(1,2)*=2; cv::initUndistortRectifyMap( fish_intrinsic,m_undis2fish_params,R,intrinsic_undis, cv::Size(intrinsic_undis.at<float>(0,2)*2, intrinsic_undis.at<float>(1,2)*2), CV_32FC1,mapx_open,mapy_open); cv::Mattest; cv::remap(disImg[3],test,mapx_open,mapy_open,cv::INTER_LINEAR);

去畸变图像相机参数的主点扩大了两倍，同时生成图像大小扩到两倍从上图中我们依然不能获得到右侧完整的黑色大方格，因此需要进一步扩大去畸变后图像相机主点位置以及生成图像的分辨率：

cv::MatR=cv::eye(3,3,CV_32F); cv::Matmapx_open,mapy_open; cv::Matintrinsic_undis; fish_intrinsic.copyTo(intrinsic_undis); intrinsic_undis.at<float>(0,2)*=4; intrinsic_undis.at<float>(1,2)*=4; cv::initUndistortRectifyMap( fish_intrinsic,m_undis2fish_params,R,intrinsic_undis, cv::Size(intrinsic_undis.at<float>(0,2)*2, intrinsic_undis.at<float>(1,2)*2), CV_32FC1,mapx_open,mapy_open); cv::Mattest; cv::remap(disImg[3],test,mapx_open,mapy_open,cv::INTER_LINEAR);

现在我已经把去畸变图像相机内参的主点扩大为fish相机内参的4倍了，生成图像的长宽也放大了4倍，像素数量总体放大16倍，这样才勉强把大方格完全显示出来。我们知道提取角点需要用到图像处理算法，显然对这么大的图像做处理的效率非常低。

2.3 相机f参数调节

到目前位置，我们只讨论了相机参数中主点的调参，想要解决上述问题还需要调整相机的 ,先不说理论，直接看调参结果，这里我们代码中只调整了去畸变图像相机参数中的,使它们缩小为原来的1/4。

cv::MatR=cv::eye(3,3,CV_32F); cv::Matmapx_open,mapy_open; cv::Matintrinsic_undis; fish_intrinsic.copyTo(intrinsic_undis); intrinsic_undis.at<float>(0,0)/=4; intrinsic_undis.at<float>(1,1)/=4; /*intrinsic_undis.at<float>(0,2)*=4; intrinsic_undis.at<float>(1,2)*=4;*/ cv::initUndistortRectifyMap( fish_intrinsic,m_undis2fish_params,R,intrinsic_undis, cv::Size(intrinsic_undis.at<float>(0,2)*2, intrinsic_undis.at<float>(1,2)*2), CV_32FC1,mapx_open,mapy_open); cv::Mattest; cv::remap(disImg[3],test,mapx_open,mapy_open,cv::INTER_LINEAR);

左侧为鱼眼图，右侧为去畸变图，分辨率均为1280*960从图中可以看出，当我们仅将相机焦距缩小时，可以看到更多的东西。虽然去畸变之后的图像很小只有1280*960，但是却可以看到完整的方格。本节我们讨论了opencv API initUndistortRectifyMap函数的主点和f参数调节对于去畸变图像的影响，接下来的第3节，我们将会从去畸变算法原理入手，C++实现一波该算法。做这件事的原因很简单：opencv只提供了整张图像从undis2fish的映射，在avm的视角转换中，我们需要进行单个像素点的undis2fish，因此，我们需要自己实现一波这个去畸变过程。结论：缩小相机焦距可以使FOV增大，在更小分辨率的图像上呈现出更多的内容，看上去也是更加清晰。

3 鱼眼去畸变算法及其实现

畸变映射关系鱼眼去畸变的算法实现就是遍历去畸变图像上的每一个点，寻找它们在鱼眼图像上的像素点坐标，计算它们之间的映射关系C++实现：

/* func:warpfromdistorttoundistort @paramf_dx:f/dx @paramf_dy:f/dy @paramlarge_center_h:undisimagecentery @paramlarge_center_w:undisimagecenterx @paramfish_center_h:fishimagecentery @paramfish_center_w:fishimagecenterx @paramundis_param:factoryparam @paramx:inputcoordinatexontheundisimage @paramy:inputcoordinateyontheundisimage */ cv::Vec2fwarpUndist2Fisheye(floatfish_scale,floatf_dx,floatf_dy,floatlarge_center_h, floatlarge_center_w,floatfish_center_h, floatfish_center_w,cv::Vec4dundis_param,floatx, floaty){ f_dx*=fish_scale; f_dy*=fish_scale; floaty_=(y-large_center_h)/f_dy;//normalizedplane floatx_=(x-large_center_w)/f_dx; floatr_=static_cast<float>(sqrt(pow(x_,2)+pow(y_,2))); //Lookuptable /*intnum=atan(r_)/atan(m_d)*1024; floatangle_distorted=m_Lut[num];*/ floatangle_undistorted=atan(r_);//theta floatangle_undistorted_p2=angle_undistorted*angle_undistorted; floatangle_undistorted_p3=angle_undistorted_p2*angle_undistorted; floatangle_undistorted_p5=angle_undistorted_p2*angle_undistorted_p3; floatangle_undistorted_p7=angle_undistorted_p2*angle_undistorted_p5; floatangle_undistorted_p9=angle_undistorted_p2*angle_undistorted_p7; floatangle_distorted=static_cast<float>(angle_undistorted+ undis_param[0]*angle_undistorted_p3+ undis_param[1]*angle_undistorted_p5+ undis_param[2]*angle_undistorted_p7+ undis_param[3]*angle_undistorted_p9); //scale floatscale=angle_distorted/(r_+0.00001f);//scale=r_disonthecameraimgplane //divider_undisonthenormalizedplane cv::Vec2fwarp_xy; floatxx=(x-large_center_w)/fish_scale; floatyy=(y-large_center_h)/fish_scale; warpPointOpencv(warp_xy,fish_center_h,fish_center_w,xx,yy,scale); returnwarp_xy; } voidwarpPointOpencv(cv::Vec2f&warp_xy,floatmap_center_h,floatmap_center_w, floatx_,floaty_,floatscale){ warp_xy[0]=x_*scale+map_center_w; warp_xy[1]=y_*scale+map_center_h; } 针对上述代码，我们由浅入深地讲述算法流程

3.1 基础的鱼眼去畸变（主点相关）

鱼眼相机成像模型上述代码中令fish_scale为1，先讨论最简单的，即让去畸变图像相机参数中的, 大小与鱼眼图相同，对照鱼眼相机模型这张图和代码，我们来梳理一下算法流程：算法流程

首先，对于图像平面上的像素点，要用相机的内参f、dx、dy，将其转化到归一化平面，对应上图中的e点。并计算其距离归一化平面中心的距离r_。并计算对应的入射角，即上图中的 theta角
根据Kannala-Brandt的鱼眼模型公式，使用事先拟合的k1，k2，k3，k4参数计算归一化平面上去畸变之后点的位置r_distorted
在归一化平面上计算去畸变前后点位置的比值：r_distorted/r_
3中计算的比值为归一化平面上，同样可以应用到相机成像平面以及图像平面上。因此，可以对图像平面上的像素点，乘上这个系数，就得到了鱼眼图上像素点的位置。

对于算法流程中的3，4：

总体来讲这个基础的鱼眼去畸变算法的实现思路就是：在归一化平面上计算去畸变前后的像素坐标scale，然后运用到图像平面上。

主点位置示意图如上图所示，输出的去畸变图大小为Size，去畸变图相机内参中的主点位置为 ,在算法的实现中，首先会创建Size大小的mesh_grid，即map，确定主点的位置，然后根据mesh_grid上每个点的坐标，计算其与主点的相对位置，然后进行后续的计算（转换到归一化平面、计算scale等），因此根据这个逻辑如果上面的主点不在Size的中心，就会导致相机实际拍摄到内容的中心在主点区域，但是右下角会有很多的延申。例如下图这种：

从上图原始的鱼眼图中可以看出相机拍摄的内容中心大概在棋盘格附近，然而去畸变了之后棋盘格却跑到了左上角。这就是因为我们设置的主点偏左上，没有位于生成的去畸变图的中心。这就是2.2节中提到的：initUndistortRectifyMap函数中的size参数一般都是与去畸变之后图像的相机参数中主点大小强相关的。

3.2 进阶的鱼眼去畸变（如何调整f）

正如第2节所说，我们需要在很小的图像上呈现出大方格。这就需要调整f，这个过程不太容易理解，我们画个图来理解一下：

相机焦距调整示意图上图中相机的真实焦距为f，假设cmos长度不变，我们只是把成像平面放在了 f/2 的位置上，显然调整焦距后的相机FOV更大，能够看到的东西越多。同理，对于标定车间中的大方格，假设我们调参使得 , 缩小一定的倍数，理论上就可以看到更多的内容。

将相机内参 f 缩小为 f/2 意味着我们将相机的cmos推导距离相机光心 f/2 处，在这个平面上做映射。算法流程如下：

将相机焦距调整为 f/2 后，使用新的焦距将点转换到归一化平面上去，得到
使用去畸变参数 ,,, 计算其畸变状态下在归一化平面上的位置
使用前两步的结果，计算去畸变前后线段的长度比例scale
根据已知的与前面计算的scale计算出
*2将点映射到 f 平面上，就得到了在 f/2 成像平面上的去畸变映射关系。

上述最后一步不太容易理解：实际真实的畸变图像是在 f 平面上，也就是说我们算出了 f/2 平面上畸变图上点映射到去畸变后的位置还不够，还需要进一步找到真实的畸变图上的坐标，也就是 f 平面上的位置，因为我们最后都是要去真正的畸变图上找点的， f/2 平面只是我们虚构的，只是假设cmos在 f/2 平面上。这个图中我们最后一步×2，假设我们想把相机内参设为 f/3 ，那最后一步要×3。总之一句话：f平面才是我们真实拿到的fish图，我们最终还是要在这个原始的fish图上找点。在实验中我们得到结论：

当我们调整 f 使其更小时，相同的内容集中于更小的分辨率上，对于后续的图像处理算法会更友好。很多锯齿和模糊的问题都能得到改善
在鱼眼上检测棋盘格角点要比在去畸变图上检测棋盘格角点更加准确，因为去畸变后大方格被拉伸的很严重。这个结论的依据是重投影误差以及将鱼眼检测到的角点坐标映射到去畸变图上后与直接在畸变图上的位置做视觉上的比较。

3.3 Opencv API undistortPoints的实现

前面所有讨论的都是undis2fish的过程。在实际的AVM标定中，通常是对鱼眼相机检测角点，因为去畸变之后图像拉伸效果严重，提取的角点不准确。参考张正友标定法标定相机参数时，也是在获取到的图像上直接提取角点，解一个全局优化问题。因此，除了前面讲到的undis2fish映射过程以外，我们还需要实现fish2undis的过程。这个过程Opencv提供了函数undistortPoints，即输入为鱼眼相机上点的坐标，输出为去畸变图像上点的坐标。这个过程是一个解

方程的问题，用到非线性优化，速度很慢。因此我们通过畸变表，构建了一个多项式，通过反向拟合的方法，提前拟合出fish2undis的方程系数：

#forward self.distor_para,_=curve_fit(self.func,self.data[:,0],self.data[:,1]) #inverse f_inverse_para,_=curve_fit(self.func_inverse,self.data[:,1],self.data[:,0]) 计算fish2undis的过程与undis2fish（3.1，3.2）的过程略有不同，但都是寻找 与 之间的映射关系，因为 f 平面才是我们真实拿到的fish图，我们最终还是要在这个原始的fish图上找点。

实现代码：

cv::Vec2fCalibrateInit::warpFisheye2Undist(floatfish_scale,floatf_dx,floatf_dy,floatundis_center_h, floatundis_center_w,floatfish_center_h, floatfish_center_w,cv::Vec4dundis_param,floatx, floaty){ //f_dx*=fish_scale; //f_dy*=fish_scale; floaty_=(y-fish_center_h)/f_dy;//normalizedplane floatx_=(x-fish_center_w)/f_dx; floatr_distorted=static_cast<float>(sqrt(pow(x_,2)+pow(y_,2))); floatr_distorted_p2=r_distorted*r_distorted; floatr_distorted_p3=r_distorted_p2*r_distorted; floatr_distorted_p4=r_distorted_p2*r_distorted_p2; floatr_distorted_p5=r_distorted_p2*r_distorted_p3; floatangle_undistorted=static_cast<float>(r_distorted+ undis_param[0]*r_distorted_p2+ undis_param[1]*r_distorted_p3+ undis_param[2]*r_distorted_p4+ undis_param[3]*r_distorted_p5); //scale floatr_undistorted=tanf(angle_undistorted); floatscale=r_undistorted/(r_distorted+0.00001f);//scale=r_disonthecameraimgplane //divider_undisonthenormalizedplane cv::Vec2fwarp_xy; floatxx=(x-fish_center_w)*fish_scale; floatyy=(y-fish_center_h)*fish_scale; warpPointInverse(warp_xy,undis_center_h,undis_center_w,xx,yy,scale); returnwarp_xy; } voidCalibrateInit::Vec2f&warp_xy,floatmap_center_h,floatmap_center_w, floatx_,floaty_,floatscale){ warp_xy[0]=x_*scale+map_center_w; warp_xy[1]=y_*scale+map_center_h; }