← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/samples/cpp/tutorial_code/calib3d/camera_calibration/camera_calibration.cpp
 
1
#include <iostream>
 
2
#include <sstream>
 
3
#include <time.h>
 
4
#include <stdio.h>
 
5
 
 
6
#include <opencv2/core.hpp>
 
7
#include <opencv2/core/utility.hpp>
 
8
#include <opencv2/imgproc.hpp>
 
9
#include <opencv2/calib3d.hpp>
 
10
#include <opencv2/imgcodecs.hpp>
 
11
#include <opencv2/videoio.hpp>
 
12
#include <opencv2/highgui.hpp>
 
13
 
 
14
#ifndef _CRT_SECURE_NO_WARNINGS
 
15
# define _CRT_SECURE_NO_WARNINGS
 
16
#endif
 
17
 
 
18
using namespace cv;
 
19
using namespace std;
 
20
 
 
21
static void help()
 
22
{
 
23
    cout <<  "This is a camera calibration sample." << endl
 
24
         <<  "Usage: calibration configurationFile"  << endl
 
25
         <<  "Near the sample file you'll find the configuration file, which has detailed help of "
 
26
             "how to edit it.  It may be any OpenCV supported file format XML/YAML." << endl;
 
27
}
 
28
class Settings
 
29
{
 
30
public:
 
31
    Settings() : goodInput(false) {}
 
32
    enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
 
33
    enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST };
 
34
 
 
35
    void write(FileStorage& fs) const                        //Write serialization for this class
 
36
    {
 
37
        fs << "{"
 
38
                  << "BoardSize_Width"  << boardSize.width
 
39
                  << "BoardSize_Height" << boardSize.height
 
40
                  << "Square_Size"         << squareSize
 
41
                  << "Calibrate_Pattern" << patternToUse
 
42
                  << "Calibrate_NrOfFrameToUse" << nrFrames
 
43
                  << "Calibrate_FixAspectRatio" << aspectRatio
 
44
                  << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
 
45
                  << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint
 
46
 
 
47
                  << "Write_DetectedFeaturePoints" << writePoints
 
48
                  << "Write_extrinsicParameters"   << writeExtrinsics
 
49
                  << "Write_outputFileName"  << outputFileName
 
50
 
 
51
                  << "Show_UndistortedImage" << showUndistorsed
 
52
 
 
53
                  << "Input_FlipAroundHorizontalAxis" << flipVertical
 
54
                  << "Input_Delay" << delay
 
55
                  << "Input" << input
 
56
           << "}";
 
57
    }
 
58
    void read(const FileNode& node)                          //Read serialization for this class
 
59
    {
 
60
        node["BoardSize_Width" ] >> boardSize.width;
 
61
        node["BoardSize_Height"] >> boardSize.height;
 
62
        node["Calibrate_Pattern"] >> patternToUse;
 
63
        node["Square_Size"]  >> squareSize;
 
64
        node["Calibrate_NrOfFrameToUse"] >> nrFrames;
 
65
        node["Calibrate_FixAspectRatio"] >> aspectRatio;
 
66
        node["Write_DetectedFeaturePoints"] >> writePoints;
 
67
        node["Write_extrinsicParameters"] >> writeExtrinsics;
 
68
        node["Write_outputFileName"] >> outputFileName;
 
69
        node["Calibrate_AssumeZeroTangentialDistortion"] >> calibZeroTangentDist;
 
70
        node["Calibrate_FixPrincipalPointAtTheCenter"] >> calibFixPrincipalPoint;
 
71
        node["Calibrate_UseFisheyeModel"] >> useFisheye;
 
72
        node["Input_FlipAroundHorizontalAxis"] >> flipVertical;
 
73
        node["Show_UndistortedImage"] >> showUndistorsed;
 
74
        node["Input"] >> input;
 
75
        node["Input_Delay"] >> delay;
 
76
        validate();
 
77
    }
 
78
    void validate()
 
