Subversion Repositories seema-scanner

Rev

Rev 195 | Rev 197 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 195 Rev 196
1
#include "SMCalibrationWorker.h"
1
#include "SMCalibrationWorker.h"
2
#include "SMCalibrationParameters.h"
2
#include "SMCalibrationParameters.h"
3
 
3
 
4
#include "cvtools.h"
4
#include "cvtools.h"
5
 
5
 
6
#include <QSettings>
6
#include <QSettings>
7
 
7
 
-
 
8
#include <ceres/ceres.h>
-
 
9
 
-
 
10
 
-
 
11
struct CircleResidual {
-
 
12
  CircleResidual(std::vector<cv::Point3f> _pointsOnArc)
-
 
13
      : pointsOnArc(_pointsOnArc) {}
-
 
14
 
-
 
15
  template <typename T>
-
 
16
  bool operator()(const T* const point_x, const T* const point_y, const T* const point_z,
-
 
17
                  const T* const axis_x, const T* const axis_y, const T* const axis_z,
-
 
18
                  T* residual) const {
-
 
19
 
-
 
20
    cv::Vec3f axis(*axis_x, *axis_y, *axis_z);
-
 
21
    cv::Vec3f point(*point_x, *point_y, *point_z);
-
 
22
 
-
 
23
    unsigned int l = pointsOnArc.size();
-
 
24
    std::vector<float> dI(l);
-
 
25
    for(unsigned int i=0; i<l; i++){
-
 
26
      cv::Vec3f p = cv::Vec3f(pointsOnArc[i]);
-
 
27
      // point to line distance
-
 
28
      dI[i] = cv::norm((point-p)-(point-p).dot(axis)*axis);
-
 
29
    }
-
 
30
    float sum = std::accumulate(dI.begin(), dI.end(), 0.0);
-
 
31
    float mean = sum / dI.size();
-
 
32
    float meanDev = 0;
-
 
33
    for(unsigned int k=0; k<l; k++){
-
 
34
      meanDev += std::abs(dI[k] - mean);
-
 
35
    }
-
 
36
    meanDev /= l;
-
 
37
 
-
 
38
    residual[0] = T(meanDev);
-
 
39
 
-
 
40
    return true;
-
 
41
  }
-
 
42
 
-
 
43
 private:
-
 
44
 
-
 
45
  // Observations for one checkerboard corner.
-
 
46
  const std::vector<cv::Point3f> pointsOnArc;
-
 
47
};
-
 
48
 
-
 
49
 
-
 
50
// Closed form solution to solve for the rotation axis from sets of 3D point coordinates of flat pattern feature points
-
 
51
// Algorithm according to Chen et al., Rotation axis calibration of a turntable using constrained global optimization, Optik 2014
-
 
52
// DTU, 2014, Jakob Wilm
-
 
