Subversion Repositories seema-scanner

#include "MVCalibrationDialog.h"

#include "ui_MVCalibrationDialog.h"

#include <QListWidgetItem>

#include <QSettings>

#include <QCloseEvent>

MVCalibrationDialog::MVCalibrationDialog(QWidget *parent) : QDialog(parent), ui(new Ui::MVCalibrationDialog), reviewMode(false){

    ui->setupUi(this);

    // Set up capture worker on separate thread

    captureWorker = new MVCaptureWorker();

    captureWorkerThread = new QThread(this);

    captureWorkerThread->setObjectName("captureWorkerThread");

    captureWorker->moveToThread(captureWorkerThread);

    captureWorkerThread->start();

    // Connect program logic

    qRegisterMetaType<cv::Mat>("cv::Mat");

    qRegisterMetaType< std::vector<cv::Mat> >("std::vector<cv::Mat>");

    connect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(receiveNewFrameSet(std::vector<cv::Mat>)));

    // Start capturing

    QMetaObject::invokeMethod(captureWorker, "setup", Q_ARG(bool, false));

    QMetaObject::invokeMethod(captureWorker, "doWork");

}

void MVCalibrationDialog::receiveNewFrameSet(std::vector<cv::Mat> frameSet){

    ui->videoWidget0->showImageCV(frameSet[0]);

    ui->videoWidget1->showImageCV(frameSet[1]);

}

void MVCalibrationDialog::addFrameSetToCalibration(std::vector<cv::Mat> frameSet){

    // Disconnect from further frame sets

    disconnect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(addFrameSetToCalibration(std::vector<cv::Mat>)));

    calibrationSets.push_back(frameSet);

    // Add identifier to list

    QListWidgetItem* item = new QListWidgetItem(QString("Set %1").arg(ui->listWidget->count()), ui->listWidget);

    item->setFlags(item->flags() | Qt::ItemIsUserCheckable);

    item->setCheckState(Qt::Checked);

    // Enable calibration

    if(calibrationSets.size() >= 2)

        ui->calibrateButton->setEnabled(true);

}

void MVCalibrationDialog::on_snapButton_clicked(){

    if(reviewMode){

        ui->snapButton->setText("Snap");

        connect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(receiveNewFrameSet(std::vector<cv::Mat>)));

        reviewMode = false;

        return;

    }

    // Redirect one frame set into calibration

    connect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(addFrameSetToCalibration(std::vector<cv::Mat>)));

}

void MVCalibrationDialog::on_listWidget_clicked(const QModelIndex &index){

}

void MVCalibrationDialog::on_calibrateButton_clicked(){

    reviewMode = true;

    ui->snapButton->setText("Capture");

    QCoreApplication::processEvents();

    // Disconnect form live feed for review mode

    disconnect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(receiveNewFrameSet(std::vector<cv::Mat>)));

    // Number of saddle points on calibration pattern

    cv::Size patternSize(10, 9);

    int nSets = calibrationSets.size();

    std::vector< std::vector< std::vector<cv::Point2f> > > qc(2);

    calibrationResults = calibrationSets;

    // Loop through calibration sets

    for(int i=0; i<nSets; i++){

        if(ui->listWidget->item(i)->checkState() != Qt::Checked){

            continue;

        }

        std::vector<cv::Mat> calibrationSetI = calibrationSets[i];

        std::vector< std::vector<cv::Point2f> > qci;

        // Loop through cameras

        for(unsigned int j=0; j<calibrationSetI.size(); j++){

            std::vector<cv::Point2f> qcij;

            // Extract checker corners

            bool success = cv::findChessboardCorners(calibrationSetI[j], patternSize, qcij, cv::CALIB_CB_ADAPTIVE_THRESH);

            if(success){

                cv::cornerSubPix(calibrationSetI[j], qcij, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.001));

                // Draw colored chessboard

                cv::Mat calibrationSetIJColor;

                cv::cvtColor(calibrationSetI[j], calibrationSetIJColor, CV_GRAY2RGB);

                cv::drawChessboardCorners(calibrationSetIJColor, patternSize, qcij, success);

                calibrationResults[i][j] = calibrationSetIJColor;

            } else {

                ui->listWidget->item(i)->setCheckState(Qt::Unchecked);

            }

            qci.push_back(qcij);

        }

        // Add to whole set

        if(!qci[0].empty() && !qci[1].empty() ){

            qc[0].push_back(qci[0]);

            qc[1].push_back(qci[1]);

        }

        // Show results

        ui->listWidget->setCurrentRow(i);

        QCoreApplication::processEvents();

    }

    int nValidSets = qc[0].size();

    if(nValidSets <= 2){

        std::cerr << "Not enough valid calibration sequences!" << std::endl;

        return;

    }

    // Generate world object coordinates [mm]

    std::vector<cv::Point3f> Qi;

    for (int h=0; h<patternSize.height; h++)

        for (int w=0; w<patternSize.width; w++)

            Qi.push_back(cv::Point3f(5 * w, 5* h, 0.0)); // 5mm chess field size

    std::vector< std::vector<cv::Point3f> > Q;

    for(int i=0; i<qc[0].size(); i++)

        Q.push_back(Qi);

    // calibrate the cameras

