相机世界坐标点加畸变投影到带畸变的鱼眼图上

// https://zhuanlan.zhihu.com/p/511284263
// https://github.com/WordZzzz/fisheye_calibration
void distortFishEye3dTo2d(Point3d& src3d, Point2d& dst2d, const cv::Mat& intrinsic, const cv::Mat& distortion, double apha=0){
    const double fx=intrinsic.at<double>(0,0),fy=intrinsic.at<double>(1,1);
    const double cx=intrinsic.at<double>(0,2),cy=intrinsic.at<double>(1,2);
    const double k1=distortion.at<double>(0,0);
    const double k2=distortion.at<double>(0,1);
    const double k3=distortion.at<double>(0,2);
    const double k4=distortion.at<double>(0,3);

    if(src3d.z<=0)
    {
        dst2d.x=-1;
        dst2d.y=-1;
        return;
    }

    //转归一化平面
    double a = src3d.x / src3d.z;
    double b = src3d.y / src3d.z;
    double r=a*a+b*b;
    r = sqrt(r);
    double theta = atan(r);

    double theta_d=1*theta+k1*pow(theta,3)+k2*pow(theta,5)+k3*pow(theta,7)+k4*pow(theta,9);

    double inv_r =1.0/r;
    double cdist = theta_d * inv_r;

    double xd=a*cdist;
    double yd=b*cdist;

    double u = fx*(xd + apha * yd)+cx;
    double v = fy*yd+cy;
    dst2d.x=u;
    dst2d.y=v;

}