Subversion Repositories seema-scanner

Rev

Rev 23 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 23 Rev 25
Line 1... Line 1...
1 
#include "MVCalibrationDialog.h"
 1 
#include "SMCalibrator.h"
2 
#include "ui_MVCalibrationDialog.h"
 - 
 
3 
 
 2 
 
4 
#include <QListWidgetItem>
 - 
 
5 
#include <QSettings>
 3 
#include <QSettings>
6 
#include <QCloseEvent>
 - 
 
7 
 
 4 
 
8 
 
 - 
 
9 
MVCalibrationDialog::MVCalibrationDialog(QWidget *parent) : QDialog(parent), ui(new Ui::MVCalibrationDialog), reviewMode(false){
 - 
 
10 
    ui->setupUi(this);
 - 
 
11 
 
 - 
 
12 
 
 - 
 
13 
    // Set up capture worker on separate thread
 - 
 
14 
    captureWorker = new MVCaptureWorker();
 - 
 
15 
    captureWorkerThread = new QThread(this);
 - 
 
16 
    captureWorkerThread->setObjectName("captureWorkerThread");
 - 
 
17 
 
 - 
 
18 
    captureWorker->moveToThread(captureWorkerThread);
 - 
 
19 
    captureWorkerThread->start();
 - 
 
20 
 
 - 
 
21 
 
 - 
 
22 
    // Connect program logic
 - 
 
23 
    qRegisterMetaType<cv::Mat>("cv::Mat");
 - 
 
24 
    qRegisterMetaType< std::vector<cv::Mat> >("std::vector<cv::Mat>");
 - 
 
25 
 
 - 
 
26 
    connect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(receiveNewFrameSet(std::vector<cv::Mat>)));
 - 
 
27 
 
 - 
 
28 
    // Start capturing
 - 
 
29 
    QMetaObject::invokeMethod(captureWorker, "setup", Q_ARG(bool, false));
 - 
 
30 
    QMetaObject::invokeMethod(captureWorker, "doWork");
 - 
 
31 
 
 - 
 
32 
}
 - 
 
33 
 
 - 
 
34 
void MVCalibrationDialog::receiveNewFrameSet(std::vector<cv::Mat> frameSet){
 - 
 
35 
    ui->videoWidget0->showImageCV(frameSet[0]);
 - 
 
36 
    ui->videoWidget1->showImageCV(frameSet[1]);
 - 
 
37 
}
 - 
 
38 
 
 - 
 
39 
void MVCalibrationDialog::addFrameSetToCalibration(std::vector<cv::Mat> frameSet){
 5 
void SMCalibrator::performCalibration(std::vector< CalibrationSet > calibrationData){
40 
 
 - 
 
41 
    // Disconnect from further frame sets
 - 
 
42 
    disconnect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(addFrameSetToCalibration(std::vector<cv::Mat>)));
 - 
 
43 
 
 - 
 
44 
    calibrationSets.push_back(frameSet);
 - 
 
45 
 
 - 
 
46 
    // Add identifier to list
 - 
 
47 
    QListWidgetItem* item = new QListWidgetItem(QString("Set %1").arg(ui->listWidget->count()), ui->listWidget);
 - 
 
48 
    item->setFlags(item->flags() | Qt::ItemIsUserCheckable);
 - 
 
49 
    item->setCheckState(Qt::Checked);
 - 
 
50 
 
 - 
 
51 
    // Enable calibration
 - 
 
52 
    if(calibrationSets.size() >= 2)
 - 
 
53 
        ui->calibrateButton->setEnabled(true);
 - 
 
54 
 
 - 
 
55 
}
 - 
 
56 
 
 - 
 
57 
void MVCalibrationDialog::on_snapButton_clicked(){
 - 
 
58 
 
 - 
 
59 
    if(reviewMode){
 - 
 
60 
        ui->snapButton->setText("Snap");
 - 
 
61 
        connect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(receiveNewFrameSet(std::vector<cv::Mat>)));
 - 
 
62 
        reviewMode = false;
 - 
 
63 
        return;
 - 
 
64 
    }
 - 
 
65 
 
 - 
 
66 
    // Redirect one frame set into calibration
 - 
 
67 
    connect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(addFrameSetToCalibration(std::vector<cv::Mat>)));
 - 
 