79
    {
 
80
        goodInput = true;
 
81
        if (boardSize.width <= 0 || boardSize.height <= 0)
 
82
        {
 
83
            cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << endl;
 
84
            goodInput = false;
 
85
        }
 
86
        if (squareSize <= 10e-6)
 
87
        {
 
88
            cerr << "Invalid square size " << squareSize << endl;
 
89
            goodInput = false;
 
90
        }
 
91
        if (nrFrames <= 0)
 
92
        {
 
93
            cerr << "Invalid number of frames " << nrFrames << endl;
 
94
            goodInput = false;
 
95
        }
 
96
 
 
97
        if (input.empty())      // Check for valid input
 
98
                inputType = INVALID;
 
99
        else
 
100
        {
 
101
            if (input[0] >= '0' && input[0] <= '9')
 
102
            {
 
103
                stringstream ss(input);
 
104
                ss >> cameraID;
 
105
                inputType = CAMERA;
 
106
            }
 
107
            else
 
108
            {
 
109
                if (readStringList(input, imageList))
 
110
                {
 
111
                    inputType = IMAGE_LIST;
 
112
                    nrFrames = (nrFrames < (int)imageList.size()) ? nrFrames : (int)imageList.size();
 
113
                }
 
114
                else
 
115
                    inputType = VIDEO_FILE;
 
116
            }
 
117
            if (inputType == CAMERA)
 
118
                inputCapture.open(cameraID);
 
119
            if (inputType == VIDEO_FILE)
 
120
                inputCapture.open(input);
 
121
            if (inputType != IMAGE_LIST && !inputCapture.isOpened())
 
122
                    inputType = INVALID;
 
123
        }
 
124
        if (inputType == INVALID)
 
125
        {
 
126
            cerr << " Input does not exist: " << input;
 
127
            goodInput = false;
 
128
        }
 
129
 
 
130
        flag = CALIB_FIX_K4 | CALIB_FIX_K5;
 
131
        if(calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
 
132
        if(calibZeroTangentDist)   flag |= CALIB_ZERO_TANGENT_DIST;
 
133
        if(aspectRatio)            flag |= CALIB_FIX_ASPECT_RATIO;
 
134
 
 
135
        if (useFisheye) {
 
136
            // the fisheye model has its own enum, so overwrite the flags
 
137
            flag = fisheye::CALIB_FIX_SKEW | fisheye::CALIB_RECOMPUTE_EXTRINSIC |
 
138
                   // fisheye::CALIB_FIX_K1 |
 
139
                   fisheye::CALIB_FIX_K2 | fisheye::CALIB_FIX_K3 | fisheye::CALIB_FIX_K4;
 
140
        }
 
141
 
 
142
        calibrationPattern = NOT_EXISTING;
 
143
        if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD;
 
144
        if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID;
 
145
        if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID;
 
146
        if (calibrationPattern == NOT_EXISTING)
 
147
        {
 
148
            cerr << " Camera calibration mode does not exist: " << patternToUse << endl;
 
149
            goodInput = false;
 
150
        }
 
151
        atImageList = 0;
 
152
 
 
153
    }
 
154
    Mat nextImage()
 
155
    {
 
156
        Mat result;
 
157
        if( inputCapture.isOpened() )
 
158
        {
 
159
            Mat view0;
 
160
            inputCapture >> view0;
 
161
            view0.copyTo(result);
 
162
        }
 
163
        else if( atImageList < imageList.size() )
 
164
            result = imread(imageList[atImageList++], IMREAD_COLOR);
 
165
 
 
166
        return result;
 
167
    }
 
168
 
 
169
    static bool readStringList( const string& filename, vector<string>& l )
 
170
    {
 
171
        l.clear();
 
172
        FileStorage fs(filename, FileStorage::READ);
 
173
        if( !fs.isOpened() )
 
174
            return false;
 
175
        FileNode n = fs.getFirstTopLevelNode();
 
176
        if( n.type() != FileNode::SEQ )
 
177
            return false;
 
178
        FileNodeIterator it = n.begin(), it_end = n.end();
 
179
        for( ; it != it_end; ++it )
 
180
            l.push_back((string)*it);
 
181
        return true;
 
182
    }
 
183
public:
 
184
    Size boardSize;              // The size of the board -> Number of items by width and height
 
185
    Pattern calibrationPattern;  // One of the Chessboard, circles, or asymmetric circle pattern
 
186
    float squareSize;            // The size of a square in your defined unit (point, millimeter,etc).
 
187
    int nrFrames;                // The number of frames to use from the input for calibration
 
188
    float aspectRatio;           // The aspect ratio
 
189
    int delay;                   // In case of a video input
 
190
    bool writePoints;            // Write detected feature points
 
191
    bool writeExtrinsics;        // Write extrinsic parameters
 
192
    bool calibZeroTangentDist;   // Assume zero tangential distortion
 
193
    bool calibFixPrincipalPoint; // Fix the principal point at the center
 
194
    bool flipVertical;           // Flip the captured images around the horizontal axis
 
195
    string outputFileName;       // The name of the file where to write
 
196
    bool showUndistorsed;        // Show undistorted images after calibration
 
197
    string input;                // The input ->
 
198
    bool useFisheye;             // use fisheye camera model for calibration
 
199
 
 
200
    int cameraID;
 
201
    vector<string> imageList;
 
202
    size_t atImageList;
 
203
    VideoCapture inputCapture;
 
204
    InputType inputType;
 
205
    bool goodInput;
 
206
    int flag;
 
207
 
 
208
private:
 
209
    string patternToUse;
 
210
 
 
211
 
 
212
};
 
