畸变校正 on undistortion

畸变校正是一种映射，将畸变的像素投影到校正的像素位置上。

计算机视觉中的映射有两种模式，一种是正向投影src->dst，一种是反向投影dst->src。

一般反向投影比较容易，正向投影比较麻烦。这是因为src的位置已知，我们计算出来的dst的位置，可能是浮点数，这时候给dst图像的一个浮点数的位置的像素值赋值，比较麻烦。而反向投影，我们从各个已知的dst的位置出发，去src中寻找对应的像素点，这时候虽然是浮点数，但是我们可以通过插值的方式，确定一个合适的像素值赋值给dst。

畸变校正的问题中，反向投影也是比较容易的，而正向比较难。

先来看看反向投影的做法，顺便看看畸变是如何发生的：

这是从校正后的图像（dst）反向投影到畸变图像（src）的过程。(u, v)是dst上的点，先归一化，然后旋转到畸变相机的世界，一般R是单位矩阵，即不发生旋转。然后公式5和公式6就是畸变的过程，最后乘以相机的内参矩阵，得到畸变之后的点的位置map_x，map_y。运算比较简单。

正向投影的话，我们要知道src上的点在dst上的位置。这一般校正几个特征点的时候会用到。思路就是反向投影的过程从最后一步到第一步。

我们首先将src上的点归一化。然后我们要做公式5和公式6的逆向计算，已知x'', y''，求x', y'。整个正向的过程是这里比较不好做，计算量比较大。

这是一个解二元高次方程的问题。我目前也不会。不过OpenCV的代码里面有解法，大概是说：where undistort() is an approximate iterative algorithm that estimates the normalized original point coordinates out of the normalized distorted point coordinates (“normalized” means that the coordinates do not depend on the camera matrix).http://docs.opencv.org/2.4/modules/imgproc/doc/geometric_transformations.html#undistortpoints

OpenCV代码写的比较复杂，考虑的边界条件很多。不过读起来也不难。在这里：https://github.com/opencv/opencv/blob/0624411875fd69c009ce76311630f5933484f7bf/modules/imgproc/src/undistort.cpp#L297

不过我又找到了更简明的版本，更好理解吧，道理是一样的，而且这个版本应该就是从OpenCV这里挖过来简化的：https://github.com/MRPT/mrpt/blob/master/libs/vision/src/pinhole.cpp#L203

    double x = inPt.x;
    double y = inPt.y;

    double x0 = x = (x - cx) * ifx;
    double y0 = y = (y - cy) * ify;

    // compensate distortion iteratively
    for (unsigned int j = 0; j < 5; j++)
    {
        double r2 = x * x + y * y;
        double icdist =
            1. / (1 +
                  ((cameraModel.dist[4] * r2 + cameraModel.dist[1]) * r2 +
                   cameraModel.dist[0]) *
                      r2);
        double deltaX = 2 * cameraModel.dist[2] * x * y +
                        cameraModel.dist[3] * (r2 + 2 * x * x);
        double deltaY = cameraModel.dist[2] * (r2 + 2 * y * y) +
                        2 * cameraModel.dist[3] * x * y;
        x = (x0 - deltaX) * icdist;
        y = (y0 - deltaY) * icdist;
    }

    // Save undistorted pixel coords:
    outPt.x = x * fx + cx;
    outPt.y = y * fy + cy;

算法描述：
初始化：将校正之后的点(x', y')假设为src的归一化点(x'', y'')

1. 通过(x', y')计算畸变因子distFactor。
2. 通过(x'', y'')和distFactor，计算新的校正点，赋值给(x', y')。
3. 通过(x', y')计算其畸变之后的点与(x'', y'')之间的error。
4. 如果未达到预先设定的迭代次数，或者error未达到阈值，回到1。