53
static void rotationAxisCalibration(const std::vector< std::vector<cv::Point3f> > Qcam, const std::vector<cv::Point3f> Qobj, cv::Vec3f &axis, cv::Vec3f &point, float &error){
-
 
54
 
-
 
55
    // number of frames (points on each arch)
-
 
56
    int l = Qcam.size();
-
 
57
 
-
 
58
    // number of points in each frame
-
 
59
    size_t mn = Qobj.size();
-
 
60
 
-
 
61
    assert(mn == Qcam[0].size());
-
 
62
 
-
 
63
    // construct matrix for axis determination
-
 
64
    cv::Mat M(6, 6, CV_32F, cv::Scalar(0));
-
 
65
 
-
 
66
    for(int k=0; k<l; k++){
-
 
67
        for(unsigned int idx=0; idx<mn; idx++){
-
 
68
 
-
 
69
//            float i = Qobj[idx].x+4;
-
 
70
//            float j = Qobj[idx].y+4;
-
 
71
            float i = Qobj[idx].x;
-
 
72
            float j = Qobj[idx].y;
-
 
73
 
-
 
74
            float x = Qcam[k][idx].x;
-
 
75
            float y = Qcam[k][idx].y;
-
 
76
            float z = Qcam[k][idx].z;
-
 
77
 
-
 
78
            M += (cv::Mat_<float>(6,6) << x*x, x*y, x*z, x, i*x, j*x,
-
 
79
                                            0, y*y, y*z, y, i*y, j*y,
-
 
80
                                            0,   0, z*z, z, i*z, j*z,
-
 
81
                                            0,   0,   0, 1,   i,   j,
-
 
82
                                            0,   0,   0, 0, i*i, i*j,
-
 
83
                                            0,   0,   0, 0,   0, j*j);
-
 
84
 
-
 
85
        }
-
 
86
    }
-
 
87
 
-
 
88
    cv::completeSymm(M, false);
-
 
89
 
-
 
90
    // solve for axis
-
 
91
    std::vector<float> lambda;
-
 
92
    cv::Mat u;
-
 
93
    cv::eigen(M, lambda, u);
-
 
94
 
-
 
95
    float minLambda = std::abs(lambda[0]);
-
 
96
    int idx = 0;
-
 
97
    for(unsigned int i=1; i<lambda.size(); i++){
-
 
98
        if(abs(lambda[i]) < minLambda){
-
 
99
            minLambda = lambda[i];
-
 
100
            idx = i;
-
 
101
        }
-
 
102
    }
-
 
103
 
-
 
104
    axis = u.row(idx).colRange(0, 3);
-
 
105
    axis = cv::normalize(axis);
-
 
106
 
-
 
107
    float nx = u.at<float>(idx, 0);
-
 
108
    float ny = u.at<float>(idx, 1);
-
 
109
    float nz = u.at<float>(idx, 2);
-
 
110
    //float d  = u.at<float>(idx, 3);
-
 
111
    float dh = u.at<float>(idx, 4);
-
 
112
    float dv = u.at<float>(idx, 5);
-
 
113
 
-
 
114
//    // Paper version: c is initially eliminated
-
 
115
//    cv::Mat A(l*mn, mn+2, CV_32F, cv::Scalar(0.0));
-
 
116
//    cv::Mat bb(l*mn, 1, CV_32F);
-
 
117
 
-
 
118
//    for(int k=0; k<l; k++){
-
 
119
//        for(unsigned int idx=0; idx<mn; idx++){
-
 
120
 
-
 
121
//            float i = Qobj[idx].x;
-
 
122
//            float j = Qobj[idx].y;
-
 
123
 
-
 
124
//            float x = Qcam[k][idx].x;
-
 
125
//            float y = Qcam[k][idx].y;
-
 
126
//            float z = Qcam[k][idx].z;
-
 
127
 
-
 
128
//            float f = x*x + y*y + z*z + (2*x*nx + 2*y*ny + 2*z*nz)*(i*dh + j*dv);
-
 
129
 
-
 
130
//            int row = k*mn+idx;
-
 
131
//            A.at<float>(row, 0) = 2*x - (2*z*nx)/nz;
-
 
132
//            A.at<float>(row, 1) = 2*y - (2*z*ny)/nz;
-
 
133
//            A.at<float>(row, idx+2) = 1.0;
-
 
134
 
-
 
135
//            bb.at<float>(row, 0) = f + (2*z*d)/nz;
-
 
136
//        }
-
 
137
//    }
-
 
138
 
-
 
139
//    // solve for point
-
 
140
//    cv::Mat abe;
-
 
141
//    cv::solve(A, bb, abe, cv::DECOMP_SVD);
-
 
142
 
-
 
143
//    float a = abe.at<float>(0, 0);
-
 
144
//    float b = abe.at<float>(1, 0);
-
 
145
//    float c = -(nx*a+ny*b+d)/nz;
-
 
146
 
-
 
147
    // Our version: solve simultanously for a,b,c
-
 
148
    cv::Mat A(l*mn, mn+3, CV_32F, cv::Scalar(0.0));
-
 
149
    cv::Mat bb(l*mn, 1, CV_32F);
-
 
150
 
-
 
151
    for(int k=0; k<l; k++){
-
 
152
        for(unsigned int idx=0; idx<mn; idx++){
-
 
153
 
-
 
154
            float i = Qobj[idx].x;
-
 
155
            float j = Qobj[idx].y;
-
 
156
 
-
 
157
            float x = Qcam[k][idx].x;
-
 
158
            float y = Qcam[k][idx].y;
-
 
159
            float z = Qcam[k][idx].z;
-
 
160
 
-
 
161
            float f = x*x + y*y + z*z + (2*x*nx + 2*y*ny + 2*z*nz)*(i*dh + j*dv);
-
 
162
 
-
 
163
            int row = k*mn+idx;
-
 
164
            A.at<float>(row, 0) = 2*x;
-
 
165
            A.at<float>(row, 1) = 2*y;
-
 
166
            A.at<float>(row, 2) = 2*z;
-
 
167
            A.at<float>(row, idx+3) = 1.0;
-
 
168
 
-
 
169
            bb.at<float>(row, 0) = f;
-
 
170
        }
-
 
171
    }
-
 
172
 
-
 
173
    // solve for point
-
 
174
    cv::Mat abe;
-
 
175
    cv::solve(A, bb, abe, cv::DECOMP_SVD);
-
 
176
 
-
 
177
    float a = abe.at<float>(0, 0);
-
 
178
    float b = abe.at<float>(1, 0);
-
 
179
    float c = abe.at<float>(2, 0);
-
 
180
 
-
 
181
    point[0] = a;
-
 
182
    point[1] = b;
-
 
183
    point[2] = c;
-
 
184
 
-
 
185
    // Non-linear optimization using Ceres
-
 
186
    double point_x = point[0];
-
 
187
    double point_y = point[1];
-
 
188
    double point_z = point[2];
-
 
189
    double axis_x = axis[0];
-
 
190
    double axis_y = axis[1];
-
 
191
    double axis_z = axis[2];
-
 
192
 
-
 
193
    ceres::Problem problem;
-
 
194
    // loop through saddle points
-
 
195
    for(unsigned int idx=0; idx<mn; idx++){
-
 
196
        std::vector<cv::Point3f> pointsOnArch(l);
-
 
197
        for(int k=0; k<l; k++){
-
 
198
            pointsOnArch[k] = Qcam[k][idx];
-
 
199
        }
-
 
200
        ceres::CostFunction* cost_function =
-
 
201
           new ceres::NumericDiffCostFunction<CircleResidual, ceres::CENTRAL, 1, 1, 1, 1, 1, 1, 1>(
-
 
202
               new CircleResidual(pointsOnArch));
-
 
203
        problem.AddResidualBlock(cost_function, NULL, &point_x, &point_y, &point_z, &axis_x, &axis_y, &axis_z);
-
 
204
    }
-
 
205
 
-
 
206
    // Run the solver!
-
 
207
    ceres::Solver::Options options;
-
 
208
    options.linear_solver_type = ceres::DENSE_QR;
-
 
209
    options.minimizer_progress_to_stdout = true;
-
 
210
    ceres::Solver::Summary summary;
-
 
211
    ceres::Solve(options, &problem, &summary);
-
 
212
 
-
 
213
    std::cout << summary.BriefReport() << "\n";
-
 
214
 
-
 
215
 
-
 
216
    point = cv::Point3f(point_x, point_y, point_z);
-
 
217
    axis = cv::Point3f(axis_x, axis_y, axis_z);
-
 
218
 
-
 
219
 
-
 
220
    // Error estimate (mean 3D point deviations from circles around rotation axis)
-
 
221
    error = 0;
-
 
222
    // loop through saddle points
-
 
223
    for(unsigned int idx=0; idx<mn; idx++){
-
 
224
 
-
 
225
        // vector of distances from rotation axis
-
 
226
        std::vector<float> dI(l);
-
 
227
        // loop through angular positions
-
 
228
        for(int k=0; k<l; k++){
-
 
229
            cv::Vec3f p = cv::Vec3f(Qcam[k][idx]);
-
 
230
            // point to line distance
-
 
231
            dI[k] = cv::norm((point-p)-(point-p).dot(axis)*axis);
-
 
232
        }
-
 
233
        float sum = std::accumulate(dI.begin(), dI.end(), 0.0);
-
 
234
        float mean = sum / dI.size();
-
 
235
        float meanDev = 0;
-
 
236
        for(int k=0; k<l; k++){
-
 
237
            meanDev += std::abs(dI[k] - mean);
-
 
238
        }
-
 
239
        meanDev /= l;
-
 
240
        //std::cout << meanDev << std::endl;
-
 
241
        error += meanDev;
-
 
242
    }
-
 
243
    error /= mn;
-
 
244
 
-
 
245
}
-
 