213
 
 
214
static inline void read(const FileNode& node, Settings& x, const Settings& default_value = Settings())
 
215
{
 
216
    if(node.empty())
 
217
        x = default_value;
 
218
    else
 
219
        x.read(node);
 
220
}
 
221
 
 
222
static inline void write(FileStorage& fs, const String&, const Settings& s )
 
223
{
 
224
    s.write(fs);
 
225
}
 
226
 
 
227
enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
 
228
 
 
229
bool runCalibrationAndSave(Settings& s, Size imageSize, Mat&  cameraMatrix, Mat& distCoeffs,
 
230
                           vector<vector<Point2f> > imagePoints );
 
231
 
 
232
int main(int argc, char* argv[])
 
233
{
 
234
    help();
 
235
 
 
236
    //! [file_read]
 
237
    Settings s;
 
238
    const string inputSettingsFile = argc > 1 ? argv[1] : "default.xml";
 
239
    FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
 
240
    if (!fs.isOpened())
 
241
    {
 
242
        cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
 
243
        return -1;
 
244
    }
 
245
    fs["Settings"] >> s;
 
246
    fs.release();                                         // close Settings file
 
247
    //! [file_read]
 
248
 
 
249
    //FileStorage fout("settings.yml", FileStorage::WRITE); // write config as YAML
 
250
    //fout << "Settings" << s;
 
251
 
 
252
    if (!s.goodInput)
 
253
    {
 
254
        cout << "Invalid input detected. Application stopping. " << endl;
 
255
        return -1;
 
256
    }
 
257
 
 
258
    vector<vector<Point2f> > imagePoints;
 
259
    Mat cameraMatrix, distCoeffs;
 
260
    Size imageSize;
 
261
    int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
 
262
    clock_t prevTimestamp = 0;
 
263
    const Scalar RED(0,0,255), GREEN(0,255,0);
 
264
    const char ESC_KEY = 27;
 
265
 
 
266
    //! [get_input]
 
267
    for(;;)
 
268
    {
 
269
        Mat view;
 
270
        bool blinkOutput = false;
 
271
 
 
272
        view = s.nextImage();
 
273
 
 
274
        //-----  If no more image, or got enough, then stop calibration and show result -------------
 
275
        if( mode == CAPTURING && imagePoints.size() >= (size_t)s.nrFrames )
 
276
        {
 
277
          if( runCalibrationAndSave(s, imageSize,  cameraMatrix, distCoeffs, imagePoints))
 
278
              mode = CALIBRATED;
 
279
          else
 
280
              mode = DETECTION;
 
281
        }
 
282
        if(view.empty())          // If there are no more images stop the loop
 
283
        {
 
284
            // if calibration threshold was not reached yet, calibrate now
 
285
            if( mode != CALIBRATED && !imagePoints.empty() )
 
286
                runCalibrationAndSave(s, imageSize,  cameraMatrix, distCoeffs, imagePoints);
 
287
            break;
 
288
        }
 
289
        //! [get_input]
 
290
 
 
291
        imageSize = view.size();  // Format input image.
 
292
        if( s.flipVertical )    flip( view, view, 0 );
 
293
 
 
294
        //! [find_pattern]
 
295
        vector<Point2f> pointBuf;
 
296
 
 
297
        bool found;
 
298
 
 
299
        int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
 
300
 
 
301
        if(!s.useFisheye) {
 
302
            // fast check erroneously fails with high distortions like fisheye
 
303
            chessBoardFlags |= CALIB_CB_FAST_CHECK;
 
304
        }
 
305
 
 
306
        switch( s.calibrationPattern ) // Find feature points on the input format
 
307
        {
 
308
        case Settings::CHESSBOARD:
 
309
            found = findChessboardCorners( view, s.boardSize, pointBuf, chessBoardFlags);
 
310
            break;
 
311
        case Settings::CIRCLES_GRID:
 
312
            found = findCirclesGrid( view, s.boardSize, pointBuf );
 
313
            break;
 
314
        case Settings::ASYMMETRIC_CIRCLES_GRID:
 
315
            found = findCirclesGrid( view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID );
 
316
            break;
 
317
        default:
 
318
            found = false;
 
319
            break;
 
320
        }
 
321
        //! [find_pattern]
 
322
        //! [pattern_found]
 
323
        if ( found)                // If done with success,
 
324
        {
 
325
              // improve the found corners' coordinate accuracy for chessboard
 
326
                if( s.calibrationPattern == Settings::CHESSBOARD)
 
327
                {
 
328
                    Mat viewGray;
 
329
                    cvtColor(view, viewGray, COLOR_BGR2GRAY);
 
330
                    cornerSubPix( viewGray, pointBuf, Size(11,11),
 
331
                        Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));
 
332
                }
 
333
 
 
334
                if( mode == CAPTURING &&  // For camera only take new samples after delay time
 
335
                    (!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC) )
 
336
                {
 
337
                    imagePoints.push_back(pointBuf);
 
338
                    prevTimestamp = clock();
 
339
                    blinkOutput = s.inputCapture.isOpened();
 
340
                }
 
341
 
 
342
                // Draw the corners.
 
343
                drawChessboardCorners( view, s.boardSize, Mat(pointBuf), found );
 
344
        }
 
345
        //! [pattern_found]
 
346
        //----------------------------- Output Text ------------------------------------------------
 
347
        //! [output_text]
 
348
        string msg = (mode == CAPTURING) ? "100/100" :
 
349
                      mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
 
350
        int baseLine = 0;
 
351
        Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
 
352
        Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10);
 
