forked from sourishg/stereo-calibration
-
Notifications
You must be signed in to change notification settings - Fork 0
/
calib_intrinsic.cpp
126 lines (108 loc) · 4.4 KB
/
calib_intrinsic.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdio.h>
#include <iostream>
#include "popt_pp.h"
using namespace std;
using namespace cv;
vector< vector< Point3f > > object_points;
vector< vector< Point2f > > image_points;
vector< Point2f > corners;
vector< vector< Point2f > > left_img_points;
Mat img, gray;
Size im_size;
void setup_calibration(int board_width, int board_height, int num_imgs,
float square_size, char* imgs_directory, char* imgs_filename,
char* extension) {
Size board_size = Size(board_width, board_height);
int board_n = board_width * board_height;
for (int k = 1; k <= num_imgs; k++) {
char img_file[100];
sprintf(img_file, "%s%s%d.%s", imgs_directory, imgs_filename, k, extension);
img = imread(img_file, CV_LOAD_IMAGE_COLOR);
cv::cvtColor(img, gray, CV_BGR2GRAY);
bool found = false;
found = cv::findChessboardCorners(img, board_size, corners,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
if (found)
{
cornerSubPix(gray, corners, cv::Size(5, 5), cv::Size(-1, -1),
TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
drawChessboardCorners(gray, board_size, corners, found);
}
vector< Point3f > obj;
for (int i = 0; i < board_height; i++)
for (int j = 0; j < board_width; j++)
obj.push_back(Point3f((float)j * square_size, (float)i * square_size, 0));
if (found) {
cout << k << ". Found corners!" << endl;
image_points.push_back(corners);
object_points.push_back(obj);
}
}
}
double computeReprojectionErrors(const vector< vector< Point3f > >& objectPoints,
const vector< vector< Point2f > >& imagePoints,
const vector< Mat >& rvecs, const vector< Mat >& tvecs,
const Mat& cameraMatrix , const Mat& distCoeffs) {
vector< Point2f > imagePoints2;
int i, totalPoints = 0;
double totalErr = 0, err;
vector< float > perViewErrors;
perViewErrors.resize(objectPoints.size());
for (i = 0; i < (int)objectPoints.size(); ++i) {
projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix,
distCoeffs, imagePoints2);
err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);
int n = (int)objectPoints[i].size();
perViewErrors[i] = (float) std::sqrt(err*err/n);
totalErr += err*err;
totalPoints += n;
}
return std::sqrt(totalErr/totalPoints);
}
int main(int argc, char const **argv)
{
int board_width, board_height, num_imgs;
float square_size;
char* imgs_directory;
char* imgs_filename;
char* out_file;
char* extension;
static struct poptOption options[] = {
{ "board_width",'w',POPT_ARG_INT,&board_width,0,"Checkerboard width","NUM" },
{ "board_height",'h',POPT_ARG_INT,&board_height,0,"Checkerboard height","NUM" },
{ "num_imgs",'n',POPT_ARG_INT,&num_imgs,0,"Number of checkerboard images","NUM" },
{ "square_size",'s',POPT_ARG_FLOAT,&square_size,0,"Size of checkerboard square","NUM" },
{ "imgs_directory",'d',POPT_ARG_STRING,&imgs_directory,0,"Directory containing images","STR" },
{ "imgs_filename",'i',POPT_ARG_STRING,&imgs_filename,0,"Image filename","STR" },
{ "extension",'e',POPT_ARG_STRING,&extension,0,"Image extension","STR" },
{ "out_file",'o',POPT_ARG_STRING,&out_file,0,"Output calibration filename (YML)","STR" },
POPT_AUTOHELP
{ NULL, 0, 0, NULL, 0, NULL, NULL }
};
POpt popt(NULL, argc, argv, options, 0);
int c;
while((c = popt.getNextOpt()) >= 0) {}
setup_calibration(board_width, board_height, num_imgs, square_size,
imgs_directory, imgs_filename, extension);
printf("Starting Calibration\n");
Mat K;
Mat D;
vector< Mat > rvecs, tvecs;
int flag = 0;
flag |= CV_CALIB_FIX_K4;
flag |= CV_CALIB_FIX_K5;
calibrateCamera(object_points, image_points, img.size(), K, D, rvecs, tvecs, flag);
cout << "Calibration error: " << computeReprojectionErrors(object_points, image_points, rvecs, tvecs, K, D) << endl;
FileStorage fs(out_file, FileStorage::WRITE);
fs << "K" << K;
fs << "D" << D;
fs << "board_width" << board_width;
fs << "board_height" << board_height;
fs << "square_size" << square_size;
printf("Done Calibration\n");
return 0;
}