246
 
8
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
247
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
9
 
248
 
10
    QSettings settings;
249
    QSettings settings;
11
 
250
 
12
    // Number of saddle points on calibration pattern
251
    // Number of saddle points on calibration pattern
13
    int checkerCountX = settings.value("calibration/patternSizeX", 22).toInt();
252
    int checkerCountX = settings.value("calibration/patternSizeX", 22).toInt();
14
    int checkerCountY = settings.value("calibration/patternSizeY", 13).toInt();
253
    int checkerCountY = settings.value("calibration/patternSizeY", 13).toInt();
15
    cv::Size checkerCount(checkerCountX, checkerCountY);
254
    cv::Size checkerCount(checkerCountX, checkerCountY);
16
 
255
 
17
    unsigned int nSets = calibrationData.size();
256
    unsigned int nSets = calibrationData.size();
18
 
257
 
19
    // 2D Points collected for OpenCV's calibration procedures
258
    // 2D Points collected for OpenCV's calibration procedures
20
    std::vector< std::vector<cv::Point2f> > qc0, qc1;
259
    std::vector< std::vector<cv::Point2f> > qc0, qc1;
21
    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
260
    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;
22
 
261
 
23
    std::vector<bool> success0(nSets), success1(nSets);
262
    std::vector<bool> success0(nSets), success1(nSets);
24
 
263
 
25
    std::vector<float> angles;
264
    std::vector<float> angles;
26
 
265
 
27
    // Loop through calibration sets
266
    // Loop through calibration sets
28
    for(unsigned int i=0; i<nSets; i++){
267
    for(unsigned int i=0; i<nSets; i++){
29
 
268
 
30
        SMCalibrationSet SMCalibrationSetI = calibrationData[i];
269
        SMCalibrationSet SMCalibrationSetI = calibrationData[i];
31
 
270
 
32
        if(!SMCalibrationSetI.checked)
271
        if(!SMCalibrationSetI.checked)
33
            continue;
272
            continue;
34
 
273
 
35
        // Camera 0
274
        // Camera 0
36
        std::vector<cv::Point2f> qci0;
275
        std::vector<cv::Point2f> qci0;
37
 
276
 
38
        // Convert to grayscale
277
        // Convert to grayscale
39
        cv::Mat gray;
278
        cv::Mat gray;
40
        if(SMCalibrationSetI.frame0.channels() == 1)
279
        if(SMCalibrationSetI.frame0.channels() == 1)
41
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);
280
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_BayerBG2GRAY);
42
        else
281
        else
43
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_RGB2GRAY);
282
            cv::cvtColor(SMCalibrationSetI.frame0, gray, CV_RGB2GRAY);
44
 
283
 
45
        // Extract checker corners
284
        // Extract checker corners
