鱼眼相机去畸变

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

关键词：鱼眼相机、AVM全景环视系统

鱼眼相机基础

鱼眼相机模型

经过P点的入射光线没有透镜的话，本应交于相机成像平面的e点。然而，经过鱼眼相机的折射，光线会交于相机成像平面的d点，就产生了畸变，因此畸变图像整体上呈现出像素朝图像中心点聚集的态势。

而去畸变，就是将折射到d点的点，重新映射回到e点，因此去畸变之后的图像与原始的鱼眼图像相比，仿佛是把向心聚集的像素又重新向四周铺展开来。下表中的两幅图分别为鱼眼图和去畸变之后的展开图：

基于畸变表的拟合方法

每个相机都有它固定的相机参数，包含内参、畸变系数。可以使用特定的相机标定方法，得到这些相机参数。通常，我们可以使用相机在不同位置、不同角度对着标定板拍摄几十张照片，然后用某种优化方法，计算出相机参数的最优解，例如张正友棋盘格标定法。

然而，基于标定板的方法标定出的结果取决于光照是否充足、图像序列是否充足、全部的标定板序列是否能够充满整个图像幅面等因素。也就是说汽车标定的过程中需要人工摆放标定板的位置，指望算法工程师将产线上的工人培训得和他们一样专业显然不现实。大部分标定车间都是车开到标定场中间，十几秒标定出AVM系统所需要的参数，主要是4个鱼眼相机的外参，而非相机本身的参数（内参、畸变系数）。大概几十秒搞定一辆车，不可能有人工标定相机内参的过程。

好在相机在出厂时厂家一般都会提供相机的必要参数

• 内参

登录后复制

{
 "focal_length": 950,
 "dx": 3,
 "dy": 3,
 "cx": 640,
 "cy": 480
 },

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.

通过这些参数可以计算出内参矩阵：

登录后复制

"intrinsic" : [
 316.66,
 0.0,
 640,
 0.0,
 316.66,
 480,
 0.0,
 0.0,
 1.0
 ]

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.

内参计算公式：

 

• 畸变表：

 

登录后复制

# load the distortion table and fit the distortion params
  self.data = self.load_distortion_table(self.distortion_table_path)
  self.data[:, 0] = self.data[:, 0] / 180 * np.pi
  self.data[:, 1] = self.data[:, 1] / 0.91 #focal length = 0.91
  self.distor_para, _ = curve_fit(self.func, self.data[:, 0], self.data[:, 1])

• 1.
• 2.
• 3.
• 4.
• 5.

综上，我们通过曲线拟合的方法得到了畸变参数。

Opencv API 鱼眼图像去畸变方法

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

基础鱼眼图像去畸变

登录后复制

@param K Camera intrinsic matrix \f$cameramatrix{K}\f$.
    @param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$.
    @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3
    1-channel or 1x1 3-channel
    @param P New camera intrinsic matrix (3x3) or new projection matrix (3x4)
    @param size Undistorted image size.
    @param m1type Type of the first output map that can be CV_32FC1 or CV_16SC2 . See convertMaps()
    for details.
    @param map1 The first output map.
    @param map2 The second output map.
     */
    CV_EXPORTS_W void initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P,
        const cv::Size& size, int m1type, OutputArray map1, OutputArray map2);

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.

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

登录后复制

cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
cv::Mat mapx_open, mapy_open;
cv::Mat intrinsic_undis;
fish_intrinsic.copyTo(intrinsic_undis);
//intrinsic_undis.at<float>(0,2) *= 2;
//intrinsic_undis.at<float>(1,2) *= 2;
cv::fisheye::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::Mat test;
cv::remap(disImg[3], test, mapx_open, mapy_open, cv::INTER_LINEAR);

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.

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

相机主点参数调节

登录后复制

cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
 cv::Mat mapx_open, mapy_open;
 cv::Mat intrinsic_undis;
 fish_intrinsic.copyTo(intrinsic_undis);
 intrinsic_undis.at<float>(0,2) *= 2;
 intrinsic_undis.at<float>(1,2) *= 2;
 cv::fisheye::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::Mat test;
 cv::remap(disImg[3], test, mapx_open, mapy_open, cv::INTER_LINEAR);

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.

 

