surround-view/cameraCalibrator.cpp at 0.1 · sheeaza/surround-view · GitHub

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
#include "cameraCalibrator.h"
#include <QThread>
#include <QDebug>

#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>

#include <iostream>
#include <fstream>

#include "cameraparameter.h"

CameraCalibrator::CameraCalibrator(CameraParameter &p, QObject *parent)
    : QObject(parent)
{
    boardSize = p.boardSize;
    squareSize = p.squareSize;
    imgSize = p.imgSize;
}

CameraCalibrator::~CameraCalibrator()
{
}

void CameraCalibrator::calibrateImage(Mat &distorImage, Mat &undistortImage)
{
    cv::remap(distorImage, undistortImage, map1, map2, INTER_LINEAR);
}

void CameraCalibrator::initCalibMap()
{
    fisheye::initUndistortRectifyMap(intrinsic, distCoeffs, Matx33d::eye(),
                                     intrinsic, imgSize, CV_16SC2,
                                     map1, map2);
}

void CameraCalibrator::findAndDrawChessboard(cv::Mat *input)
{
    vector<Point2f> pointBuf;

    bool found = findChessboardCorners((*input), boardSize, pointBuf);
    if (found) {
        Mat viewGray;
        cvtColor((*input), viewGray, COLOR_RGB2GRAY);
        cornerSubPix(viewGray, pointBuf, Size(7,7), Size(-1,-1),
                     TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1));
        imagePoints.push_back(pointBuf);
        drawChessboardCorners((*input), boardSize, Mat(pointBuf), found);
    }
    emit isChessboardFound(found);
}

void CameraCalibrator::calibrate()
{
    Mat _rvecs, _tvecs;
    vector<Mat> rvecs, tvecs;

    intrinsic = Mat::eye(3, 3, CV_64F);
    distCoeffs = Mat::zeros(4, 1, CV_64F);

    vector<vector<Point3f> > objectPoints(1);

    objectPoints[0].clear();
    for( int i = 0; i < boardSize.height; ++i )
        for( int j = 0; j < boardSize.width; ++j )
            objectPoints[0].push_back(Point3f(j*squareSize, i*squareSize, 0));
    objectPoints.resize(imagePoints.size(), objectPoints[0]);

    int flags = 0;
    flags |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
    flags |= cv::fisheye::CALIB_CHECK_COND;
    flags |= cv::fisheye::CALIB_FIX_SKEW;
    fisheye::calibrate(objectPoints, imagePoints, imgSize,
                             intrinsic, distCoeffs, _rvecs, _tvecs,
                             flags);
    rvecs.reserve(_rvecs.rows);
    tvecs.reserve(_tvecs.rows);
    for(int i = 0; i < int(objectPoints.size()); i++){
        rvecs.push_back(_rvecs.row(i));
        tvecs.push_back(_tvecs.row(i));
    }

    bool ok = checkRange(intrinsic) && checkRange(distCoeffs);
    if (ok) {
        vector<float> reprojErrs;
        totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
                                                       rvecs, tvecs, intrinsic,
                                                       distCoeffs, reprojErrs);
        initCalibMap();
    }
    emit sendCalibResults(ok);
}

void CameraCalibrator::cleanPoints()
{
    imagePoints.clear();
}

double CameraCalibrator::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<float> &perViewErrors)
{
    vector<Point2f> imagePoints2;
    size_t totalPoints = 0;
    double totalErr = 0, err;
    perViewErrors.resize(objectPoints.size());

    for(size_t i = 0; i < objectPoints.size(); ++i )
    {
        fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i], tvecs[i], cameraMatrix,
                               distCoeffs);

        err = norm(imagePoints[i], imagePoints2, NORM_L2);

        size_t n = objectPoints[i].size();
        perViewErrors[i] = (float) std::sqrt(err*err/n);
        totalErr        += err*err;
        totalPoints     += n;
    }

    return std::sqrt(totalErr/totalPoints);
}