Subversion Repositories seema-scanner

Rev

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

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