46
        success0[i] = cv::findChessboardCorners(gray, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
285
        success0[i] = cv::findChessboardCorners(gray, checkerCount, qci0, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
47
        if(success0[i]){
286
        if(success0[i]){
48
            cv::cornerSubPix(gray, qci0, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
287
            cv::cornerSubPix(gray, qci0, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
49
            // Draw colored chessboard
288
            // Draw colored chessboard
50
            cv::Mat color;
289
            cv::Mat color;
51
            if(SMCalibrationSetI.frame0.channels() == 1)
290
            if(SMCalibrationSetI.frame0.channels() == 1)
52
                cv::cvtColor(SMCalibrationSetI.frame0, color, CV_BayerBG2RGB);
291
                cv::cvtColor(SMCalibrationSetI.frame0, color, CV_BayerBG2RGB);
53
            else
292
            else
54
                color = SMCalibrationSetI.frame0.clone();
293
                color = SMCalibrationSetI.frame0.clone();
55
 
294
 
56
            cvtools::drawChessboardCorners(color, checkerCount, qci0, success0[i], 10);
295
            cvtools::drawChessboardCorners(color, checkerCount, qci0, success0[i], 10);
57
            SMCalibrationSetI.frame0Result = color;
296
            SMCalibrationSetI.frame0Result = color;
58
        }
297
        }
59
 
298
 
60
        emit newFrameResult(i, 0, success0[i], SMCalibrationSetI.frame0Result);
299
        emit newFrameResult(i, 0, success0[i], SMCalibrationSetI.frame0Result);
61
 
300
 
62
        // Camera 1
301
        // Camera 1
63
        std::vector<cv::Point2f> qci1;
302
        std::vector<cv::Point2f> qci1;
64
 
303
 
65
        // Convert to grayscale
304
        // Convert to grayscale
66
        if(SMCalibrationSetI.frame1.channels() == 1)
305
        if(SMCalibrationSetI.frame1.channels() == 1)
67
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);
306
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_BayerBG2GRAY);
68
        else
307
        else
69
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_RGB2GRAY);
308
            cv::cvtColor(SMCalibrationSetI.frame1, gray, CV_RGB2GRAY);
70
 
309
 
71
        // Extract checker corners
310
        // Extract checker corners
72
        success1[i] = cv::findChessboardCorners(gray, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
311
        success1[i] = cv::findChessboardCorners(gray, checkerCount, qci1, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_FAST_CHECK);
73
        if(success1[i]){
312
        if(success1[i]){
74
            cv::cornerSubPix(gray, qci1, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
313
            cv::cornerSubPix(gray, qci1, cv::Size(6, 6), cv::Size(1, 1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 20, 0.0001));
75
            // Draw colored chessboard
314
            // Draw colored chessboard
76
            cv::Mat color;
315
            cv::Mat color;
77
            if(SMCalibrationSetI.frame1.channels() == 1)
316
            if(SMCalibrationSetI.frame1.channels() == 1)
78
                cv::cvtColor(SMCalibrationSetI.frame1, color, CV_BayerBG2RGB);
317
                cv::cvtColor(SMCalibrationSetI.frame1, color, CV_BayerBG2RGB);
79
            else
318
            else
80
                color = SMCalibrationSetI.frame1.clone();
319
                color = SMCalibrationSetI.frame1.clone();
81
 
320
 
82
            cvtools::drawChessboardCorners(color, checkerCount, qci1, success1[i], 10);
321
            cvtools::drawChessboardCorners(color, checkerCount, qci1, success1[i], 10);
83
            SMCalibrationSetI.frame1Result = color;
322
            SMCalibrationSetI.frame1Result = color;
84
        }
323
        }
85
 
324
 
86
        emit newFrameResult(i, 1, success1[i], SMCalibrationSetI.frame1Result);
325
        emit newFrameResult(i, 1, success1[i], SMCalibrationSetI.frame1Result);
87
 
326
 
88
        if(success0[i])
327
        if(success0[i])
89
            qc0.push_back(qci0);
328
            qc0.push_back(qci0);
90
 
329
 
91
        if(success1[i])
330
        if(success1[i])
92
            qc1.push_back(qci1);
331
            qc1.push_back(qci1);
93
 
332
 
94
        if(success0[i] && success1[i]){
333
        if(success0[i] && success1[i]){
95
            qc0Stereo.push_back(qci0);
334
            qc0Stereo.push_back(qci0);
96
            qc1Stereo.push_back(qci1);
335
            qc1Stereo.push_back(qci1);
97
            angles.push_back(SMCalibrationSetI.rotationAngle);
336
            angles.push_back(SMCalibrationSetI.rotationAngle);
98
        }
337
        }
99
 
338
 
100
        // Show progress
339
        // Show progress
101
        emit newSetProcessed(i);
340
        emit newSetProcessed(i);
102
    }
341
    }
103
 
342
 
104
    size_t nValidSets = angles.size();
343
    size_t nValidSets = angles.size();
105
    if(nValidSets < 2){
344
    if(nValidSets < 2){
106
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
345
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
107
        emit done();
346
        emit done();
108
        return;
347
        return;
109
    }
348
    }
110
 
349
 
111
    // Generate world object coordinates [mm]
350
    // Generate world object coordinates [mm]
112
    float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat(); // width and height of one checker square in mm
351
    float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat(); // width and height of one checker square in mm
113
    std::vector<cv::Point3f> Qi;
352
    std::vector<cv::Point3f> Qi;
114
    for (int h=0; h<checkerCount.height; h++)
353
    for (int h=0; h<checkerCount.height; h++)
115
        for (int w=0; w<checkerCount.width; w++)
354
        for (int w=0; w<checkerCount.width; w++)
116
            Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));
355
            Qi.push_back(cv::Point3f(checkerSize * w, checkerSize* h, 0.0));