去畸变图像相机参数的主点扩大了两倍，同时生成图像大小扩到两倍

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

登录后复制

cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
cv::Mat mapx_open, mapy_open;
cv::Mat intrinsic_undis;
fish_intrinsic.copyTo(intrinsic_undis);
intrinsic_undis.at<float>(0,2) *= 4;
intrinsic_undis.at<float>(1,2) *= 4;
cv::fisheye::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::Mat test;
cv::remap(disImg[3], test, mapx_open, mapy_open, cv::INTER_LINEAR);

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.

 

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

相机f参数调节

登录后复制

cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
 cv::Mat mapx_open, mapy_open;
 cv::Mat intrinsic_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::fisheye::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::Mat test;
 cv::remap(disImg[3], test, mapx_open, mapy_open, cv::INTER_LINEAR);

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.
• 14.
• 15.

 

左侧为鱼眼图，右侧为去畸变图，分辨率均为1280*960

从图中可以看出，当我们仅将相机焦距缩小时，可以看到更多的东西。虽然去畸变之后的图像很小只有1280*960，但是却可以看到完整的方格。

本节我们讨论了opencv API initUndistortRectifyMap函数的主点和f参数调节对于去畸变图像的影响，接下来的第3节，我们将会从去畸变算法原理入手，C++实现一波该算法。做这件事的原因很简单：opencv只提供了整张图像从undis2fish的映射，在avm的视角转换中，我们需要进行单个像素点的undis2fish，因此，我们需要自己实现一波这个去畸变过程。

结论：缩小相机焦距可以使FOV增大，在更小分辨率的图像上呈现出更多的内容，看上去也是更加清晰。

鱼眼去畸变算法及其实现

畸变映射关系

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

C++实现：

登录后复制

/*
func: warp from distort to undistort

@param   f_dx:f/dx
@param   f_dy:f/dy
@param   large_center_h:    undis image center y
@param   large_center_w:    undis image center x
@param   fish_center_h:     fish image center y
@param   fish_center_w:     fish image center x
@param   undis_param:       factory param
@param   x:                 input coordinate x on the undis image
@param   y:                 input coordinate y on the undis image
*/
cv::Vec2f warpUndist2Fisheye(float fish_scale, float f_dx, float f_dy, float large_center_h,
    float large_center_w, float fish_center_h,
    float fish_center_w, cv::Vec4d undis_param, float x,
                         float y) {
  f_dx *= fish_scale;
  f_dy *= fish_scale;                        
  float y_ = (y - large_center_h) / f_dy;  // normalized plane
  float x_ = (x - large_center_w) / f_dx;
  float r_ = static_cast<float>(sqrt(pow(x_, 2) + pow(y_, 2)));

  // Look up table
  /*int num = atan(r_) / atan(m_d) * 1024;
  float angle_distorted = m_Lut[num];*/

  float angle_undistorted = atan(r_);  // theta
  float angle_undistorted_p2 = angle_undistorted * angle_undistorted;
  float angle_undistorted_p3 = angle_undistorted_p2 * angle_undistorted;
  float angle_undistorted_p5 = angle_undistorted_p2 * angle_undistorted_p3;
  float angle_undistorted_p7 = angle_undistorted_p2 * angle_undistorted_p5;
  float angle_undistorted_p9 = angle_undistorted_p2 * angle_undistorted_p7;

  float angle_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
  float scale = angle_distorted / (r_ + 0.00001f);  // scale = r_dis on the camera img plane
                                       // divide r_undis on the normalized plane
  cv::Vec2f warp_xy;

  float xx = (x - large_center_w) / fish_scale;
  float yy = (y - large_center_h) / fish_scale;

  warpPointOpencv(warp_xy, fish_center_h, fish_center_w, xx, yy, scale);

  return warp_xy;
}