68 
 
 - 
 
69 
}
 - 
 
70 
 
 - 
 
71 
void MVCalibrationDialog::on_listWidget_clicked(const QModelIndex &index){
 - 
 
72 
 
 - 
 
73 
 
 - 
 
74 
 
 - 
 
75 
}
 - 
 
76 
 
 - 
 
77 
void MVCalibrationDialog::on_calibrateButton_clicked(){
 - 
 
78 
 
 - 
 
79 
    reviewMode = true;
 - 
 
80 
    ui->snapButton->setText("Capture");
 - 
 
81 
    QCoreApplication::processEvents();
 - 
 
82 
 
 - 
 
83 
    // Disconnect form live feed for review mode
 - 
 
84 
    disconnect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(receiveNewFrameSet(std::vector<cv::Mat>)));
 - 
 
85 
 
 6 
 
86 
    // Number of saddle points on calibration pattern
 7 
    // Number of saddle points on calibration pattern
87 
    cv::Size patternSize(10, 9);
 8 
    cv::Size patternSize(10, 9);
88 
 
 9 
 
89 
    int nSets = calibrationSets.size();
 10 
    int nSets = calibrationData.size();
90 
 
 11 
 
91 
    std::vector< std::vector< std::vector<cv::Point2f> > > qc(2);
 12 
    std::vector< std::vector< std::vector<cv::Point2f> > > qc(2);
92 
 
 13 
 
93 
    calibrationResults = calibrationSets;
 - 
 
94 
 
 - 
 
95 
    // Loop through calibration sets
 14 
    // Loop through calibration sets
96 
    for(int i=0; i<nSets; i++){
 15 
    for(int i=0; i<nSets; i++){
97 
 
 16 
 
- 
 
 17 
        CalibrationSet calibrationSetI = calibrationData[i];
- 
 
 18 
 
98 
        if(ui->listWidget->item(i)->checkState() != Qt::Checked){
 19 
        if(!calibrationSetI.checked)
99 
            continue;
 20 
            continue;
100 
        }
 - 
 
101 
 
 21 
 
- 
 
 22 
        // Camera 0
102 
        std::vector<cv::Mat> calibrationSetI = calibrationSets[i];
 23 
        std::vector<cv::Point2f> qci0;
103 
        std::vector< std::vector<cv::Point2f> > qci;
 24 
        // Extract checker corners
- 
 
 25 
        bool success0 = cv::findChessboardCorners(calibrationSetI.frame0, patternSize, qci0, cv::CALIB_CB_ADAPTIVE_THRESH);
- 
 
 26 
        if(success0){
- 
 
 27 
            cv::cornerSubPix(calibrationSetI.frame0, qci0, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));
- 
 
 28 
            // Draw colored chessboard
- 
 
 29 
            cv::cvtColor(calibrationSetI.frame0, calibrationSetI.frame0Result, CV_GRAY2RGB);
- 
 
 30 
            cv::drawChessboardCorners(calibrationSetI.frame0Result, patternSize, qci0, success0);
- 
 
 31 
        }
104 
 
 32 
 
105 
        // Loop through cameras
 33 
        // Camera 1
106 
        for(unsigned int j=0; j<calibrationSetI.size(); j++){
 - 
 
107 
            std::vector<cv::Point2f> qcij;
 34 
        std::vector<cv::Point2f> qci1;
108 
            // Extract checker corners
 35 
        // Extract checker corners
109 
            bool success = cv::findChessboardCorners(calibrationSetI[j], patternSize, qcij, cv::CALIB_CB_ADAPTIVE_THRESH);
 36 
        bool success1 = cv::findChessboardCorners(calibrationSetI.frame1, patternSize, qci1, cv::CALIB_CB_ADAPTIVE_THRESH);
110 
            if(success){
 37 
        if(success1){
111 
                cv::cornerSubPix(calibrationSetI[j], qcij, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));
 38 
            cv::cornerSubPix(calibrationSetI.frame1, qci1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));
112 
                // Draw colored chessboard
 39 
            // Draw colored chessboard
113 
                cv::Mat calibrationSetIJColor;
 - 
 
114 
                cv::cvtColor(calibrationSetI[j], calibrationSetIJColor, CV_GRAY2RGB);
 40 
            cv::cvtColor(calibrationSetI.frame1, calibrationSetI.frame1Result, CV_GRAY2RGB);
115 
                cv::drawChessboardCorners(calibrationSetIJColor, patternSize, qcij, success);
 41 
            cv::drawChessboardCorners(calibrationSetI.frame1Result, patternSize, qci1, success1);
116 
                calibrationResults[i][j] = calibrationSetIJColor;
 - 
 
117 
            } else {
 - 
 
118 
                ui->listWidget->item(i)->setCheckState(Qt::Unchecked);
 - 
 
119 
            }
 - 
 
120 
            qci.push_back(qcij);
 - 
 
121 
        }
 42 
        }
