/*
* calibrate delay values: must only be called *after* interrupts are allowed
*
* NOTE1: we use TimerD so we restore the RS232 stuff
* NOTE2: some systems (e.g. Aranym) do not implement TimerD; we leave
* the default delay values as-is in this case
*/
void calibrate_delay(void)
{
#if CONF_WITH_MFP
ULONG loopcount, intcount;
/*
* disable interrupts then run the calibration
*/
jdisint(TIMERD_INTNUM);
loopcount = CALIBRATION_TIME * loopcount_1_msec;
intcount = run_calibration(loopcount);
/*
* disable interrupts then restore the RS232
* serial port stuff (in case we're using it)
*/
jdisint(TIMERD_INTNUM);
rsconf1(B9600, 0, 0x88, 1, 1, 0); /* just like init_serport() */
/*
* intcount is the number of interrupts that occur during 'loopcount'
* loops. an interrupt occurs every 1/960 sec (see delayasm.S).
* so the number of loops per second = loopcount/(intcount/960).
* so, loops per millisecond = (loopcount*960)/(intcount*1000)
* = (loopcount*24)/(intcount*25).
*/
if (intcount) /* check for valid */
loopcount_1_msec = (loopcount * 24) / (intcount * 25);
#endif
}
static void
calibrate_button_clicked_cb (GtkButton *button,
CcWacomPage *page)
{
int i, calibration[4];
GVariant *variant;
int *current;
gsize ncal;
gint monitor;
monitor = csd_wacom_device_get_display_monitor (page->priv->stylus);
if (monitor < 0) {
/* The display the tablet should be mapped to could not be located.
* This shouldn't happen if the EDID data is good...
*/
g_critical("Output associated with the tablet is not connected. Unable to calibrate.");
return;
}
variant = g_settings_get_value (page->priv->wacom_settings, "area");
current = (int *) g_variant_get_fixed_array (variant, &ncal, sizeof (gint32));
if (ncal != 4) {
g_warning("Device calibration property has wrong length. Got %"G_GSIZE_FORMAT" items; expected %d.\n", ncal, 4);
g_free (current);
return;
}
for (i = 0; i < 4; i++)
calibration[i] = current[i];
if (calibration[0] == -1 &&
calibration[1] == -1 &&
calibration[2] == -1 &&
calibration[3] == -1) {
gint *device_cal;
device_cal = csd_wacom_device_get_area (page->priv->stylus);
for (i = 0; i < 4 && device_cal; i++) {
calibration[i] = device_cal[i];
}
g_free (device_cal);
}
run_calibration (page, calibration, monitor);
gtk_widget_set_sensitive (GTK_WIDGET (button), FALSE);
}
void loop(void)
{
int16_t user_input;
hal.console->println();
hal.console->println_P(PSTR(
"Menu:\r\n"
" c) calibrate accelerometers\r\n"
" d) display offsets and scaling\r\n"
" l) level (capture offsets from level)\r\n"
" t) test\r\n"
" r) reboot"));
// wait for user input
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
// read in user input
while( hal.console->available() ) {
user_input = hal.console->read();
if( user_input == 'c' || user_input == 'C' ) {
run_calibration();
display_offsets_and_scaling();
}
if( user_input == 'd' || user_input == 'D' ) {
display_offsets_and_scaling();
}
if( user_input == 't' || user_input == 'T' ) {
run_test();
}
if( user_input == 'r' || user_input == 'R' ) {
hal.scheduler->reboot(false);
}
}
}
int main( int argc, char** argv )
{
CvSize board_size = {0,0};
float square_size = 1.f, aspect_ratio = 1.f;
const char* out_filename = "out_camera_data.yml";
const char* input_filename = 0;
int i, image_count = 10;
int write_extrinsics = 0, write_points = 0;
int flags = 0;
CvCapture* capture = 0;
FILE* f = 0;
char imagename[1024];
CvMemStorage* storage;
CvSeq* image_points_seq = 0;
int elem_size, flip_vertical = 0;
int delay = 1000;
clock_t prev_timestamp = 0;
CvPoint2D32f* image_points_buf = 0;
CvFont font = cvFont( 1, 1 );
double _camera[9], _dist_coeffs[4];
CvMat camera = cvMat( 3, 3, CV_64F, _camera );
CvMat dist_coeffs = cvMat( 1, 4, CV_64F, _dist_coeffs );
CvMat *extr_params = 0, *reproj_errs = 0;
double avg_reproj_err = 0;
int mode = DETECTION;
int undistort_image = 0;
CvSize img_size = {0,0};
const char* live_capture_help =
"When the live video from camera is used as input, the following hot-keys may be used:\n"
" <ESC>, 'q' - quit the program\n"
" 'g' - start capturing images\n"
" 'u' - switch undistortion on/off\n";
if( argc < 2 )
{
// calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [<list_of_views.txt>]
printf( "This is a camera calibration sample.\n"
"Usage: calibration\n"
" -w <board_width> # the number of inner corners per one of board dimension\n"
" -h <board_height> # the number of inner corners per another board dimension\n"
" [-n <number_of_frames>] # the number of frames to use for calibration\n"
" # (if not specified, it will be set to the number\n"
" # of board views actually available)\n"
" [-di <disk_images> # Number of disk images before triggering undistortion\n"
" [-d <delay>] # a minimum delay in ms between subsequent attempts to capture a next view\n"
" # (used only for video capturing)\n"
" [-s <square_size>] # square size in some user-defined units (1 by default)\n"
" [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n"
" [-op] # write detected feature points\n"
" [-oe] # write extrinsic parameters\n"
" [-zt] # assume zero tangential distortion\n"
" [-a <aspect_ratio>] # fix aspect ratio (fx/fy)\n"
" [-p] # fix the principal point at the center\n"
" [-v] # flip the captured images around the horizontal axis\n"
" [input_data] # input data, one of the following:\n"
" # - text file with a list of the images of the board\n"
" # - name of video file with a video of the board\n"
" # if input_data not specified, a live view from the camera is used\n"
"\n" );
printf( "%s", live_capture_help );
return 0;
}
for( i = 1; i < argc; i++ )
{
const char* s = argv[i];
if( strcmp( s, "-w" ) == 0 )
{
if( sscanf( argv[++i], "%u", &board_size.width ) != 1 || board_size.width <= 0 )
return fprintf( stderr, "Invalid board width\n" ), -1;
}
else if( strcmp( s, "-h" ) == 0 )
{
if( sscanf( argv[++i], "%u", &board_size.height ) != 1 || board_size.height <= 0 )
return fprintf( stderr, "Invalid board height\n" ), -1;
}
else if( strcmp( s, "-s" ) == 0 )
{
if( sscanf( argv[++i], "%f", &square_size ) != 1 || square_size <= 0 )
return fprintf( stderr, "Invalid board square width\n" ), -1;
}
else if( strcmp( s, "-n" ) == 0 )
{
if( sscanf( argv[++i], "%u", &image_count ) != 1 || image_count <= 3 )
return printf("Invalid number of images\n" ), -1;
}
else if( strcmp( s, "-di") == 0)
{
if( sscanf( argv[++i], "%d", &images_from_file) != 1 || images_from_file < 3)
return printf("Invalid di, must be >= 3\n"), -1;
}
else if( strcmp( s, "-a" ) == 0 )
{
if( sscanf( argv[++i], "%f", &aspect_ratio ) != 1 || aspect_ratio <= 0 )
return printf("Invalid aspect ratio\n" ), -1;
}
else if( strcmp( s, "-d" ) == 0 )
{
if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 )
return printf("Invalid delay\n" ), -1;
}
else if( strcmp( s, "-op" ) == 0 )
{
write_points = 1;
}
else if( strcmp( s, "-oe" ) == 0 )
{
write_extrinsics = 1;
}
else if( strcmp( s, "-zt" ) == 0 )
{
flags |= CV_CALIB_ZERO_TANGENT_DIST;
}
else if( strcmp( s, "-p" ) == 0 )
{
flags |= CV_CALIB_FIX_PRINCIPAL_POINT;
}
else if( strcmp( s, "-v" ) == 0 )
{
flip_vertical = 1;
}
else if( strcmp( s, "-o" ) == 0 )
{
out_filename = argv[++i];
}
else if( s[0] != '-' )
input_filename = s;
else
return fprintf( stderr, "Unknown option %s", s ), -1;
}
if( input_filename )
{
capture = cvCreateFileCapture( input_filename );
if( !capture )
{
f = fopen( input_filename, "rt" );
if( !f )
return fprintf( stderr, "The input file could not be opened\n" ), -1;
image_count = -1;
}
mode = CAPTURING;
}
else
capture = cvCreateCameraCapture(0);
if( !capture && !f )
return fprintf( stderr, "Could not initialize video capture\n" ), -2;
if( capture )
printf( "%s", live_capture_help );
elem_size = board_size.width*board_size.height*sizeof(image_points_buf[0]);
storage = cvCreateMemStorage( MAX( elem_size*4, 1 << 16 ));
image_points_buf = (CvPoint2D32f*)cvAlloc( elem_size );
image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
cvNamedWindow( "Image View", 1 );
cvNamedWindow( "Undistort",1);
int disk_image_cnt = 0;
for(;;)
{
IplImage *view = 0, *view_gray = 0;
int count = 0, found, blink = 0;
CvPoint text_origin;
CvSize text_size = {0,0};
int base_line = 0;
char s[100];
int key;
if( f && fgets( imagename, sizeof(imagename)-2, f ))
{
int l = strlen(imagename);
if( l > 0 && imagename[l-1] == '\n' )
imagename[--l] = '\0';
if( l > 0 )
{
if( imagename[0] == '#' )
continue;
view = cvLoadImage( imagename, 1 );
disk_image_cnt++;
}
}
else if( capture )
{
IplImage* view0 = cvQueryFrame( capture );
if( view0 )
{
view = cvCreateImage( cvGetSize(view0), IPL_DEPTH_8U, view0->nChannels );
if( view0->origin == IPL_ORIGIN_BL )
cvFlip( view0, view, 0 );
else
cvCopy( view0, view );
}
}
if( !view || (disk_image_cnt == images_from_file))
{
if( image_points_seq->total > 0 )
{
image_count = image_points_seq->total;
goto calibrate;
}
break;
}
if( flip_vertical )
cvFlip( view, view, 0 );
img_size = cvGetSize(view);
found = cvFindChessboardCorners( view, board_size,
image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH );
#if 1
// improve the found corners' coordinate accuracy
view_gray = cvCreateImage( cvGetSize(view), 8, 1 );
cvCvtColor( view, view_gray, CV_BGR2GRAY );
cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11),
cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
cvReleaseImage( &view_gray );
#endif
if( mode == CAPTURING && found && (f || clock() - prev_timestamp > delay*1e-3*CLOCKS_PER_SEC) )
{
cvSeqPush( image_points_seq, image_points_buf );
prev_timestamp = clock();
blink = !f;
#if 1
if( capture )
{
sprintf( imagename, "view%03d.png", image_points_seq->total - 1 );
cvSaveImage( imagename, view );
}
#endif
}
cvDrawChessboardCorners( view, board_size, image_points_buf, count, found );
cvGetTextSize( "100/100", &font, &text_size, &base_line );
text_origin.x = view->width - text_size.width - 10;
text_origin.y = view->height - base_line - 10;
if( mode == CAPTURING )
{
if( image_count > 0 )
sprintf( s, "%d/%d", image_points_seq ? image_points_seq->total : 0, image_count );
else
sprintf( s, "%d/?", image_points_seq ? image_points_seq->total : 0 );
}
else if( mode == CALIBRATED )
sprintf( s, "Calibrated" );
else
sprintf( s, "Press 'g' to start" );
cvPutText( view, s, text_origin, &font, mode != CALIBRATED ?
CV_RGB(255,0,0) : CV_RGB(0,255,0));
if( blink )
cvNot( view, view );
//Rectify or Undistort the image
if( mode == CALIBRATED && undistort_image )
{
IplImage* t = cvCloneImage( view );
cvShowImage("Image View", view);
cvUndistort2( t, view, &camera, &dist_coeffs );
cvReleaseImage( &t );
cvShowImage( "Undistort", view );
cvWaitKey(0);
}
else{
cvShowImage( "Image View", view );
key = cvWaitKey(capture ? 50 : 500);
}
if( key == 27 )
break;
if( key == 'u' && mode == CALIBRATED ){
undistort_image = !undistort_image;
}
if( capture && key == 'g' )
{
mode = CAPTURING;
cvClearMemStorage( storage );
image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
}
if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count )
{
calibrate:
if(disk_image_cnt == images_from_file)
undistort_image = !undistort_image;
cvReleaseMat( &extr_params );
cvReleaseMat( &reproj_errs );
int code = run_calibration( image_points_seq, img_size, board_size,
square_size, aspect_ratio, flags, &camera, &dist_coeffs, &extr_params,
&reproj_errs, &avg_reproj_err );
// save camera parameters in any case, to catch Inf's/NaN's
save_camera_params( out_filename, image_count, img_size,
board_size, square_size, aspect_ratio, flags,
&camera, &dist_coeffs, write_extrinsics ? extr_params : 0,
write_points ? image_points_seq : 0, reproj_errs, avg_reproj_err );
if( code )
mode = CALIBRATED;
else
mode = DETECTION;
}
if( !view )
break;
cvReleaseImage( &view );
}
if( capture )
cvReleaseCapture( &capture );
return 0;
}