117
 
356
 
118
    std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;
357
    std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;
119
    for(unsigned int i=0; i<qc0.size(); i++)
358
    for(unsigned int i=0; i<qc0.size(); i++)
120
        Q0.push_back(Qi);
359
        Q0.push_back(Qi);
121
    for(unsigned int i=0; i<qc1.size(); i++)
360
    for(unsigned int i=0; i<qc1.size(); i++)
122
        Q1.push_back(Qi);
361
        Q1.push_back(Qi);
123
    for(unsigned int i=0; i<nValidSets; i++)
362
    for(unsigned int i=0; i<nValidSets; i++)
124
        QStereo.push_back(Qi);
363
        QStereo.push_back(Qi);
125
 
364
 
126
    // calibrate the cameras
365
    // calibrate the cameras
127
    SMCalibrationParameters cal;
366
    SMCalibrationParameters cal;
128
    cal.frameWidth = calibrationData[0].frame0.cols;
367
    cal.frameWidth = calibrationData[0].frame0.cols;
129
    cal.frameHeight = calibrationData[0].frame0.rows;
368
    cal.frameHeight = calibrationData[0].frame0.rows;
130
    cv::Size frameSize(cal.frameWidth, cal.frameHeight);
369
    cv::Size frameSize(cal.frameWidth, cal.frameHeight);
131
 
370
 
132
    // determine only k1, k2 for lens distortion
371
    // determine only k1, k2 for lens distortion
133
    int flags = cv::CALIB_FIX_ASPECT_RATIO + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;
372
    int flags = cv::CALIB_FIX_ASPECT_RATIO + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;
134
    // Note: several of the output arguments below must be cv::Mat, otherwise segfault
373
    // Note: several of the output arguments below must be cv::Mat, otherwise segfault
135
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
374
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
136
    cal.cam0_error = cv::calibrateCamera(Q0, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags,
375
    cal.cam0_error = cv::calibrateCamera(Q0, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, flags,
137
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
376
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
138
//std::cout << cal.k0 << std::endl;
377
//std::cout << cal.k0 << std::endl;
139
//    // refine extrinsics for camera 0
378
//    // refine extrinsics for camera 0
140
//    for(int i=0; i<Q.size(); i++)
379
//    for(int i=0; i<Q.size(); i++)
141
//        cv::solvePnPRansac(Q[i], qc0[i], cal.K0, cal.k0, cam_rvecs0[i], cam_tvecs0[i], true, 100, 0.05, 100, cv::noArray(), CV_ITERATIVE);
380
//        cv::solvePnPRansac(Q[i], qc0[i], cal.K0, cal.k0, cam_rvecs0[i], cam_tvecs0[i], true, 100, 0.05, 100, cv::noArray(), CV_ITERATIVE);
142
 
381
 
143
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
382
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
144
    cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags,
383
    cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, flags,
145
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
384
                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));
146
//std::cout << cal.k1 << std::endl;
385
//std::cout << cal.k1 << std::endl;
147
    // stereo calibration
386
    // stereo calibration
148
    int flags_stereo = cv::CALIB_FIX_INTRINSIC;// + cv::CALIB_FIX_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT + cv::CALIB_FIX_ASPECT_RATIO;
387
    int flags_stereo = cv::CALIB_FIX_INTRINSIC;// + cv::CALIB_FIX_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT + cv::CALIB_FIX_ASPECT_RATIO;
149
    cv::Mat E, F, R1, T1;
388
    cv::Mat E, F, R1, T1;
150
    cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,
389
    cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,
151
                                              frameSize, R1, T1, E, F,
390
                                              frameSize, R1, T1, E, F,
152
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON),
391
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON),
153
                                              flags_stereo);
392
                                              flags_stereo);
154
 
393
 
155
    cal.R1 = R1;
394
    cal.R1 = R1;
156
    cal.T1 = T1;
395
    cal.T1 = T1;
157
    cal.E = E;
396
    cal.E = E;
158
    cal.F = F;
397
    cal.F = F;
159
 
398
 
160
    // Determine per-view reprojection errors:
399
    // Determine per-view reprojection errors:
161
    cal.cam0_errors_per_view.resize(nSets);
400
    cal.cam0_errors_per_view.resize(nSets);
162
    int idx = 0;
401
    int idx = 0;
163
    for(unsigned int i = 0; i < nSets; ++i){
402
    for(unsigned int i = 0; i < nSets; ++i){
164
        if(success0[i]){
403
        if(success0[i]){
165
            int n = (int)Q0[idx].size();
404
            int n = (int)Q0[idx].size();
166
            std::vector<cv::Point2f> qc_proj;
405
            std::vector<cv::Point2f> qc_proj;
167
            cv::projectPoints(cv::Mat(Q0[idx]), cam_rvecs0[idx], cam_tvecs0[idx], cal.K0,  cal.k0, qc_proj);
406
            cv::projectPoints(cv::Mat(Q0[idx]), cam_rvecs0[idx], cam_tvecs0[idx], cal.K0,  cal.k0, qc_proj);
168
            float err = 0;
407
            float err = 0;
169
            for(unsigned int j=0; j<qc_proj.size(); j++){
408
            for(unsigned int j=0; j<qc_proj.size(); j++){
170
                cv::Point2f d = qc0[idx][j] - qc_proj[j];
409
                cv::Point2f d = qc0[idx][j] - qc_proj[j];
171
                err += cv::sqrt(d.x*d.x + d.y*d.y);
410
                err += cv::sqrt(d.x*d.x + d.y*d.y);
172
            }
411
            }
173
            cal.cam0_errors_per_view[i] = (float)err/n;
412
            cal.cam0_errors_per_view[i] = (float)err/n;
174
            idx++;
413
            idx++;
175
        } else
414
        } else
176
            cal.cam0_errors_per_view[i] = NAN;
415
            cal.cam0_errors_per_view[i] = NAN;
177
    }
416
    }
