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();
}