电阻屏校正 tslib 五点校正算法

#include   <stdio.h>

struct calibration {
    int x[5];
    int y[5];
    int xfb[5];
    int yfb[5];
    int a[5];
};
struct calibration cal;
int perform_calibration(struct calibration *cal) {
    int j;
    float n, x, y, x2, y2, xy, z, zx, zy;
    float det, a, b, c, e, f, i;
    float scaling = 65536.0;
// Get sums for matrix
    n = x = y = x2 = y2 = xy = 0;
    for(j = 0; j < 5; j++) {
        n += 1.0;
        x += (float)cal->x[j];
        y += (float)cal->y[j];
        x2 += (float)(cal->x[j]*cal->x[j]);
        y2 += (float)(cal->y[j]*cal->y[j]);
        xy += (float)(cal->x[j]*cal->y[j]);
    }
// Get determinant of matrix -- check if determinant is too small
    det = n*(x2*y2 - xy*xy) + x*(xy*y - x*y2) + y*(x*xy - y*x2);
    if(det < 0.1 && det > -0.1) {
        printf("ts_calibrate: determinant is too small -- %f\n",det);
        return 0;
    }
// Get elements of inverse matrix
    a = (x2*y2 - xy*xy)/det;
    b = (xy*y - x*y2)/det;
    c = (x*xy - y*x2)/det;
    e = (n*y2 - y*y)/det;
    f = (x*y - n*xy)/det;
    i = (n*x2 - x*x)/det;
// Get sums for x calibration
    z = zx = zy = 0;
    for(j=0;j<5;j++) {
        z += (float)cal->xfb[j];
        zx += (float)(cal->xfb[j]*cal->x[j]);
        zy += (float)(cal->xfb[j]*cal->y[j]);
    }
// Now multiply out to get the calibration for framebuffer x coord
    cal->a[0] = (int)((a*z + b*zx + c*zy)*(scaling));
    cal->a[1] = (int)((b*z + e*zx + f*zy)*(scaling));
    cal->a[2] = (int)((c*z + f*zx + i*zy)*(scaling));
    printf("%f %f %f\n",(a*z + b*zx + c*zy),
                (b*z + e*zx + f*zy),
                (c*z + f*zx + i*zy));
// Get sums for y calibration
    z = zx = zy = 0;
    for(j=0;j<5;j++) {
        z += (float)cal->yfb[j];
        zx += (float)(cal->yfb[j]*cal->x[j]);
        zy += (float)(cal->yfb[j]*cal->y[j]);
    }
// Now multiply out to get the calibration for framebuffer y coord
    cal->a[3] = (int)((a*z + b*zx + c*zy)*(scaling));
    cal->a[4] = (int)((b*z + e*zx + f*zy)*(scaling));
    cal->a[5] = (int)((c*z + f*zx + i*zy)*(scaling));
    printf("%f %f %f\n",(a*z + b*zx + c*zy),
                (b*z + e*zx + f*zy),
                (c*z + f*zx + i*zy));
// If we got here, we're OK, so assign scaling to a[6] and return
    cal->a[6] = (int)scaling;
    printf("%d %d %d %d %d %d %d\n",cal->a[0],cal->a[1],cal->a[2],cal->a[3],
cal->a[4],cal->a[5],(int)cal->a[6]);
    // 0 1 2 3 4 5 6
    // 1 2 0 4 5 3 6
    /* 验证校验 x=100 y=150 */
    printf("x %d\n", (cal->a[0] + cal->a[1]*100 + cal->a[2]*150 ) / cal->a[6]);
    printf("y %d\n", (cal->a[3] + cal->a[4]*100 + cal->a[5]*150 ) / cal->a[6]);
    return 1;
}
int main(int argc, char *argv[])
{
    /* 校准时点击的坐标 */
    cal.x[0] = 100;
    cal.y[0] = 100;
    cal.x[1] = 1600 - 100;
    cal.y[1] = 50;
    cal.x[2] = 1600 - 100;
    cal.y[2] = 960 - 100;
    cal.x[3] = 100;
    cal.y[3] = 960 - 100;
    cal.x[4] = 800;
    cal.y[4] = 480;
    /* 逻辑坐标 */
    cal.xfb[0] = 50;
    cal.yfb[0] = 50;
    cal.xfb[1] = 800 - 50;
    cal.yfb[1] = 50;
    cal.xfb[2] = 800 - 50;
    cal.yfb[2] = 480 - 50;
    cal.xfb[3] = 50;
    cal.yfb[3] = 480 - 50;
    cal.xfb[4] = 400;
    cal.yfb[4] = 240;
    perform_calibration(&cal);
    return 0;
}