鱼眼相机模型-MEI

参考文献： Single View Point Omnidirectional Camera Calibration from Planar Grids

1. 相机模型如下：

   // 相机坐标系下的点投影到畸变图像// 输入：相机坐标系点坐标cam  输出： 畸变图像素点坐标disPtvoid FisheyeCamAdapter::cam2DistImg(cv::Point3f &cam, cv::Point2f &disPt){double r = cv::norm(cam);double dx_u = 0, dy_u = 0;if (r != 0){cv::Point3f ps = cam / r;double x = ps.x / (ps.z + camInt.fXi);double y = ps.y / (ps.z + camInt.fXi);distortion(x, y, &dx_u, &dy_u);x += dx_u;y += dy_u;disPt.x = x * camInt.fGammaX + camInt.fCx;disPt.y = y * camInt.fGammaY + camInt.fCy;}else{disPt.x = camInt.fCx;disPt.y = camInt.fCy;}}// 无畸变图像到畸变图像void FisheyeCamAdapter::distortion(double mx_u, double my_u, double *dx_u, double *dy_u){double mx2_u = 0., my2_u = 0., mxy_u = 0., rho2_u = 0., rad_dist_u = 0.;double k1 = camInt.distortCoeff[0];double k2 = camInt.distortCoeff[1];double p1 = camInt.distortCoeff[2];double p2 = camInt.distortCoeff[3];double k5 = camInt.distortCoeff[4];mx2_u = mx_u * mx_u;my2_u = my_u * my_u;mxy_u = mx_u * my_u;rho2_u = mx2_u + my2_u;rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u + k5 * rho2_u * rho2_u * rho2_u;*dx_u = mx_u * rad_dist_u + 2 * p1 * mxy_u + p2 * (rho2_u + 2 * mx2_u);*dy_u = my_u * rad_dist_u + 2 * p2 * mxy_u + p1 * (rho2_u + 2 * my2_u);}

   反投影过程：畸变图中的像素坐标计算相机坐标系下坐标

    cv::Point3f FisheyeCamAdapter::pointDis2Camera(const cv::Point2f &disPoint){double mx_d, my_d, mx_u, my_u;double lambda;double xi = camInt.fXi;// Lift points to normalised planefloat inv_K11 = 1 / camInt.fGammaX;float inv_K13 = -camInt.fCx / camInt.fGammaX;float inv_K22 = 1 / camInt.fGammaY;float inv_K23 = -camInt.fCy / camInt.fGammaY;mx_d = inv_K11 * (disPoint.x) + inv_K13;my_d = inv_K22 * (disPoint.y) + inv_K23;undistortGN(mx_d, my_d, &mx_u, &my_u, 100); // 去畸变坐标// Lift normalised points to the sphere (inv_hslash)cv::Point3f camera;if (xi == 1){lambda = 2 / (mx_u * mx_u + my_u * my_u + 1);camera.x = lambda * mx_u;camera.y = lambda * my_u;camera.z = lambda - 1;}else{double sqrt_i = 1.0 + (1.0 - xi * xi) * (mx_u * mx_u + my_u * my_u);if (sqrt_i < 0){camera.x = -1000000;camera.y = -1000000;camera.z = 1;}else{lambda = (xi + sqrt(sqrt_i)) / (1.0 + mx_u * mx_u + my_u * my_u);camera.x = lambda * mx_u;camera.y = lambda * my_u;camera.z = lambda - xi;}}return camera;}