353
 
 
354
        if( mode == CAPTURING )
 
355
        {
 
356
            if(s.showUndistorsed)
 
357
                msg = format( "%d/%d Undist", (int)imagePoints.size(), s.nrFrames );
 
358
            else
 
359
                msg = format( "%d/%d", (int)imagePoints.size(), s.nrFrames );
 
360
        }
 
361
 
 
362
        putText( view, msg, textOrigin, 1, 1, mode == CALIBRATED ?  GREEN : RED);
 
363
 
 
364
        if( blinkOutput )
 
365
            bitwise_not(view, view);
 
366
        //! [output_text]
 
367
        //------------------------- Video capture  output  undistorted ------------------------------
 
368
        //! [output_undistorted]
 
369
        if( mode == CALIBRATED && s.showUndistorsed )
 
370
        {
 
371
            Mat temp = view.clone();
 
372
            undistort(temp, view, cameraMatrix, distCoeffs);
 
373
        }
 
374
        //! [output_undistorted]
 
375
        //------------------------------ Show image and check for input commands -------------------
 
376
        //! [await_input]
 
377
        imshow("Image View", view);
 
378
        char key = (char)waitKey(s.inputCapture.isOpened() ? 50 : s.delay);
 
379
 
 
380
        if( key  == ESC_KEY )
 
