/* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
// Copied from Cal3DS2 :-(
// but specialized with k1,k2 non-zero only and fx=fy and s=0
const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_);
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol)
break;
const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g;
}
if (iteration >= maxIterations)
throw std::runtime_error(
"Cal3DS2::calibrate fails to converge. need a better initialization");
return pn;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
Matrix Dcal, Dp;
uncalibrate(p, Dcal, Dp);
Matrix H(2, 5);
H << Dp, Dcal;
return H;
}
int
main(int argc, char *argv[], char *env[])
{
char *display_name = NULL;
char *device_name = NULL;
char *output_name = NULL;
XSetWindowAttributes xswa;
int i = 0;
double a, a1, a2, b, b1, b2, xerr, yerr;
int xi_opcode, event, error;
XExtensionVersion *version;
XDeviceInfo *info;
XDevice *device;
long calib_data[4];
unsigned long mask;
unsigned char swap;
int keep_cursor = 0, ch;
/* Crosshair placement */
int cpx[] = { 0, 0, 1, 1, 1 };
int cpy[] = { 0, 1, 0, 0, 1 };
while ((ch = getopt(argc, argv, "cD:d:o:v")) != -1) {
switch (ch) {
case 'c':
keep_cursor++;
break;
case 'D':
display_name = optarg;
break;
case 'd':
device_name = optarg;
break;
case 'o':
output_name = optarg;
break;
case 'v':
verbose = True;
break;
default:
usage();
/* NOTREACHED */
}
}
argc -= optind;
argv += optind;
if (argc != 0)
usage();
/* connect to X server */
if ((display = XOpenDisplay(display_name)) == NULL) {
fprintf(stderr, "%s: cannot connect to X server %s\n",
__progname, XDisplayName(display_name));
exit(1);
}
screen = DefaultScreen(display);
root = RootWindow(display, screen);
/* get screen size from display structure macro */
xpos = 0;
ypos = 0;
width = DisplayWidth(display, screen);
height = DisplayHeight(display, screen);
if (XRRQueryExtension(display, &event, &error)) {
int major, minor;
if (XRRQueryVersion(display, &major, &minor) != True) {
fprintf(stderr, "Error querying XRandR version");
} else {
printf("XRandR extension version %d.%d present\n",
major, minor);
has_xrandr = True;
if (major > 1 || (major == 1 && minor >=2))
has_xrandr_1_2 = True;
if (major > 1 || (major == 1 && minor >=3))
has_xrandr_1_3 = True;
}
}
if (output_name != NULL) {
if (has_xrandr_1_2) {
get_xrandr_config(display, root, output_name,
&xpos, &ypos, &width, &height);
} else {
fprintf(stderr, "%s: can not specify an output "
"whithout XRandr 1.2 or later", __progname);
exit(2);
}
}
if (!XQueryExtension(display, INAME, &xi_opcode,
&event, &error)) {
fprintf(stderr, "%s: X Input extension not available.\n",
__progname);
exit(1);
}
version = XGetExtensionVersion(display, INAME);
if (version == NULL ||
version == (XExtensionVersion *)NoSuchExtension) {
fprintf(stderr, "Cannot query X Input version.\n");
exit(1);
}
XFree(version);
prop_calibration = XInternAtom(display, WS_PROP_CALIBRATION, True);
if (prop_calibration == None) {
fprintf(stderr, "Unable to find the \"%s\" device property.\n"
"There are probably no calibrable devices "
"on this system.\n", WS_PROP_CALIBRATION);
exit(1);
}
prop_swap = XInternAtom(display, WS_PROP_SWAP_AXES, True);
if (prop_swap == None) {
fprintf(stderr, "Unable to find the \"%s\" device property\n",
WS_PROP_SWAP_AXES);
exit(1);
}
info = find_device_info(device_name);
if (info == NULL) {
fprintf(stderr, "Unable to find the %s device\n",
device_name ? device_name : "default");
exit(1);
}
/* setup window attributes */
xswa.override_redirect = True;
xswa.background_pixel = BlackPixel(display, screen);
xswa.event_mask = ExposureMask | KeyPressMask;
mask = CWOverrideRedirect | CWBackPixel | CWEventMask;
if (!keep_cursor) {
xswa.cursor = create_empty_cursor();
mask |= CWCursor;
}
win = XCreateWindow(display, RootWindow(display, screen),
xpos, ypos, width, height, 0,
CopyFromParent, InputOutput, CopyFromParent,
mask, &xswa);
render_init();
XMapWindow(display, win);
XGrabKeyboard(display, win, False, GrabModeAsync, GrabModeAsync,
CurrentTime);
XGrabServer(display);
XClearWindow(display, win);
if (verbose)
printf("Calibrating %s\n", info->name);
device = XOpenDevice(display, info->id);
if (!device) {
fprintf(stderr, "Unable to open the X input device \"%s\"\n",
info->name);
return 0;
}
if (!register_events(info, device, 0))
exit(1);
uncalibrate(device);
calib:
XftDrawRect(draw, &bg, 0, 0, width, height);
for (i = 0; i < 5; i++) {
draw_graphics(cpx[i], cpy[i], i);
XFlush(display);
if (!get_events(i))
break;
XftDrawRect(draw, &bg, 0, 0, width, height);
}
if (interrupted)
cleanup_exit(device);
/* Check if X and Y should be swapped */
if (fabs(x[0] - x[1]) > fabs(y[0] - y[1])) {
calib.swapxy = 1;
for (i = 0; i < 5; i++) {
int t = x[i];
x[i] = y[i];
y[i] = t;
}
}
/* get touch pad resolution to screen resolution ratio */
a1 = (double) (x[4] - x[0]) / (double) (cx[4] - cx[0]);
a2 = (double) (x[3] - x[1]) / (double) (cx[3] - cx[1]);
/* get the minimum pad position on the X-axis */
b1 = x[0] - a1 * cx[0];
b2 = x[1] - a2 * cx[1];
/* use the average ratio and average minimum position */
a = (a1 + a2) / 2.0;
b = (b1 + b2) / 2.0;
xerr = a * width / 2 + b - x[2];
if (fabs(xerr) > fabs(a * width * .01)) {
fprintf(stderr, "Calibration problem: X axis error (%.2f) too high, try again\n",
fabs(xerr));
goto err;
}
calib.minx = (int) (b + 0.5);
calib.maxx = (int) (a * width + b + 0.5);
/* get touch pad resolution to screen resolution ratio */
a1 = (double) (y[4] - y[0]) / (double) (cy[4] - cy[0]);
a2 = (double) (y[3] - y[1]) / (double) (cy[3] - cy[1]);
/* get the minimum pad position on the Y-axis */
b1 = y[0] - a1 * cy[0];
b2 = y[1] - a2 * cy[1];
/* use the average ratio and average minimum position */
a = (a1 + a2) / 2.0;
b = (b1 + b2) / 2.0;
yerr = a * height / 2 + b - y[2];
if (fabs(yerr) > fabs(a * height * 0.01)) {
fprintf(stderr, "Calibration problem: Y axis error (%.2f) too high, try again\n",
fabs(yerr));
goto err;
}
calib.miny = (int) (b + 0.5);
calib.maxy = (int) (a * height + b + 0.5);
XFlush(display);
calib.resx = width;
calib.resy = height;
/* Send new values to the X server */
calib_data[0] = calib.minx;
calib_data[1] = calib.maxx;
calib_data[2] = calib.miny;
calib_data[3] = calib.maxy;
XChangeDeviceProperty(display, device, prop_calibration,
XA_INTEGER, 32, PropModeReplace, (unsigned char *)calib_data, 4);
swap = calib.swapxy;
XChangeDeviceProperty(display, device, prop_swap,
XA_INTEGER, 8, PropModeReplace, (unsigned char *)&swap, 1);
XCloseDevice(display, device);
XCloseDisplay(display);
/* And print them for storage in wsconsctl.conf */
printf("mouse.scale=%d,%d,%d,%d,%d,%d,%d\n",
calib.minx, calib.maxx,
calib.miny, calib.maxy,
calib.swapxy,
calib.resx, calib.resy);
return 0;
err:
draw_text(error_message, &errorColor);
XFlush(display);
sleep(2);
goto calib;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
Matrix Dcal;
uncalibrate(p, Dcal, boost::none);
return Dcal;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
Matrix Dp;
uncalibrate(p, boost::none, Dp);
return Dp;
}
