根据前面得到的两个径向畸变系数来校正失真图像。
校正函数:
//校正图象
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;
}