381
            break;
 
382
 
 
383
        if( key == 'u' && mode == CALIBRATED )
 
384
           s.showUndistorsed = !s.showUndistorsed;
 
385
 
 
386
        if( s.inputCapture.isOpened() && key == 'g' )
 
387
        {
 
388
            mode = CAPTURING;
 
389
            imagePoints.clear();
 
390
        }
 
391
        //! [await_input]
 
392
    }
 
393
 
 
394
    // -----------------------Show the undistorted image for the image list ------------------------
 
395
    //! [show_results]
 
396
    if( s.inputType == Settings::IMAGE_LIST && s.showUndistorsed )
 
397
    {
 
398
        Mat view, rview, map1, map2;
 
399
 
 
400
        if (s.useFisheye)
 
401
        {
 
402
            Mat newCamMat;
 
403
            fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix, distCoeffs, imageSize,
 
404
                                                                Matx33d::eye(), newCamMat, 1);
 
405
            fisheye::initUndistortRectifyMap(cameraMatrix, distCoeffs, Matx33d::eye(), newCamMat, imageSize,
 
406
                                             CV_16SC2, map1, map2);
 
407
        }
 
408
        else
 
409
        {
 
410
            initUndistortRectifyMap(
 
411
                cameraMatrix, distCoeffs, Mat(),
 
412
                getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize,
 
413
                CV_16SC2, map1, map2);
 
414
        }
 
415
 
 
416
        for(size_t i = 0; i < s.imageList.size(); i++ )
 
417
        {
 
418
            view = imread(s.imageList[i], 1);
 
419
            if(view.empty())
 
420
                continue;
 
421
            remap(view, rview, map1, map2, INTER_LINEAR);
 
422
            imshow("Image View", rview);
 
423
            char c = (char)waitKey();
 
424
            if( c  == ESC_KEY || c == 'q' || c == 'Q' )
 
425
                break;
 
426
        }
 
427
    }
 
428
    //! [show_results]
 
429
 
 
430
    return 0;
 
431
}
 
432
 
 
433
//! [compute_errors]
 
434
static double computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
 
435
                                         const vector<vector<Point2f> >& imagePoints,
 
436
                                         const vector<Mat>& rvecs, const vector<Mat>& tvecs,
 
437
                                         const Mat& cameraMatrix , const Mat& distCoeffs,
 
438
                                         vector<float>& perViewErrors, bool fisheye)
 
439
{
 
440
    vector<Point2f> imagePoints2;
 
441
    size_t totalPoints = 0;
 
442
    double totalErr = 0, err;
 
443
    perViewErrors.resize(objectPoints.size());
 
444
 
 
445
    for(size_t i = 0; i < objectPoints.size(); ++i )
 
446
    {
 
447
        if (fisheye)
 
448
        {
 
449
            fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i], tvecs[i], cameraMatrix,
 
450
                                   distCoeffs);
 
451
        }
 
452
        else
 
453
        {
 
454
            projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2);
 
455
        }
 
456
        err = norm(imagePoints[i], imagePoints2, NORM_L2);
 
457
 
 
458
        size_t n = objectPoints[i].size();
 
459
        perViewErrors[i] = (float) std::sqrt(err*err/n);
 
460
        totalErr        += err*err;
 
461
        totalPoints     += n;
 
462
    }
 
463
 
 
464
    return std::sqrt(totalErr/totalPoints);
 
465
}
 
466
//! [compute_errors]
 
467
//! [board_corners]
 