122 
 
 43 
 
- 
 
 44 
        calibrationSetI.success = success0 && success1;
- 
 
 45 
 
123 
        // Add to whole set
 46 
        // Add to whole set
124 
        if(!qci[0].empty() && !qci[1].empty() ){
 47 
        if(calibrationSetI.success){
125 
            qc[0].push_back(qci[0]);
 48 
            qc[0].push_back(qci0);
126 
            qc[1].push_back(qci[1]);
 49 
            qc[1].push_back(qci1);
127 
        }
 50 
        }
128 
 
 51 
 
129 
        // Show results
 52 
        // Show results
130 
        ui->listWidget->setCurrentRow(i);
 - 
 
131 
        QCoreApplication::processEvents();
 53 
        emit newFrameProcessed(i);
132 
    }
 54 
    }
133 
 
 55 
 
134 
    int nValidSets = qc[0].size();
 56 
    int nValidSets = qc[0].size();
135 
    if(nValidSets <= 2){
 57 
    if(nValidSets <= 2){
136 
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
 58 
        std::cerr << "Not enough valid calibration sequences!" << std::endl;
Line 145... Line 67...
145 
    std::vector< std::vector<cv::Point3f> > Q;
 67 
    std::vector< std::vector<cv::Point3f> > Q;
146 
    for(int i=0; i<qc[0].size(); i++)
 68 
    for(int i=0; i<qc[0].size(); i++)
147 
        Q.push_back(Qi);
 69 
        Q.push_back(Qi);
148 
 
 70 
 
149 
    // calibrate the cameras
 71 
    // calibrate the cameras
150 
//    cv::Size frameSize(calibrationSets[0][0].cols, calibrationSets[0][0].rows);
 72 
    cv::Size frameSize(calibrationData[0].frame0.cols, calibrationData[0].frame0.rows);
151 
    cv::Size frameSize(640, 480);
 73 
//    cv::Size frameSize(640, 480);
152 
    int flags = 0; //cv::CALIB_FIX_K3 + cv::CALIB_FIX_INTRINSIC;
 74 
    int flags = 0; //cv::CALIB_FIX_K3 + cv::CALIB_FIX_INTRINSIC;
153 
 
 75 
 
154 
    std::vector< std::vector<cv::Point2f> > qc0 = qc[0];
 76 
    std::vector< std::vector<cv::Point2f> > qc0 = qc[0];
155 
    std::vector< std::vector<cv::Point2f> > qc1 = qc[1];
 77 
    std::vector< std::vector<cv::Point2f> > qc1 = qc[1];
156 
 
 78 
 
157 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
 79 
    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;
- 
 
 80 
    cv::Mat K0, k0;
158 
    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0);
 81 
    double cam0_error = cv::calibrateCamera(Q, qc0, frameSize, K0, k0, cam_rvecs0, cam_tvecs0);
159 
 
 82 
 
160 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
 83 
    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;
- 
 
 84 
    cv::Mat K1, k1;
161 
    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1);
 85 
    double cam1_error = cv::calibrateCamera(Q, qc1, frameSize, K1, k1, cam_rvecs1, cam_tvecs1);
162 
 
 86 
 
163 
    // stereo calibration (don't change K0, K1, k0, k1)
 87 
    // stereo calibration (don't change K0, K1, k0, k1)
164 
    int flags_stereo = flags + cv::CALIB_FIX_INTRINSIC;
 88 
    int flags_stereo = flags + cv::CALIB_FIX_INTRINSIC;
165 
    cv::Mat E, F, R1, T1;
 89 
    cv::Mat E, F, R1, T1;