178
    cal.cam1_errors_per_view.resize(nSets);
417
    cal.cam1_errors_per_view.resize(nSets);
179
    idx = 0;
418
    idx = 0;
180
    for(unsigned int i = 0; i < nSets; ++i){
419
    for(unsigned int i = 0; i < nSets; ++i){
181
        if(success1[i]){
420
        if(success1[i]){
182
            int n = (int)Q1[idx].size();
421
            int n = (int)Q1[idx].size();
183
            std::vector<cv::Point2f> qc_proj;
422
            std::vector<cv::Point2f> qc_proj;
184
            cv::projectPoints(cv::Mat(Q1[idx]), cam_rvecs1[idx], cam_tvecs1[idx], cal.K1,  cal.k1, qc_proj);
423
            cv::projectPoints(cv::Mat(Q1[idx]), cam_rvecs1[idx], cam_tvecs1[idx], cal.K1,  cal.k1, qc_proj);
185
            float err = 0;
424
            float err = 0;
186
            for(unsigned int j=0; j<qc_proj.size(); j++){
425
            for(unsigned int j=0; j<qc_proj.size(); j++){
187
                cv::Point2f d = qc1[idx][j] - qc_proj[j];
426
                cv::Point2f d = qc1[idx][j] - qc_proj[j];
188
                err += cv::sqrt(d.x*d.x + d.y*d.y);
427
                err += cv::sqrt(d.x*d.x + d.y*d.y);
189
            }
428
            }
190
            cal.cam1_errors_per_view[i] = (float)err/n;
429
            cal.cam1_errors_per_view[i] = (float)err/n;
191
            idx++;
430
            idx++;
192
       } else
431
       } else
193
            cal.cam1_errors_per_view[i] = NAN;
432
            cal.cam1_errors_per_view[i] = NAN;
194
    }
433
    }
195
 
434
 
196
//    // hand-eye calibration
435
//    // hand-eye calibration
197
//    std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame
436
//    std::vector<cv::Matx33f> Rc(nValidSets - 1); // rotations/translations of the checkerboard in camera 0 reference frame
198
//    std::vector<cv::Vec3f> Tc(nValidSets - 1);
437
//    std::vector<cv::Vec3f> Tc(nValidSets - 1);
199
//    std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame
438
//    std::vector<cv::Matx33f> Rr(nValidSets - 1); // in rotation stage reference frame
200
//    std::vector<cv::Vec3f> Tr(nValidSets - 1);
439
//    std::vector<cv::Vec3f> Tr(nValidSets - 1);
201
//    for(int i=0; i<nValidSets-1; i++){
440
//    for(int i=0; i<nValidSets-1; i++){
202
//        // relative transformations in camera
441
//        // relative transformations in camera
203
//        cv::Mat cRw1, cRw2;
442
//        cv::Mat cRw1, cRw2;
204
//        cv::Rodrigues(cam_rvecs0[i], cRw1);
443
//        cv::Rodrigues(cam_rvecs0[i], cRw1);
205
//        cv::Rodrigues(cam_rvecs0[i+1], cRw2);
444
//        cv::Rodrigues(cam_rvecs0[i+1], cRw2);
206
//        cv::Mat cTw1 = cam_tvecs0[i];
445
//        cv::Mat cTw1 = cam_tvecs0[i];
207
//        cv::Mat cTw2 = cam_tvecs0[i+1];
446
//        cv::Mat cTw2 = cam_tvecs0[i+1];
208
//        cv::Mat w1Rc = cRw1.t();
447
//        cv::Mat w1Rc = cRw1.t();
209
//        cv::Mat w1Tc = -cRw1.t()*cTw1;
448
//        cv::Mat w1Tc = -cRw1.t()*cTw1;
210
//        Rc[i] = cv::Mat(cRw2*w1Rc);
449
//        Rc[i] = cv::Mat(cRw2*w1Rc);
211
//        Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);
450
//        Tc[i] = cv::Mat(cRw2*w1Tc+cTw2);
212
 
451
 
213
//        // relative transformations in rotation stage
452
//        // relative transformations in rotation stage
214
//        // we define the rotation axis to be in origo, pointing in positive y direction
453
//        // we define the rotation axis to be in origo, pointing in positive y direction
215
//        float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;
454
//        float angleRadians = (angles[i+1]-angles[i])/180.0*M_PI;
216
//        cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
455
//        cv::Vec3f rot_rvec(0.0, -angleRadians, 0.0);
217
//        cv::Mat Rri;
456
//        cv::Mat Rri;
218
//        cv::Rodrigues(rot_rvec, Rri);
457
//        cv::Rodrigues(rot_rvec, Rri);
219
//        Rr[i] = Rri;
458
//        Rr[i] = Rri;
220
//        Tr[i] = 0.0;
459
//        Tr[i] = 0.0;
221
 