468
static void calcBoardCornerPositions(Size boardSize, float squareSize, vector<Point3f>& corners,
 
469
                                     Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
 
470
{
 
471
    corners.clear();
 
472
 
 
473
    switch(patternType)
 
474
    {
 
475
    case Settings::CHESSBOARD:
 
476
    case Settings::CIRCLES_GRID:
 
477
        for( int i = 0; i < boardSize.height; ++i )
 
478
            for( int j = 0; j < boardSize.width; ++j )
 
479
                corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
 
480
        break;
 
481
 
 
482
    case Settings::ASYMMETRIC_CIRCLES_GRID:
 
483
        for( int i = 0; i < boardSize.height; i++ )
 
484
            for( int j = 0; j < boardSize.width; j++ )
 
485
                corners.push_back(Point3f((2*j + i % 2)*squareSize, i*squareSize, 0));
 
486
        break;
 
487
    default:
 
488
        break;
 
489
    }
 
490
}
 
491
//! [board_corners]
 
492
static bool runCalibration( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
 
493
                            vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs, vector<Mat>& tvecs,
 
494
                            vector<float>& reprojErrs,  double& totalAvgErr)
 
495
{
 
496
    //! [fixed_aspect]
 
497
    cameraMatrix = Mat::eye(3, 3, CV_64F);
 
498
    if( s.flag & CALIB_FIX_ASPECT_RATIO )
 
499
        cameraMatrix.at<double>(0,0) = s.aspectRatio;
 
500
    //! [fixed_aspect]
 
501
    if (s.useFisheye) {
 
502
        distCoeffs = Mat::zeros(4, 1, CV_64F);
 
503
    } else {
 
504
        distCoeffs = Mat::zeros(8, 1, CV_64F);
 
505
    }
 
506
 
 
507
    vector<vector<Point3f> > objectPoints(1);
 
508
    calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0], s.calibrationPattern);
 
509
 
 
510
    objectPoints.resize(imagePoints.size(),objectPoints[0]);
 
511
 
 
512
    //Find intrinsic and extrinsic camera parameters
 
513
    double rms;
 
514
 
 
515
    if (s.useFisheye) {
 
516
        Mat _rvecs, _tvecs;
 
517
        rms = fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, _rvecs,
 
518
                                 _tvecs, s.flag);
 
519
 
 
520
        rvecs.reserve(_rvecs.rows);
 
521
        tvecs.reserve(_tvecs.rows);
 
522
        for(int i = 0; i < int(objectPoints.size()); i++){
 
523
            rvecs.push_back(_rvecs.row(i));
 
524
            tvecs.push_back(_tvecs.row(i));
 
525
        }
 
526
    } else {
 
527
        rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs,
 
528
                              s.flag);
 
529
    }
 
530
 
 
531
    cout << "Re-projection error reported by calibrateCamera: "<< rms << endl;
 
532
 
 
533
    bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
 
534
 
 
535
    totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs, tvecs, cameraMatrix,
 
536
                                            distCoeffs, reprojErrs, s.useFisheye);
 
537
 
 
538
    return ok;
 
539
}
 
540
 
 
541
// Print camera parameters to the output file
 
542
static void saveCameraParams( Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
 
543
                              const vector<Mat>& rvecs, const vector<Mat>& tvecs,
 
544
                              const vector<float>& reprojErrs, const vector<vector<Point2f> >& imagePoints,
 
545
                              double totalAvgErr )
 
