void
camera_control_read_calibration(CameraControl* cc,
char* intrinsicsFile, char* distortionFile)
{
CvMat *intrinsic = (CvMat*) cvLoad(intrinsicsFile, 0, 0, 0);
CvMat *distortion = (CvMat*) cvLoad(distortionFile, 0, 0, 0);
if (cc->mapx) {
cvReleaseImage(&cc->mapx);
}
if (cc->mapy) {
cvReleaseImage(&cc->mapy);
}
if (intrinsic && distortion) {
if (!cc->frame3chUndistort) {
cc->frame3chUndistort = cvCloneImage(
camera_control_query_frame(cc));
}
int width, height;
get_metrics(&width, &height);
cc->mapx = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
cc->mapy = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
cvInitUndistortMap(intrinsic, distortion, cc->mapx, cc->mapy);
// TODO: Shouldn't we free intrinsic and distortion here?
} else {
fprintf(stderr, "Warning: No lens calibration files found.\n");
}
}
void
camera_control_read_calibration(CameraControl* cc,
char* intrinsicsFile, char* distortionFile)
{
CvMat *intrinsic = (CvMat*) cvLoad(intrinsicsFile, 0, 0, 0);
CvMat *distortion = (CvMat*) cvLoad(distortionFile, 0, 0, 0);
if (cc->mapx) {
cvReleaseImage(&cc->mapx);
}
if (cc->mapy) {
cvReleaseImage(&cc->mapy);
}
if (intrinsic && distortion) {
if (!cc->frame3chUndistort) {
enum PSMove_Bool new_frame;
cc->frame3chUndistort = cvCloneImage(
camera_control_query_frame(cc, NULL, NULL, &new_frame));
}
int width, height;
get_metrics(&width, &height);
cc->mapx = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
cc->mapy = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
cvInitUndistortMap(intrinsic, distortion, cc->mapx, cc->mapy);
cc->focl_x = CV_MAT_ELEM(*intrinsic, float, 0, 0);
cc->focl_y = CV_MAT_ELEM(*intrinsic, float, 1, 1);
// TODO: Shouldn't we free intrinsic and distortion here?
} else {
void cameraCalibration() {
board_w = 5; // Board width in squares
board_h = 8; // Board height
n_boards = 8; // Number of boards
int board_n = board_w * board_h;
CvSize board_sz = cvSize(board_w, board_h);
CameraControl* cc = camera_control_new(0);
//cvNamedWindow("Calibration", 0);
// Allocate Sotrage
CvMat* image_points = cvCreateMat(n_boards * board_n, 2, CV_32FC1);
CvMat* object_points = cvCreateMat(n_boards * board_n, 3, CV_32FC1);
CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);
CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);
CvMat* distortion_coeffs = cvCreateMat(5, 1, CV_32FC1);
IplImage *image;
CvPoint2D32f corners[board_n];
int i = 0;
int j = 0;
for (i = 0; i < board_n; i++)
corners[i] = cvPoint2D32f(0, 0);
int corner_count;
int successes = 0;
int step = 0;
while (1) {
cvWaitKey(10);
image = camera_control_query_frame(cc);
if (image)
break;
}
IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1);
// Capture Corner views loop until we've got n_boards
// succesful captures (all corners on the board are found)
while (successes < n_boards) {
// Skp every board_dt frames to allow user to move chessboard
// skip a second to allow user to move the chessboard
image = camera_control_query_frame(cc); // Get next image
//if (frame++ % board_dt == 0)
{
// Find chessboard corners:
int found = cvFindChessboardCorners(image, board_sz, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
cvWaitKey(1);
// Get subpixel accuracy on those corners
cvCvtColor(image, gray_image, CV_BGR2GRAY);
cvFindCornerSubPix(gray_image, corners, corner_count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
// Draw it
cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
char text[222];
sprintf(text, "calibration image %d/%d", successes, n_boards);
th_put_text(image, text, cvPoint(20, 20), th_white, 1.0);
cvShowImage("Calibration", image);
// If we got a good board, add it to our data
if (corner_count == board_n ) {
step = successes * board_n;
for (i = step, j = 0; j < board_n; ++i, ++j) {
CV_MAT_ELEM( *image_points, float, i, 0 ) = corners[j].x;
CV_MAT_ELEM( *image_points, float, i, 1 ) = corners[j].y;
CV_MAT_ELEM( *object_points, float, i, 0 ) = j / board_w;
CV_MAT_ELEM( *object_points, float, i, 1 ) = j % board_w;
CV_MAT_ELEM( *object_points, float, i, 2 ) = 0.0f;
}
CV_MAT_ELEM( *point_counts, int, successes, 0 ) = board_n;
successes++;
}
}
}
