C++
1#pragma warning(disable:4996)2 3#include <opencv2\opencv.hpp>4#include <opencv/cv.h>5#include <opencv/highgui.h>6 7#define IMAGE_NUM (25) /* 画像数 */8#define PAT_ROW (7) /* パターンの行数 */9#define PAT_COL (10) /* パターンの列数 */10#define PAT_SIZE (PAT_ROW*PAT_COL)11#define ALL_POINTS (IMAGE_NUM*PAT_SIZE)12#define CHESS_SIZE (20.0) /* パターン1マスの1辺サイズ[mm] */13 14int15main(int argc, char *argv[])16{17 int i, j, k;18 int corner_count, found;19 int p_count[IMAGE_NUM];20 IplImage *src_img[IMAGE_NUM];21 IplImage *before_img, *after_img;22 CvSize pattern_size = cvSize(PAT_COL, PAT_ROW);23 CvPoint3D32f objects[ALL_POINTS];24 CvPoint2D32f *corners = (CvPoint2D32f *)cvAlloc(sizeof(CvPoint2D32f) * ALL_POINTS);25 CvMat object_points;26 CvMat image_points;27 CvMat point_counts;28 CvMat *intrinsic = cvCreateMat(3, 3, CV_32FC1);29 CvMat *rotation = cvCreateMat(1, 3, CV_32FC1);30 CvMat *translation = cvCreateMat(1, 3, CV_32FC1);31 CvMat *distortion = cvCreateMat(1, 4, CV_32FC1);32 33 // (1)キャリブレーション画像の読み込み34 for (i = 0; i < IMAGE_NUM; i++) {35 char buf[32];36 sprintf(buf, "calib_image/CheckerBoard%02d.jpg", i);37 if ((src_img[i] = cvLoadImage(buf, CV_LOAD_IMAGE_COLOR)) == NULL) {38 fprintf(stderr, "cannot load image file : %s\n", buf);39 }40 }41 42 // (2)3次元空間座標の設定43 for (i = 0; i < IMAGE_NUM; i++) {44 for (j = 0; j < PAT_ROW; j++) {45 for (k = 0; k < PAT_COL; k++) {46 objects[i * PAT_SIZE + j * PAT_COL + k].x = j * CHESS_SIZE;47 objects[i * PAT_SIZE + j * PAT_COL + k].y = k * CHESS_SIZE;48 objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0;49 }50 }51 }52 cvInitMatHeader(&object_points, ALL_POINTS, 3, CV_32FC1, objects);53 54 // (3)チェスボード（キャリブレーションパターン）のコーナー検出55 int found_num = 0;56 cvNamedWindow("Calibration", CV_WINDOW_AUTOSIZE);57 for (i = 0; i < IMAGE_NUM; i++) {58 found = cvFindChessboardCorners(src_img[i], pattern_size, &corners[i * PAT_SIZE], &corner_count);59 fprintf(stderr, "%02d...", i);60 if (found) {61 fprintf(stderr, "ok\n");62 found_num++;63 }64 else {65 fprintf(stderr, "fail\n");66 }67 // (4)コーナー位置をサブピクセル精度に修正，描画68 IplImage *src_gray = cvCreateImage(cvGetSize(src_img[i]), IPL_DEPTH_8U, 1);69 cvCvtColor(src_img[i], src_gray, CV_BGR2GRAY);70 cvFindCornerSubPix(src_gray, &corners[i * PAT_SIZE], corner_count,71 cvSize(3, 3), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));72 cvDrawChessboardCorners(src_img[i], pattern_size, &corners[i * PAT_SIZE], corner_count, found);73 p_count[i] = corner_count;74 cvShowImage("Calibration", src_img[i]);75 cvWaitKey(0);76 }77 cvDestroyWindow("Calibration");78 79 if (found_num != IMAGE_NUM)80 return -1;81 cvInitMatHeader(&image_points, ALL_POINTS, 1, CV_32FC2, corners);82 cvInitMatHeader(&point_counts, IMAGE_NUM, 1, CV_32SC1, p_count);83 84 // (5)内部パラメータ，歪み係数の推定85 cvCalibrateCamera2(&object_points, &image_points, &point_counts, cvSize(640, 480), intrinsic, distortion);86 87 // (6)外部パラメータの推定88 CvMat sub_image_points, sub_object_points;89 int base = 0;90 cvGetRows(&image_points, &sub_image_points, base * PAT_SIZE, (base + 1) * PAT_SIZE);91 cvGetRows(&object_points, &sub_object_points, base * PAT_SIZE, (base + 1) * PAT_SIZE);92 cvFindExtrinsicCameraParams2(&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation);93 94 // (7)XMLファイルへの書き出し95 CvFileStorage *fs;96 fs = cvOpenFileStorage("./camera.xml", 0, CV_STORAGE_WRITE);97 cvWrite(fs, "intrinsic", intrinsic);98 cvWrite(fs, "rotation", rotation);99 cvWrite(fs, "translation", translation);100 cvWrite(fs, "distortion", distortion);101 cvReleaseFileStorage(&fs);102 103 for (i = 0; i < IMAGE_NUM; i++) {104 cvReleaseImage(&src_img[i]);105 }106 107 // (8)歪み補正108 before_img = cvLoadImage("before_image/test_before.jpg", CV_LOAD_IMAGE_COLOR);109 if (!before_img) {110 fprintf(stderr, "Error!\n");111 return 0;112 }113 after_img = cvCloneImage(before_img);114 cvUndistort2(before_img, after_img, intrinsic, distortion);115 116 // (9)画像の表示 117 cvNamedWindow("Distortion", CV_WINDOW_AUTOSIZE);118 cvShowImage("Distortion", before_img);119 cvNamedWindow("UnDistortion", CV_WINDOW_AUTOSIZE);120 cvShowImage("UnDistortion", after_img);121 cvWaitKey(0);122 123 cvDestroyWindow("Distortion");124 cvDestroyWindow("UnDistortion");125 126 // (10)画像の保存127 cvSaveImage("after_image/test_after.jpg", after_img);128 129 return 0;130}
0 コメント