460
 
222
////        std::cout << i << std::endl;
461
////        std::cout << i << std::endl;
223
////        std::cout << "cTw1" << cTw1 << std::endl;
462
////        std::cout << "cTw1" << cTw1 << std::endl;
224
////        std::cout << "cTw2" << cTw2 << std::endl;
463
////        std::cout << "cTw2" << cTw2 << std::endl;
225
////        std::cout << "w2Rc" << w2Rc << std::endl;
464
////        std::cout << "w2Rc" << w2Rc << std::endl;
226
////        std::cout << "w2Tc" << w2Tc << std::endl;
465
////        std::cout << "w2Tc" << w2Tc << std::endl;
227
 
466
 
228
////        std::cout << "w2Rc" << w2Rc << std::endl;
467
////        std::cout << "w2Rc" << w2Rc << std::endl;
229
////        std::cout << "w2Tc" << w2Tc << std::endl;
468
////        std::cout << "w2Tc" << w2Tc << std::endl;
230
 
469
 
231
////        cv::Mat Rci;
470
////        cv::Mat Rci;
232
////        cv::Rodrigues(Rc[i], Rci);
471
////        cv::Rodrigues(Rc[i], Rci);
233
////        std::cout << "Rci" << Rci << std::endl;
472
////        std::cout << "Rci" << Rci << std::endl;
234
////        std::cout << "Tc[i]" << Tc[i] << std::endl;
473
////        std::cout << "Tc[i]" << Tc[i] << std::endl;
235
 
474
 
236
////        std::cout << "rot_rvec" << rot_rvec << std::endl;
475
////        std::cout << "rot_rvec" << rot_rvec << std::endl;
237
////        std::cout << "Tr[i]" << Tr[i] << std::endl;
476
////        std::cout << "Tr[i]" << Tr[i] << std::endl;
238
////        std::cout << std::endl;
477
////        std::cout << std::endl;
239
//    }
478
//    }
240
 
479
 
241
//    // determine the transformation from rotation stage to camera 0
480
//    // determine the transformation from rotation stage to camera 0
242
//    cvtools::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
481
//    cvtools::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
243
 
482
 
244
//    for(int i=0; i<nValidSets-1; i++){
483
//    for(int i=0; i<nValidSets-1; i++){
245
//        std::cout << i << std::endl;
484
//        std::cout << i << std::endl;
246
 
485
 
247
//        cv::Mat Rci;
486
//        cv::Mat Rci;
248
//        cv::Rodrigues(Rc[i], Rci);
487
//        cv::Rodrigues(Rc[i], Rci);
249
//        std::cout << "Rc[i]" << Rci << std::endl;
488
//        std::cout << "Rc[i]" << Rci << std::endl;
250
//        std::cout << "Tc[i]" << Tc[i] << std::endl;
489
//        std::cout << "Tc[i]" << Tc[i] << std::endl;
251
 
490
 
252
//        cv::Mat Rri;
491
//        cv::Mat Rri;
253
//        cv::Rodrigues(Rr[i], Rri);
492
//        cv::Rodrigues(Rr[i], Rri);
254
//        std::cout << "Rr[i]" << Rri << std::endl;
493
//        std::cout << "Rr[i]" << Rri << std::endl;
255
//        std::cout << "Tr[i]" << Tr[i] << std::endl;
494
//        std::cout << "Tr[i]" << Tr[i] << std::endl;
256
 
495
 
257
//        cv::Mat Rcr = cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t());
496
//        cv::Mat Rcr = cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t());
258
//        cv::Rodrigues(Rcr, Rcr);
497
//        cv::Rodrigues(Rcr, Rcr);
259
//        cv::Mat Tcr = -cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t())*cv::Mat(cal.Tr) + cv::Mat(cal.Rr)*cv::Mat(Tc[i]) + cv::Mat(cal.Tr);
498
//        cv::Mat Tcr = -cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t())*cv::Mat(cal.Tr) + cv::Mat(cal.Rr)*cv::Mat(Tc[i]) + cv::Mat(cal.Tr);
260
//        std::cout << "Rcr[i]" << Rcr << std::endl;
499
//        std::cout << "Rcr[i]" << Rcr << std::endl;
261
//        std::cout << "Tcr[i]" << Tcr << std::endl;
500
//        std::cout << "Tcr[i]" << Tcr << std::endl;
262
//        std::cout << std::endl;
501
//        std::cout << std::endl;
263
//    }
502
//    }
264
 
503
 
265
 
504
 
266
    // Direct rotation axis calibration //
505
    // Direct rotation axis calibration //
267
    // full camera matrices
506
    // full camera matrices
268
    cv::Matx34f P0 = cv::Matx34f::eye();
507
    cv::Matx34f P0 = cv::Matx34f::eye();
269
    cv::Mat RT1(3, 4, CV_32F);
508
    cv::Mat RT1(3, 4, CV_32F);
270
    cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));
509
    cv::Mat(cal.R1).copyTo(RT1(cv::Range(0, 3), cv::Range(0, 3)));
271
    cv::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));
510
    cv::Mat(cal.T1).copyTo(RT1(cv::Range(0, 3), cv::Range(3, 4)));