546
{
 
547
    FileStorage fs( s.outputFileName, FileStorage::WRITE );
 
548
 
 
549
    time_t tm;
 
550
    time( &tm );
 
551
    struct tm *t2 = localtime( &tm );
 
552
    char buf[1024];
 
553
    strftime( buf, sizeof(buf), "%c", t2 );
 
554
 
 
555
    fs << "calibration_time" << buf;
 
556
 
 
557
    if( !rvecs.empty() || !reprojErrs.empty() )
 
558
        fs << "nr_of_frames" << (int)std::max(rvecs.size(), reprojErrs.size());
 
559
    fs << "image_width" << imageSize.width;
 
560
    fs << "image_height" << imageSize.height;
 
561
    fs << "board_width" << s.boardSize.width;
 
562
    fs << "board_height" << s.boardSize.height;
 
563
    fs << "square_size" << s.squareSize;
 
564
 
 
565
    if( s.flag & CALIB_FIX_ASPECT_RATIO )
 
566
        fs << "fix_aspect_ratio" << s.aspectRatio;
 
567
 
 
568
    if (s.flag)
 
569
    {
 
570
        if (s.useFisheye)
 
571
        {
 
572
            sprintf(buf, "flags:%s%s%s%s%s%s",
 
573
                     s.flag & fisheye::CALIB_FIX_SKEW ? " +fix_skew" : "",
 
574
                     s.flag & fisheye::CALIB_FIX_K1 ? " +fix_k1" : "",
 
575
                     s.flag & fisheye::CALIB_FIX_K2 ? " +fix_k2" : "",
 
576
                     s.flag & fisheye::CALIB_FIX_K3 ? " +fix_k3" : "",
 
577
                     s.flag & fisheye::CALIB_FIX_K4 ? " +fix_k4" : "",
 
578
                     s.flag & fisheye::CALIB_RECOMPUTE_EXTRINSIC ? " +recompute_extrinsic" : "");
 
579
        }
 
580
        else
 
581
        {
 
582
            sprintf(buf, "flags:%s%s%s%s",
 
583
                     s.flag & CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess" : "",
 
584
                     s.flag & CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "",
 
585
                     s.flag & CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point" : "",
 
586
                     s.flag & CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : "");
 
587
        }
 
588
        cvWriteComment(*fs, buf, 0);
 
589
    }
 
590
 
 
591
    fs << "flags" << s.flag;
 
592
 
 
593
    fs << "fisheye_model" << s.useFisheye;
 
594
 
 
595
    fs << "camera_matrix" << cameraMatrix;
 
596
    fs << "distortion_coefficients" << distCoeffs;
 
597
 
 
598
    fs << "avg_reprojection_error" << totalAvgErr;
 
599
    if (s.writeExtrinsics && !reprojErrs.empty())
 
600
        fs << "per_view_reprojection_errors" << Mat(reprojErrs);
 
601
 
 
602
    if(s.writeExtrinsics && !rvecs.empty() && !tvecs.empty() )
 
603
    {
 
604
        CV_Assert(rvecs[0].type() == tvecs[0].type());
 
605
        Mat bigmat((int)rvecs.size(), 6, rvecs[0].type());
 
606
        for( size_t i = 0; i < rvecs.size(); i++ )
 
607
        {
 
608
            Mat r = bigmat(Range(int(i), int(i+1)), Range(0,3));
 
609
            Mat t = bigmat(Range(int(i), int(i+1)), Range(3,6));
 
610
 
 
611
            CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
 
612
            CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
 
613
            //*.t() is MatExpr (not Mat) so we can use assignment operator
 
614
            r = rvecs[i].t();
 
615
            t = tvecs[i].t();
 
616
        }
 
617
        //cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
 
618
        fs << "extrinsic_parameters" << bigmat;
 
619
    }
 
620
 
 
621
    if(s.writePoints && !imagePoints.empty() )
 
622
    {
 
623
        Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2);
 
624
        for( size_t i = 0; i < imagePoints.size(); i++ )
 
625
        {
 
626
            Mat r = imagePtMat.row(int(i)).reshape(2, imagePtMat.cols);
 
627
            Mat imgpti(imagePoints[i]);
 
628
            imgpti.copyTo(r);
 
629
        }
 
630
        fs << "image_points" << imagePtMat;
 
631
    }
 
632
}
 
633
 
 
634
//! [run_and_save]
 
635
bool runCalibrationAndSave(Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
 
636
                           vector<vector<Point2f> > imagePoints)
 
637
{
 
638
    vector<Mat> rvecs, tvecs;
 
639
    vector<float> reprojErrs;
 
640
    double totalAvgErr = 0;
 
641
 
 
642
    bool ok = runCalibration(s, imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, reprojErrs,
 
643
                             totalAvgErr);
 
644
    cout << (ok ? "Calibration succeeded" : "Calibration failed")
 
645
         << ". avg re projection error = " << totalAvgErr << endl;
 
646
 
 
647
    if (ok)
 
648
        saveCameraParams(s, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, imagePoints,
 
649
                         totalAvgErr);
 
650
    return ok;
 
651
}
 
652
//! [run_and_save]