void warpPointOpencv(cv::Vec2f &warp_xy, float map_center_h, float map_center_w,
                     float x_, float y_, float scale) {
  warp_xy[0] = x_ * scale + map_center_w;
  warp_xy[1] = y_ * scale + map_center_h;
}

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.
• 14.
• 15.
• 16.
• 17.
• 18.
• 19.
• 20.
• 21.
• 22.
• 23.
• 24.
• 25.
• 26.
• 27.
• 28.
• 29.
• 30.
• 31.
• 32.
• 33.
• 34.
• 35.
• 36.
• 37.
• 38.
• 39.
• 40.
• 41.
• 42.
• 43.
• 44.
• 45.
• 46.
• 47.
• 48.
• 49.
• 50.
• 51.
• 52.
• 53.
• 54.
• 55.
• 56.
• 57.

针对上述代码，我们由浅入深地讲述算法流程

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

算法流程

1. 首先，对于图像平面上的像素点，要用相机的内参f、dx、dy，将其转化到归一化平面，对应上图中的e点。并计算其距离归一化平面中心的距离r_。并计算对应的入射角，即上图中的 theta角

2. 根据Kannala-Brandt的鱼眼模型公式，使用事先拟合的k1，k2，k3，k4参数计算归一化平面上去畸变之后点的位置r_distorted

3. 在归一化平面上计算去畸变前后点位置的比值： r_distorted/r_

4. 3中计算的比值为归一化平面上，同样可以应用到相机成像平面以及图像平面上。因此，可以对图像平面上的像素点，乘上这个系数，就得到了鱼眼图上像素点的位置。

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

 

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

 

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

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

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

 

Opencv API undistortPoints的实现

前面所有讨论的都是undis2fish的过程。在实际的AVM标定中，通常是对鱼眼相机检测角点，因为去畸变之后图像拉伸效果严重，提取的角点不准确。参考张正友标定法标定相机参数时，也是在获取到的图像上直接提取角点，解一个全局优化问题。

登录后复制

#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])

• 1.
• 2.
• 3.
• 4.

实现代码：

登录后复制

cv::Vec2f CalibrateInit::warpFisheye2Undist(float fish_scale, float f_dx, float f_dy, float undis_center_h,
    float undis_center_w, float fish_center_h,
    float fish_center_w, cv::Vec4d undis_param, float x,
    float y) {
    // f_dx *= fish_scale;
    // f_dy *= fish_scale;                        
    float y_ = (y - fish_center_h) / f_dy;  // normalized plane
    float x_ = (x - fish_center_w) / f_dx;
    float r_distorted = static_cast<float>(sqrt(pow(x_, 2) + pow(y_, 2)));

    float r_distorted_p2 = r_distorted * r_distorted;
    float r_distorted_p3 = r_distorted_p2 * r_distorted;
    float r_distorted_p4 = r_distorted_p2 * r_distorted_p2;
    float r_distorted_p5 = r_distorted_p2 * r_distorted_p3;
    float angle_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
    float r_undistorted = tanf(angle_undistorted);

    float scale = r_undistorted / (r_distorted + 0.00001f);  // scale = r_dis on the camera img plane
                                         // divide r_undis on the normalized plane
    cv::Vec2f warp_xy;

    float xx = (x - fish_center_w) * fish_scale;
    float yy = (y - fish_center_h) * fish_scale;

    warpPointInverse(warp_xy, undis_center_h, undis_center_w, xx, yy, scale);

    return warp_xy;
}

void CalibrateInit::warpPointInverse(cv::Vec2f& warp_xy, float map_center_h, float map_center_w,
    float x_, float y_, float scale) {
    warp_xy[0] = x_ * scale + map_center_w;
    warp_xy[1] = y_ * scale + map_center_h;
}

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.
• 14.
• 15.
• 16.
• 17.
• 18.
• 19.
• 20.
• 21.
• 22.
• 23.
• 24.
• 25.
• 26.
• 27.
• 28.
• 29.
• 30.
• 31.
• 32.
• 33.
• 34.
• 35.
• 36.
• 37.
• 38.
• 39.

总结

本贴讨论的内容为鱼眼相机图像基于畸变表的处理方法，AVM中畸变的运用非常灵活，所以笔者必须对它进行实现才可以灵活运用。据笔者所知有些AVM供应商的鱼眼畸变参数并不一定是依赖畸变表，有的也会拿来一批摄像头自行标定。具体那种方法更优，可能需要更多同行同学的实验和讨论得到结论。

原创作者: whaosoft 转载于: https://blog.51cto.com/whaosoft/11683515