给定相机内参与畸变系数将三维点投影到像素点

摘要 ： 本文主要写了，将相机坐标系上的点投影到像素平面上，并去畸变的过程,并附带C# 代码实现。

基础知识部分可以参考这位博主的文章（点击蓝色字体即可跳转）。

当不考虑畸变时， 投影方程如下所示：

当考虑畸变时：

切向畸变可以用下面式子去矫正：

$$x_{corr}=x_{dis}(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_1{x}_{dis}{y}_{dis}+p_2(r^2+2x_{dis}^2)$$
$$y_{corr}=y_{dis}(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+p_1(r^2+2y_{dis}^2)+2p_2{x}_{dis}{y}_{dis}$$

其中，$（x_{corr},y_{corr}）$ 在归一化相机平面上（$z_c=1$），我们需要将其还原到像素坐标，就是去畸变后的像素坐标了。

$$u^{,}=x_{corr}\ast f_x+c_x$$
$$v^{,}=y_{corr}\ast f_y+c_y$$

   public static void Project2(Matrix<double> cameraMatrix, Vector<double> distCoeffs, Point3D point, out Point2D projectedPoint)
        {
            double xp = point.X / point.Z;
            double yp = point.Y / point.Z;

            double fx = cameraMatrix[0, 0];
            double fy = cameraMatrix[1, 1];
            double cx = cameraMatrix[0, 2];
            double cy = cameraMatrix[1, 2];
            double k1 = distCoeffs[0];
            double k2 = distCoeffs[1];
            double k3 = distCoeffs[4];
            double p1 = distCoeffs[2];
            double p2 = distCoeffs[3];

            // compute f(xp, yp)
            double rsquared = xp * xp + yp * yp;
            double g = 1 + k1 * rsquared + k2 * rsquared * rsquared + k3 * rsquared * rsquared * rsquared;
            double xpp = xp * g + 2 * p1 * xp * yp + p2 * (rsquared + 2 * xp * xp);
            double ypp = yp * g + p1 * (rsquared + 2 * yp * yp) + 2* p2 * xp * yp;
            projectedPoint = new Point2D(fx * xpp + cx, fy * ypp + cy);
        }

参考链接：
1、知乎 摄像机模型、坐标系转换及去畸变详解