鱼眼相机模型-MEI

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

1. 相机模型如下：

   // 相机坐标系下的点投影到畸变图像
   // 输入：相机坐标系点坐标cam  输出： 畸变图像素点坐标disPt
   void 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 plane
        float 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;
    }