//    cv::Size frameSize(calibrationSets[0][0].cols, calibrationSets[0][0].rows);

    cv::Size frameSize(640, 480);

    int flags = 0; //cv::CALIB_FIX_K3 + cv::CALIB_FIX_INTRINSIC;

    std::vector< std::vector<cv::Point2f> > qc0 = qc[0];

    std::vector< std::vector<cv::Point2f> > qc1 = qc[1];

    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;

    cal.cam0_error = cv::calibrateCamera(Q, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0);

    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;

    cal.cam1_error = cv::calibrateCamera(Q, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1);

    // stereo calibration (don't change K0, K1, k0, k1)

    int flags_stereo = flags + cv::CALIB_FIX_INTRINSIC;

    cv::Mat E, F, R1, T1;

    cal.stereo_error = cv::stereoCalibrate(Q, qc[0], qc[1], cal.K0, cal.k0, cal.K1, cal.k1,

                                              frameSize, R1, T1, E, F,

                                              cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON),

                                              flags_stereo);

    cal.R1 = R1;

    cal.T1 = T1;

    cal.E = E;

    cal.F = F;

    cal.print(std::cout);

    ui->saveButton->setEnabled(true);

}

void MVCalibrationDialog::on_listWidget_currentRowChanged(int currentRow){

    reviewMode = true;

    ui->snapButton->setText("Capture");

    // Disconnect form live feed for review mode

    disconnect(captureWorker, SIGNAL(newFrameSet(std::vector<cv::Mat>)), this, SLOT(receiveNewFrameSet(std::vector<cv::Mat>)));

    unsigned int idx = ui->listWidget->currentRow();

    // Show result if available, otherwise captured image

    if(!calibrationResults[idx][0].empty())

        ui->videoWidget0->showImageCV(calibrationResults[idx][0]);

    else

        ui->videoWidget0->showImageCV(calibrationSets[idx][0]);

    if(!calibrationResults[idx][1].empty())

        ui->videoWidget1->showImageCV(calibrationResults[idx][1]);

    else

        ui->videoWidget1->showImageCV(calibrationSets[idx][1]);

}

void MVCalibrationDialog::closeEvent(QCloseEvent *event){

    connect(captureWorker, SIGNAL(finished()), captureWorker, SLOT(deleteLater()));

    QMetaObject::invokeMethod(captureWorker, "stopWorking");

    captureWorkerThread->quit();

    captureWorkerThread->wait();

//this->deleteLater();

    event->accept();

}

MVCalibrationDialog::~MVCalibrationDialog(){

//    QMetaObject::invokeMethod(captureWorker, "stopWorking");

//    captureWorkerThread->quit();

//    captureWorkerThread->wait();

//    delete captureWorker;

    delete captureWorkerThread;

    delete ui;

}

void MVCalibrationDialog::on_saveButton_clicked(){

//     Register as meta type and stream operators for use in QSettings

//    qRegisterMetaTypeStreamOperators<MVCalibrationData>("MVCalibrationData");

//    QSettings settings;

//    settings.setValue("Calibration", QVariant::fromValue<MVCalibrationData>(cal));

    cal.save("calibration.xml");

    this->close();

}