由两个径向畸变系数生成不失真图像

根据前面得到的两个径向畸变系数来校正失真图像。

校正函数：

//校正图象
void UndistortImage(const std::string &in, //原始畸变图文件名
                    const camera_params_t &camera,//图像的焦距，和两个径向畸变系数
                    const std::string &out)//校正后的无畸变图
{ 
    printf("校正图象 %s\n", in.c_str());

    img_t *img = LoadJPEG(in.c_str());//取得原始图
    int w = img->w;
    int h = img->h;
    
    img_t *img_out = img_new(w, h);//建一个空图
   
    double f2_inv = 1.0 / (camera.f * camera.f);//焦距用于生成畸变校正公式中的 r

    for (int y = 0; y < h; y++) {//对新图（无畸变）中的每一点计算原图（畸变）中的坐标
        for (int x = 0; x < w; x++) {
            double x_c = x - 0.5 * w;//坐标变换（以畸变中心为原点）
            double y_c = y - 0.5 * h;
            
            double r2 = (x_c * x_c + y_c * y_c) * f2_inv;//畸变校正公式中的 r
            double factor = 1.0 + camera.k[0] * r2 + camera.k[1] * r2 * r2;//畸变因子
            
            x_c *= factor;//校正前坐标（畸变坐标）
            y_c *= factor;

            x_c += 0.5 * w;//坐标变换（还原）
            y_c += 0.5 * h;
            
            fcolor_t c;
            if (x_c >= 0 && x_c < w - 1 && y_c >= 0 && y_c < h - 1) {
		/*在给定图像中，使用线性插值计算点的颜色( x,y )*/
                c = pixel_lerp(img, x_c, y_c);
            } else {
                c = fcolor_new(0.0, 0.0, 0.0);//超出原图为黑色
            }
            
            img_set_pixel(img_out, x, y, 
                          iround(c.r), iround(c.g), iround(c.b));//写入新图
        }
    }

    WriteJPEG(img_out, (char *) out.c_str());//保存校正后图

    img_free(img);
    img_free(img_out);
}

线性插值函数：

/* 使用线性插值计算点( x，y )的颜色
 * 在给定的图像中 */
fcolor_t pixel_lerp(img_t *img, double x, double y) {
    int xf = (int) floor(x), yf = (int) floor(y);//下取整
    double xp = x - xf, yp = y - yf;//超出差值


    color_t pixels[4];
    fcolor_t col;
    double rd, gd, bd;

    //取得周围四个点的颜色
    pixels[0] = img_get_pixel(img, xf, yf);
    pixels[1] = img_get_pixel(img, xf + 1, yf);
    pixels[2] = img_get_pixel(img, xf, yf + 1);
    pixels[3] = img_get_pixel(img, xf + 1, yf + 1);

//计算公式
#define LERP(x0, x1, f0, f1, f2, f3) ((1.0 - (x1)) * ((1.0 - (x0)) * (f0) + (x0) * (f1)) + (x1) * ((1.0 - (x0)) * (f2) + (x0) * (f3)))

    //红绿蓝分别计算
    rd = LERP(xp, yp, pixels[0].r, pixels[1].r, pixels[2].r, pixels[3].r); 
    gd = LERP(xp, yp, pixels[0].g, pixels[1].g, pixels[2].g, pixels[3].g); 
    bd = LERP(xp, yp, pixels[0].b, pixels[1].b, pixels[2].b, pixels[3].b); 

    col.r = (float) rd;
    col.g = (float) gd;
    col.b = (float) bd;


    return col;
}