272
    cv::Matx34f P1 = cv::Matx34f(RT1);
511
    cv::Matx34f P1 = cv::Matx34f(RT1);
273
 
512
 
274
    // calibration points in camera 0 frame
513
    // calibration points in camera 0 frame
275
    std::vector< std::vector<cv::Point3f> > Qcam;
514
    std::vector< std::vector<cv::Point3f> > Qcam;
276
 
515
 
277
    for(unsigned int i=0; i<nValidSets; i++){
516
    for(unsigned int i=0; i<nValidSets; i++){
278
        std::vector<cv::Point2f> qc0i, qc1i;
517
        std::vector<cv::Point2f> qc0i, qc1i;
279
 
518
 
280
        cv::undistortPoints(qc0Stereo[i], qc0i, cal.K0, cal.k0);
519
        cv::undistortPoints(qc0Stereo[i], qc0i, cal.K0, cal.k0);
281
        cv::undistortPoints(qc1Stereo[i], qc1i, cal.K1, cal.k1);
520
        cv::undistortPoints(qc1Stereo[i], qc1i, cal.K1, cal.k1);
282
//        qc0i = qc0[i];
521
//        qc0i = qc0[i];
283
//        qc1i = qc1[i];
522
//        qc1i = qc1[i];
284
 
523
 
285
        cv::Mat Qhom, Qcami;
524
        cv::Mat Qhom, Qcami;
286
        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
525
        cv::triangulatePoints(P0, P1, qc0i, qc1i, Qhom);
287
        cvtools::convertMatFromHomogeneous(Qhom, Qcami);
526
        cvtools::convertMatFromHomogeneous(Qhom, Qcami);
288
        std::vector<cv::Point3f> QcamiPoints;
527
        std::vector<cv::Point3f> QcamiPoints;
289
        cvtools::matToPoints3f(Qcami, QcamiPoints);
528
        cvtools::matToPoints3f(Qcami, QcamiPoints);
290
 
529
 
291
        Qcam.push_back(QcamiPoints);
530
        Qcam.push_back(QcamiPoints);
292
    }
531
    }
293
 
532
 
294
//    cv::Mat QcamMat(Qcam.size(), Qcam[0].size(), CV_32FC3);
533
//    cv::Mat QcamMat(Qcam.size(), Qcam[0].size(), CV_32FC3);
295
//    for(int i=0; i<Qcam.size(); i++)
534
//    for(int i=0; i<Qcam.size(); i++)
296
//        for(int j=0; j<Qcam[0].size(); j++)
535
//        for(int j=0; j<Qcam[0].size(); j++)
297
//            QcamMat.at<cv::Point3f>(i,j) = Qcam[i][j];
536
//            QcamMat.at<cv::Point3f>(i,j) = Qcam[i][j];
298
//    cvtools::writeMat(QcamMat, "Qcam.mat", "Qcam");
537
//    cvtools::writeMat(QcamMat, "Qcam.mat", "Qcam");
299
 
538
 
300
    cv::Vec3f axis, point;
539
    cv::Vec3f axis, point;
301
    float rot_axis_error;
540
    float rot_axis_error;
302
    cvtools::rotationAxisCalibration(Qcam, Qi, axis, point, rot_axis_error);
541
    rotationAxisCalibration(Qcam, Qi, axis, point, rot_axis_error);
303
 
542
 
304
    // construct transformation matrix
543
    // construct transformation matrix
305
    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));
544
    cv::Vec3f ex = axis.cross(cv::Vec3f(0,0,1.0));
306
    ex = cv::normalize(ex);
545
    ex = cv::normalize(ex);
307
    cv::Vec3f ez = ex.cross(axis);
546
    cv::Vec3f ez = ex.cross(axis);
308
    ez = cv::normalize(ez);
547
    ez = cv::normalize(ez);
309
 
548
 
310
    cv::Mat RrMat(3, 3, CV_32F);
549
    cv::Mat RrMat(3, 3, CV_32F);
311
    cv::Mat(ex).copyTo(RrMat.col(0));
550
    cv::Mat(ex).copyTo(RrMat.col(0));
312
    cv::Mat(axis).copyTo(RrMat.col(1));
551
    cv::Mat(axis).copyTo(RrMat.col(1));
313
    cv::Mat(ez).copyTo(RrMat.col(2));
552
    cv::Mat(ez).copyTo(RrMat.col(2));
314
 
553
 
315
    cal.Rr = cv::Matx33f(RrMat).t();
554
    cal.Rr = cv::Matx33f(RrMat).t();
316
    cal.Tr = -cv::Matx33f(RrMat).t()*point;
555
    cal.Tr = -cv::Matx33f(RrMat).t()*point;
317
    cal.rot_axis_error = rot_axis_error;
556
    cal.rot_axis_error = rot_axis_error;
318
 
557
 
319
    // Print to std::cout
558
    // Print to std::cout
320
    cal.print();
559
    cal.print();
321
 
560
 
322
    // save to (reentrant qsettings object)
561
    // save to (reentrant qsettings object)
323
    settings.setValue("calibration/parameters", QVariant::fromValue(cal));
562
    settings.setValue("calibration/parameters", QVariant::fromValue(cal));
324
 
563
 
325
    emit done();
564
    emit done();
326
 
565
 
327
}
566
}
328
 
567