166 
    cal.stereo_error = cv::stereoCalibrate(Q, qc[0], qc[1], cal.K0, cal.k0, cal.K1, cal.k1,
 90 
    double stereo_error = cv::stereoCalibrate(Q, qc[0], qc[1], K0, k0, K1, k1,
167 
                                              frameSize, R1, T1, E, F,
 91 
                                              frameSize, R1, T1, E, F,
168 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
 92 
                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),
169 
                                              flags_stereo);
 93 
                                              flags_stereo);
170 
 
 94 
 
171 
    cal.R1 = R1;
 - 
 
172 
    cal.T1 = T1;
 - 
 
173 
    cal.E = E;
 - 
 
174 
    cal.F = F;
 - 
 
175 
 
 - 
 
176 
    cal.print(std::cout);
 - 
 
177 
 
 - 
 
178 
    ui->saveButton->setEnabled(true);
 - 
 
179 
 
 - 
 
180 
}
 - 
 
181 
 
 - 
 
182 
 
 - 
 
183 
void MVCalibrationDialog::on_listWidget_currentRowChanged(int currentRow){
 - 
 
184 
 
 - 
 
185 
    reviewMode = true;
 - 
 
186 
    ui->snapButton->setText("Capture");
 - 
 
187 
 
 - 
 
188 
    // Disconnect form live feed for review mode
 - 
 
189 
    disconnect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(receiveNewFrameSet(std::vector<cv::Mat>)));
 - 
 
190 
 
 - 
 
191 
    unsigned int idx = ui->listWidget->currentRow();
 - 
 
192 
 
 - 
 
193 
    // Show result if available, otherwise captured image
 - 
 
194 
    if(!calibrationResults[idx][0].empty())
 - 
 
195 
        ui->videoWidget0->showImageCV(calibrationResults[idx][0]);
 - 
 
196 
    else
 - 
 
197 
        ui->videoWidget0->showImageCV(calibrationSets[idx][0]);
 - 
 
198 
 
 - 
 
199 
    if(!calibrationResults[idx][1].empty())
 - 
 
200 
        ui->videoWidget1->showImageCV(calibrationResults[idx][1]);
 - 
 
201 
    else
 - 
 
202 
        ui->videoWidget1->showImageCV(calibrationSets[idx][1]);
 - 
 
203 
}
 - 
 
204 
 
 - 
 
205 
void MVCalibrationDialog::closeEvent(QCloseEvent *event){
 - 
 
206 
 
 - 
 
207 
    connect(captureWorker, SIGNAL(finished()), captureWorker, SLOT(deleteLater()));
 - 
 
208 
    QMetaObject::invokeMethod(captureWorker, "stopWorking");
 - 
 
209 
    captureWorkerThread->quit();
 - 
 
210 
    captureWorkerThread->wait();
 - 
 
211 
//this->deleteLater();
 - 
 
212 
    event->accept();
 - 
 
213 
}
 - 
 
214 
 
 - 
 
215 
MVCalibrationDialog::~MVCalibrationDialog(){
 - 
 
216 
 
 - 
 
217 
//    QMetaObject::invokeMethod(captureWorker, "stopWorking");
 - 
 
218 
//    captureWorkerThread->quit();
 - 
 
219 
//    captureWorkerThread->wait();
 - 
 
220 
 
 - 
 
221 
//    delete captureWorker;
 - 
 
222 
    delete captureWorkerThread;
 - 
 
223 
    delete ui;
 - 
 
224 
}
 - 
 
225 
 
 - 
 
226 
 
 - 
 
227 
void MVCalibrationDialog::on_saveButton_clicked(){
 - 
 
228 
 
 - 
 
229 
//     Register as meta type and stream operators for use in QSettings
 - 
 
230 
//    qRegisterMetaTypeStreamOperators<MVCalibrationData>("MVCalibrationData");
 - 
 
231 
//    QSettings settings;
 - 
 
232 
//    settings.setValue("Calibration", QVariant::fromValue<MVCalibrationData>(cal));
 - 
 
233 
 
 - 
 
234 
    cal.save("calibration.xml");
 - 
 
235 
 
 - 
 
236 
    this->close();
 - 
 
237 
}
 95 
}