Subversion Repositories seema-scanner

#include "SMScanner.h"

#include "ui_SMScanner.h"

#include <QMetaObject>

#include <QFileDialog>

#include <QMessageBox>

#include <QProgressDialog>

#include <pcl/io/pcd_io.h>

#include <pcl/io/ascii_io.h>

#include <pcl/io/ply_io.h>

#include <pcl/io/png_io.h>

#include <pcl/io/vtk_io.h>

#include <vtkPolyDataWriter.h>

#include <pcl/conversions.h>

#include "cvtools.h"

SMScanner::SMScanner(QWidget *parent) :QMainWindow(parent), ui(new Ui::SMScanner),

                                        calibrationReviewMode(false), captureReviewMode(false), lastCaptureId(-1){

    // Register metatypes

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

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

    qRegisterMetaType< std::vector<float> >("std::vector<float>");

    qRegisterMetaType<SMCalibrationSet>("SMCalibrationSet");

    qRegisterMetaType< std::vector<SMCalibrationSet> >("std::vector<SMCalibrationSet>");

    qRegisterMetaType<SMFrameSequence>("SMFrameSequence");

    qRegisterMetaType< std::vector<SMFrameSequence> >("std::vector<SMFrameSequence>");

    qRegisterMetaType<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>("pcl::PointCloud<pcl::PointXYZRGB>::Ptr");

    qRegisterMetaType<SMPointCloud>("SMPointCloud");

    qRegisterMetaType< std::vector<SMPointCloud> >("std::vector<SMPointCloud>");

    // Setup ui (requires stream operators registered)

    ui->setupUi(this);

    // Restore geometry

    this->restoreGeometry(settings.value("geometry/mainwindow").toByteArray());

    this->restoreState(settings.value("state/mainwindow").toByteArray());

    // Set up threads

    captureWorker = new SMCaptureWorker;

    captureWorkerThread = new QThread(this);

    captureWorkerThread->setObjectName("captureWorkerThread");

    captureWorker->moveToThread(captureWorkerThread);

    captureWorkerThread->start();

    // Connections

    connect(captureWorker, SIGNAL(newFrame(unsigned int, cv::Mat)), this, SLOT(onReceiveFrame(unsigned int, cv::Mat)));

    connect(captureWorker, SIGNAL(newCalibrationSet(SMCalibrationSet)), this, SLOT(onReceiveCalibrationSet(SMCalibrationSet)));

    connect(captureWorker, SIGNAL(newFrameSequence(SMFrameSequence)), this, SLOT(onReceiveFrameSequence(SMFrameSequence)));

    connect(captureWorker, SIGNAL(rotatedTo(float)), this, SLOT(onReceiveRotatedTo(float)));

    // Start capturing

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

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

//ui->captureCamera0ScrollArea->setWidget(ui->captureCamera0Widget);

}

void SMScanner::onReceiveFrame(unsigned int camId, cv::Mat frame){

    if(camId == 0){

        if(!calibrationReviewMode)

            ui->calibrationCamera0Widget->showImageCV(frame);

        if(!captureReviewMode)

            ui->captureCamera0Widget->showImageCV(frame);

    } else if(camId == 1){

        if(!calibrationReviewMode)

            ui->calibrationCamera1Widget->showImageCV(frame);

        if(!captureReviewMode)

            ui->captureCamera1Widget->showImageCV(frame);

    }

}

void SMScanner::on_actionPreferences_triggered(){

    connect(&preferenceDialog, SIGNAL(accepted()), this, SLOT(onPreferencesChanged()));

    preferenceDialog.show();

}

void SMScanner::onPreferencesChanged(){

    // Stop capturing thread

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

    connect(captureWorker, SIGNAL(finished()), captureWorkerThread, SLOT(quit()));

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

    captureWorkerThread->quit();

    captureWorkerThread->wait();

    // Restart capture worker

    captureWorker = new SMCaptureWorker;

    captureWorkerThread = new QThread(this);

    captureWorkerThread->setObjectName("captureWorkerThread");

    captureWorker->moveToThread(captureWorkerThread);

    captureWorkerThread->start();

    connect(captureWorker, SIGNAL(newFrame(unsigned int, cv::Mat)), this, SLOT(onReceiveFrame(unsigned int, cv::Mat)));

    connect(captureWorker, SIGNAL(newCalibrationSet(SMCalibrationSet)), this, SLOT(onReceiveCalibrationSet(SMCalibrationSet)));

    connect(captureWorker, SIGNAL(newFrameSequence(SMFrameSequence)), this, SLOT(onReceiveFrameSequence(SMFrameSequence)));

    connect(captureWorker, SIGNAL(rotatedTo(float)), this, SLOT(onReceiveRotatedTo(float)));

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

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

//    QMessageBox::warning(this, "Information", "You must quit and restart the program for new settings to take effect!", QMessageBox::Ok);

}

void SMScanner::closeEvent(QCloseEvent *event){

    // Stop capturing thread

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

    connect(captureWorker, SIGNAL(finished()), captureWorkerThread, SLOT(quit()));

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

    captureWorkerThread->quit();

    captureWorkerThread->wait();

    // Save window geometry

    settings.setValue("geometry/mainwindow", this->saveGeometry());

    settings.setValue("state/mainwindow", this->saveState());

    event->accept();

}

SMScanner::~SMScanner(){

    delete ui;

}

void SMScanner::on_singleCalibrationButton_clicked(){

    // If in review mode, go back to aquisition mode

    if(calibrationReviewMode){

        ui->calibrationListWidget->clearSelection();

        ui->singleCalibrationButton->setText("Single Aquisition");

        ui->batchCalibrationButton->setText("Batch Aquisition");

        calibrationReviewMode = false;

        return;

    }

    QMetaObject::invokeMethod(captureWorker, "acquireCalibrationSet", Q_ARG(float, -1.0));

}

void SMScanner::on_batchCalibrationButton_clicked(){

    // If in review mode, go back to aquisition mode

    if(calibrationReviewMode){

        ui->calibrationListWidget->clearSelection();

        ui->singleCalibrationButton->setText("Single Aquisition");

        ui->batchCalibrationButton->setText("Batch Aquisition");

        calibrationReviewMode = false;

        return;

    }

    // Construct vector of angles

    int angleStart = ui->calibrationBatchStartSpinBox->value();

    int angleEnd = ui->calibrationBatchEndSpinBox->value();

    int angleStep = ui->calibrationBatchStepSpinBox->value();

    if(angleStart > angleEnd)

        angleEnd += 360;

    std::vector<float> angles;

    for(int i=angleStart; i<=angleEnd; i+=angleStep)

        angles.push_back(i % 360);

    QMetaObject::invokeMethod(captureWorker, "acquireCalibrationSets", Q_ARG(std::vector<float>, angles));

    std::cout << "Aquiring sets at ";

    for(unsigned int i=0; i<angles.size(); i++)

        std::cout << angles[i] << " ";

    std::cout << " degrees" <<std::endl;

}

void SMScanner::on_calibrationRotationDial_sliderReleased(){

    float angle = ui->calibrationRotationDial->value();

    std::cout << "Rotation stage target: " << angle << std::endl;

    QMetaObject::invokeMethod(captureWorker, "rotateTo", Q_ARG(float, angle));

    ui->calibrationRotationSpinBox->setValue(angle);

    ui->captureRotationDial->setValue(angle);

    ui->captureRotationSpinBox->setValue(angle);

}

void SMScanner::on_calibrationRotationSpinBox_editingFinished(){

    float angle = ui->calibrationRotationSpinBox->value();

    std::cout << "Rotation stage target: " << angle << std::endl;

    QMetaObject::invokeMethod(captureWorker, "rotateTo", Q_ARG(float, angle));

    ui->calibrationRotationDial->setValue(angle);

    ui->captureRotationSpinBox->setValue(angle);

    ui->captureRotationDial->setValue(angle);

}

void SMScanner::onReceiveCalibrationSet(SMCalibrationSet calibrationSet){

    int id = ui->calibrationListWidget->count();

    calibrationSet.id = id;

    calibrationData.push_back(calibrationSet);

    // Add identifier to list

    QListWidgetItem* item = new QListWidgetItem(QString("Calibration Set %1 -- %2 deg").arg(id).arg(calibrationSet.rotationAngle), ui->calibrationListWidget);

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

    item->setCheckState(Qt::Checked);

    ui->calibrationListWidget->addItem(item);

    // Set enabled checkmark

    if(calibrationData.size() >= 2)

        ui->calibrateButton->setEnabled(true);

}

void SMScanner::on_calibrateButton_clicked(){

    // disable ui elements

    ui->calibrateButton->setEnabled(false);

    ui->calibrationFrame->setEnabled(false);

    // set checked flags

    for(unsigned int i=0; i<calibrationData.size(); i++){

        calibrationData[i].checked = (ui->calibrationListWidget->item(i)->checkState() == Qt::Checked);

    }

//    SMCalibrationWorker calibrationWorker;

//    calibrationWorker.performCalibration(calibrationData);

    // Set up calibration thread

    calibrationWorker = new SMCalibrationWorker;

    calibrationWorkerThread = new QThread(this);

    calibrationWorkerThread->setObjectName("calibrationWorkerThread");

    calibrationWorker->moveToThread(calibrationWorkerThread);

    calibrationWorkerThread->start();

    // Connections

    connect(calibrationWorker, SIGNAL(newSetProcessed(int)), this, SLOT(onCalibrationSetProcessed(int)));

    connect(calibrationWorker, SIGNAL(newFrameResult(int,int,bool,cv::Mat)), this, SLOT(onCalibrationFrameResult(int,int,bool,cv::Mat)));

    connect(calibrationWorker, SIGNAL(done()), this, SLOT(onCalibrationDone()));

    connect(calibrationWorker, SIGNAL(done()), ui->pointCloudWidget, SLOT(updateCalibrationParameters()));

    connect(calibrationWorker, SIGNAL(done()), calibrationWorkerThread, SLOT(quit()));

    connect(calibrationWorker, SIGNAL(done()), calibrationWorker, SLOT(deleteLater()));

    // Start calibration

    QMetaObject::invokeMethod(calibrationWorker, "performCameraCalibration", Q_ARG(std::vector<SMCalibrationSet>, calibrationData));

}

void SMScanner::onCalibrationSetProcessed(int idx){

    if(calibrationData[idx].frame0Result.empty() && calibrationData[idx].frame1Result.empty())

        ui->calibrationListWidget->item(idx)->setCheckState(Qt::Unchecked);

    ui->calibrationListWidget->setCurrentRow(idx);

}

void SMScanner::onCalibrationDone(){

    std::cout << "Calibration done!" << std::endl;

    ui->calibrateButton->setEnabled(true);

    ui->calibrationFrame->setEnabled(true);

}

void SMScanner::on_calibrationListWidget_itemSelectionChanged(){

    // if selection is just cleared

    if(ui->calibrationListWidget->selectedItems().empty())

        return;

    // clamp to available range

    if(ui->calibrationListWidget->currentRow() < 0)

        return;

    assert(ui->calibrationListWidget->currentRow() < calibrationData.size());

    int currentRow = std::min(int(calibrationData.size())-1,

                              std::max(0, ui->calibrationListWidget->currentRow()));

    calibrationReviewMode = true;

    ui->singleCalibrationButton->setText("Live View");

    ui->batchCalibrationButton->setText("Live View");

    if(calibrationData[currentRow].frame0Result.empty())

        ui->calibrationCamera0Widget->showImageCV(calibrationData[currentRow].frame0);

    else

        ui->calibrationCamera0Widget->showImageCV(calibrationData[currentRow].frame0Result);

    if(calibrationData[currentRow].frame1Result.empty())

        ui->calibrationCamera1Widget->showImageCV(calibrationData[currentRow].frame1);

    else

        ui->calibrationCamera1Widget->showImageCV(calibrationData[currentRow].frame1Result);

//     std::cout << "on_calibrationListWidget_itemSelectionChanged" << std::endl;

}

void SMScanner::onCalibrationFrameResult(int idx, int camID, bool success, cv::Mat result){

    if(success){

        if(camID == 0)

            calibrationData[idx].frame0Result = result;

        else if(camID == 1)

            calibrationData[idx].frame1Result = result;

    }

}

void SMScanner::on_captureRotationDial_sliderReleased(){

    float angle = ui->captureRotationDial->value();

    std::cout << "Rotation stage target: " << angle << std::endl;

    QMetaObject::invokeMethod(captureWorker, "rotateTo", Q_ARG(float, angle));

    ui->captureRotationSpinBox->setValue(ui->captureRotationDial->value());

    ui->calibrationRotationDial->setValue(ui->captureRotationDial->value());

    ui->calibrationRotationSpinBox->setValue(ui->captureRotationDial->value());

}

void SMScanner::on_captureRotationSpinBox_editingFinished(){

    float angle = ui->captureRotationSpinBox->value();

    std::cout << "Rotation stage target: " << angle << std::endl;

    QMetaObject::invokeMethod(captureWorker, "rotateTo", Q_ARG(float, angle));

    ui->captureRotationDial->setValue(angle);

    ui->calibrationRotationSpinBox->setValue(angle);

    ui->calibrationRotationDial->setValue(angle);

}

void SMScanner::on_singleCaptureButton_clicked(){

    // If in review mode, go back to aquisition mode

    if(captureReviewMode){

        ui->captureTreeWidget->clearSelection();

        ui->singleCaptureButton->setText("Single Aquisition");

        ui->batchCaptureButton->setText("Batch Aquisition");

        captureReviewMode = false;

        return;

    }

    QMetaObject::invokeMethod(captureWorker, "acquireFrameSequence", Q_ARG(float, -1.0));

}

void SMScanner::on_batchCaptureButton_clicked(){

    // If in review mode, go back to aquisition mode

    if(captureReviewMode){

        ui->captureTreeWidget->clearSelection();

        ui->singleCaptureButton->setText("Single Aquisition");

        ui->batchCaptureButton->setText("Batch Aquisition");

        captureReviewMode = false;

        return;

    }

    // Construct vector of angles

    int angleStart = ui->captureBatchStartSpinBox->value();

    int angleEnd = ui->captureBatchEndSpinBox->value();

    int angleStep = ui->captureBatchStepSpinBox->value();

    if(angleStart > angleEnd)

        angleEnd += 360;

    std::vector<float> angles;

    for(int i=angleStart; i<=angleEnd; i+=angleStep)

        angles.push_back(i % 360);

    std::cout << "Aquiring sequences at ";

    for(unsigned int i=0; i<angles.size(); i++)

        std::cout << angles[i] << " ";

    std::cout << " degrees" <<std::endl;

    QMetaObject::invokeMethod(captureWorker, "acquireFrameSequences", Q_ARG(std::vector<float>, angles));

}

void SMScanner::onReceiveFrameSequence(SMFrameSequence frameSequence){

    frameSequence.id = ++lastCaptureId;

    captureData.push_back(frameSequence);

    // Add identifier to list

    QTreeWidgetItem* item = new QTreeWidgetItem(ui->captureTreeWidget);

    item->setText(0, QString("Frame Sequence %1 -- %2 deg").arg(frameSequence.id).arg(frameSequence.rotationAngle));

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

    item->setData(0, Qt::UserRole, QPoint(captureData.size()-1, -1));

    //item->setCheckState(0, Qt::Checked);

    //ui->captureTreeWidget->addItem(item);

    for(unsigned int i=0; i<frameSequence.frames0.size(); i++){

        QTreeWidgetItem* subItem = new QTreeWidgetItem(item);

        subItem->setText(0, QString("frames %1").arg(i));

        subItem->setData(0, Qt::UserRole, QPoint(frameSequence.id, i));

    }

    ui->reconstructButton->setEnabled(true);

}

void SMScanner::on_captureTreeWidget_itemSelectionChanged(){

    // if selection is just cleared

    if(ui->captureTreeWidget->selectedItems().empty())

        return;

    QTreeWidgetItem *item = ui->captureTreeWidget->currentItem();

    captureReviewMode = true;

    ui->singleCaptureButton->setText("Live View");

    ui->batchCaptureButton->setText("Live View");

    QPoint idx = item->data(0, Qt::UserRole).toPoint();

    if( idx.y() != -1 && idx.x()>=0 && idx.x()<captureData.size()

            && idx.y()<captureData[idx.x()].frames0.size()

            && idx.y()<captureData[idx.x()].frames1.size() ){

        // TODO idx.x() can & WILL be out of bounds

        ui->captureCamera0Widget->showImageCV(captureData[idx.x()].frames0[idx.y()]);

        ui->captureCamera1Widget->showImageCV(captureData[idx.x()].frames1[idx.y()]);

    }

//     std::cout << "on_captureTreeWidget_itemSelectionChanged" << std::endl;

}

void SMScanner::onReceiveRotatedTo(float angle){

    // update ui with new position

    ui->calibrationRotationDial->setValue(angle);

    ui->captureRotationDial->setValue(angle);

    ui->calibrationRotationSpinBox->setValue(angle);

    ui->captureRotationSpinBox->setValue(angle);

}

void SMScanner::on_actionExport_Sets_triggered(){

    QString dirName = QFileDialog::getExistingDirectory(this, "Export calibration sets", QString());

    QProgressDialog progressDialog("Exporting Sets...", "Cancel", 0, 100, this);

    progressDialog.setWindowModality(Qt::WindowModal);

    progressDialog.setMinimumDuration(1000);

    progressDialog.show();

    cv::Mat frameBGR;

    for(unsigned int i=0; i<calibrationData.size(); i++){

        QString fileName = QString("%1/frame0_%2.png").arg(dirName).arg(i);

        progressDialog.setValue(100.0*i/calibrationData.size());

        // Convert to BGR

        if(calibrationData[i].frame0.channels() == 1)

            cv::cvtColor(calibrationData[i].frame0, frameBGR, CV_BayerBG2BGR);

        else

            cv::cvtColor(calibrationData[i].frame1, frameBGR, CV_RGB2BGR);

        cv::imwrite(fileName.toStdString(), frameBGR);

        fileName = QString("%1/frame1_%2.png").arg(dirName).arg(i);

        // Convert to BGR

        if(calibrationData[i].frame1.channels() == 1)

            cv::cvtColor(calibrationData[i].frame1, frameBGR, CV_BayerBG2BGR);

        else

            cv::cvtColor(calibrationData[i].frame1, frameBGR, CV_RGB2BGR);

        cv::imwrite(fileName.toStdString(), frameBGR);

        // Necessary to prevent pileup of video frame signals

        QCoreApplication::processEvents();

        if(progressDialog.wasCanceled())

            return;

    }

}

void SMScanner::on_actionExport_Sequences_triggered(){

    QString dirName = QFileDialog::getExistingDirectory(this, "Export frame sequences", QString());

    QProgressDialog progressDialog("Exporting Sequences...", "Cancel", 0, 100, this);

    progressDialog.setWindowModality(Qt::WindowModal);

    progressDialog.setMinimumDuration(1000);

    progressDialog.show();

    cv::Mat frameBGR;

    for(unsigned int i=0; i<captureData.size(); i++){

        QString seqDirName = QString("%1/sequence_%2").arg(dirName).arg(i);

        if(!QDir().mkdir(seqDirName))

            std::cerr << "Could not create directory " << seqDirName.toStdString() << std::endl;

        for(unsigned int j=0; j<captureData[i].frames0.size(); j++){

            progressDialog.setValue(100.0*i/captureData.size() + 100.0/captureData.size()*j/captureData[i].frames0.size());

            QString fileName0 = QString("%1/frames0_%2.png").arg(seqDirName).arg(j);

            // Convert Bayer to rgb (png needs BGR order)

            cv::cvtColor(captureData[i].frames0[j], frameBGR, CV_BayerBG2BGR);

            cv::imwrite(fileName0.toStdString(), frameBGR);

            // Necessary to prevent pileup of video frame signals

            QCoreApplication::processEvents();

            QString fileName1 = QString("%1/frames1_%2.png").arg(seqDirName).arg(j);

            // Convert Bayer to rgb (png needs BGR order)

            cv::cvtColor(captureData[i].frames1[j], frameBGR, CV_BayerBG2BGR);

            cv::imwrite(fileName1.toStdString(), frameBGR);

            // Necessary to prevent pileup of video frame signals

            QCoreApplication::processEvents();

            if(progressDialog.wasCanceled())

                return;

        }

    }

}

void SMScanner::on_actionExport_White_Frames_triggered(){

    QString dirName = QFileDialog::getExistingDirectory(this, "Export frame sequences", QString());

    QProgressDialog progressDialog("Exporting White Frames...", "Cancel", 0, 100, this);

    progressDialog.setWindowModality(Qt::WindowModal);

    progressDialog.setMinimumDuration(1000);

    cv::Mat frameBGR;

    for(unsigned int i=0; i<captureData.size(); i++){

        progressDialog.setValue(100.0*i/captureData.size());

        QString seqDirName = QString("%1/sequence_%2").arg(dirName).arg(i);

        if(!QDir().mkdir(seqDirName))

            std::cerr << "Could not create directory " << seqDirName.toStdString() << std::endl;

        QString fileName = QString("%1/frames0_0.png").arg(seqDirName);

        // Convert Bayer to rgb (png needs BGR order)

        cv::cvtColor(captureData[i].frames0[0], frameBGR, CV_BayerBG2BGR);

        cv::imwrite(fileName.toStdString(), frameBGR);

        // Necessary to prevent memory pileup

        QCoreApplication::processEvents();

        fileName = QString("%1/frames1_0.png").arg(seqDirName);

        // Convert Bayer to rgb (png needs BGR order)

        cv::cvtColor(captureData[i].frames1[0], frameBGR, CV_BayerBG2BGR);

        cv::imwrite(fileName.toStdString(), frameBGR);

        // Necessary to prevent memory pileup

        QCoreApplication::processEvents();

        if(progressDialog.wasCanceled())

            return;

    }

}

void SMScanner::on_reconstructButton_clicked(){

    // Set up reconstruction thread

    reconstructionWorker = new SMReconstructionWorker;

    reconstructionWorkerThread = new QThread(this);

    reconstructionWorkerThread->setObjectName("reconstructionWorkerThread");

    reconstructionWorker->moveToThread(reconstructionWorkerThread);

    reconstructionWorkerThread->start();

    // Connections

    connect(reconstructionWorker, SIGNAL(newPointCloud(SMPointCloud)), this, SLOT(onReceivePointCloud(SMPointCloud)));

    connect(reconstructionWorker, SIGNAL(done()), reconstructionWorkerThread, SLOT(quit()));

    connect(reconstructionWorker, SIGNAL(done()), reconstructionWorker, SLOT(deleteLater()));

    // Start reconstructing

    QMetaObject::invokeMethod(reconstructionWorker, "setup");

    // TODO is a reference working here??

    QMetaObject::invokeMethod(reconstructionWorker, "reconstructPointClouds", Q_ARG(std::vector<SMFrameSequence>, captureData));

    for(unsigned int i=0; i<captureData.size(); i++){

        if(!captureData[i].reconstructed){ // & (ui->captureTreeWidget->topLevelItem(i)->checkState(0) == Qt::Checked))

            captureData[i].reconstructed = true;

            // now done before QMetaObject::invokeMethod(reconstructionWorker, "reconstructPointCloud", Q_ARG(SMFrameSequence, captureData[i]));

        }

    }

}

void SMScanner::on_meshButton_clicked(){

//    // Set up reconstruction thread

//    meshWorker = new SMMeshingWorker;

//    meshWorkerThread = new QThread(this);

//    meshWorkerThread->setObjectName("meshWorkerThread");

//    meshWorker->moveToThread(meshWorkerThread);

//    meshWorkerThread->start();

//    // Connections

//    //connect(meshWorker, SIGNAL(newMesh(pcl::PolygonMesh)), this, SLOT(onReceiveMesh(pcl::PolygonMesh)));

//    //connect(meshWorker, SIGNAL(done()), meshWorkerThread, SLOT(quit()));

//    //connect(meshWorker, SIGNAL(done()), meshWorker, SLOT(deleteLater()));

//    // Start reconstructing

//    //QMetaObject::invokeMethod(meshWorker, "setup");

//    // TODO is a reference working here??

//    QMetaObject::invokeMethod(meshWorker, "reconstructMesh",

//                              Q_ARG(std::vector<SMPointCloud>, pointCloudData));

}

void SMScanner::onReceivePointCloud(SMPointCloud smCloud){

//    for(int i=0; i<captureData.size(); i++){

//        if(captureData[i].id == id)

//            captureData[i].reconstructed = true;

//    }

    //pointCloudData.push_back(smCloud);

    // TODO on demand resizing here is evil

    if(pointCloudData.size()<=smCloud.id) pointCloudData.resize(smCloud.id+1);

    pointCloudData[smCloud.id] = smCloud;

    // Add identifier to list

    QListWidgetItem* item = new QListWidgetItem(QString("Point Cloud %1 -- %2 deg").arg(smCloud.id).arg(smCloud.rotationAngle));

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

    item->setData(Qt::UserRole, QVariant(smCloud.id));

    item->setCheckState(Qt::Checked);

    ui->pointCloudsListWidget->addItem(item);

    ui->pointCloudWidget->addPointCloud(smCloud, smCloud.id);

    // TODO just to test

    on_meshButton_clicked();

}

void SMScanner::on_actionExport_Point_Clouds_triggered(){

    QString directory = QFileDialog::getExistingDirectory(this, "Export Point Clouds", QString());

//    //  Non native file dialog

//    QFileDialog saveDirectoryDialog(this, "Export Point Clouds", QString(), "*.pcd;;*.ply;;*.vtk;;*.png;;*.txt");

//    saveDirectoryDialog.setDefaultSuffix("ply");

//    saveDirectoryDialog.setFileMode(QFileDialog::Directory);

//    saveDirectoryDialog.exec();

//    QString directory = saveDirectoryDialog.directory().path();

//    QString type = saveDirectoryDialog.selectedNameFilter();

    QProgressDialog progressDialog("Exporting Point Clouds...", "Cancel", 0, 100, this);

    progressDialog.setWindowModality(Qt::WindowModal);

    progressDialog.setMinimumDuration(1000);

    // save point clouds in ply format

    for(unsigned int i=0; i<pointCloudData.size(); i++){

        progressDialog.setValue(100.0*i/pointCloudData.size());

    if(pointCloudData[i].id != -1){

        QString fileName = QString("%1/pointcloud_%2.ply").arg(directory).arg(i);

        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pointCloudPCL(pointCloudData[i].pointCloud);

        //pcl::io::savePLYFileBinary(fileName.toStdString(), *pointCloudPCL);

        pcl::PLYWriter w;

        // Write to ply in binary without camera

        w.write<pcl::PointXYZRGBNormal> (fileName.toStdString(), *pointCloudPCL, true, false);

    }

        QCoreApplication::processEvents();

        if(progressDialog.wasCanceled())

            return;

    }

    // save meshlab aln project file

    std::ofstream s(QString("%1/alignment.aln").arg(directory).toLocal8Bit());

    s << pointCloudData.size() << std::endl;

    for(unsigned int i=0; i<pointCloudData.size(); i++){

        if(pointCloudData[i].id != -1){

        QString fileName = QString("pointcloud_%1.ply").arg(pointCloudData[i].id);

        s << fileName.toStdString() << std::endl << "#" << std::endl;

        cv::Mat Tr = cv::Mat::eye(4, 4, CV_32F);

        cv::Mat(pointCloudData[i].R.t()).copyTo(Tr.colRange(0, 3).rowRange(0, 3));

        cv::Mat(-pointCloudData[i].R.t()*pointCloudData[i].T).copyTo(Tr.col(3).rowRange(0, 3));

        for(int j=0; j<Tr.rows; j++){

            for(int k=0; k<Tr.cols; k++){

                s << Tr.at<float>(j,k) << " ";

            }

            s << std::endl;

        }

        }

    }

    s << "0" << std::flush;

    s.close();

}

void SMScanner::on_pointCloudsListWidget_itemChanged(QListWidgetItem *item){

    int id = item->data(Qt::UserRole).toInt();

    assert(pointCloudData.size()>id);

    if(item->checkState() == Qt::Checked){

        id = std::min(int(pointCloudData.size())-1,std::max(0,id));// clamp range

        ui->pointCloudWidget->addPointCloud(pointCloudData[id], id);

    }

    else

        ui->pointCloudWidget->removePointCloud(id);

}

void SMScanner::on_actionExport_Parameters_triggered(){

    QString fileName = QFileDialog::getSaveFileName(this, "Export calibration parameters", QString(), "*.xml");

    QFileInfo info(fileName);

    QString type = info.suffix();

    if(type == ""){

        fileName.append(".xml");

    }

    SMCalibrationParameters calibration = settings.value("calibration/parameters").value<SMCalibrationParameters>();

    calibration.exportToXML(fileName);

}

void SMScanner::on_actionClear_Sequences_triggered(){

    int res = QMessageBox::question(this, "Clear Captured Sequences", "Clear all captured data?", QMessageBox::Ok, QMessageBox::Cancel);

    if(res == QMessageBox::Ok){

        captureData.clear();

        ui->captureTreeWidget->clear();

        lastCaptureId=-1;

    }

}

void SMScanner::on_actionImport_Parameters_triggered(){

    QString fileName = QFileDialog::getOpenFileName(this, "Import calibration parameters", QString(), "*.xml");

    QFileInfo info(fileName);

    QString type = info.suffix();

    if(type != "xml"){

        std::cerr << "Error: calibration parameters must be in .xml file." << std::endl;

        return;

    }

    SMCalibrationParameters cal;

    cal.importFromXML(fileName);

    settings.setValue("calibration/parameters",  QVariant::fromValue(cal));

    ui->pointCloudWidget->updateCalibrationParameters();

    std::cout << "Imported calibration parameters " << fileName.toStdString() << std::endl;

}

void SMScanner::on_actionImport_Sets_triggered(){

    QString dirName = QFileDialog::getExistingDirectory(this, "Import calibration sets", QString());

    QDir dir(dirName);

    QStringList fileNames0 = dir.entryList(QStringList("frame0_*.png"));

    QStringList fileNames1 = dir.entryList(QStringList("frame1_*.png"));

    if(fileNames0.empty() || fileNames1.empty() || fileNames0.count() != fileNames1.count()){

        std::cerr << "Error: could not load calibration sets. Directory must contain .png files for both cameras." << std::endl;

        return;

    }

    calibrationData.clear();

    ui->calibrationListWidget->clear();

    QProgressDialog progressDialog("Importing Sets...", "Cancel", 0, 100, this);

    progressDialog.setWindowModality(Qt::WindowModal);

    progressDialog.setMinimumDuration(1000);

    size_t nSets = fileNames0.size();

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

        progressDialog.setValue(100.0*i/nSets);

        SMCalibrationSet calibrationSet;

        QString fileName0 = QString("%1/frame0_%2.png").arg(dirName).arg(i);

        cv::Mat frame0BGR = cv::imread(fileName0.toStdString());

        cvtools::cvtColorBGRToBayerBG(frame0BGR, calibrationSet.frame0);

        QString fileName1 = QString("%1/frame1_%2.png").arg(dirName).arg(i);

        cv::Mat frame1BGR = cv::imread(fileName1.toStdString());

        cvtools::cvtColorBGRToBayerBG(frame1BGR, calibrationSet.frame1);

        int id = ui->calibrationListWidget->count();

        calibrationSet.id = id;

        calibrationData.push_back(calibrationSet);

        // Add identifier to list

        QListWidgetItem* item = new QListWidgetItem(QString("Calibration Set %1 -- %2 deg").arg(id).arg(calibrationSet.rotationAngle), ui->calibrationListWidget);

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

        item->setCheckState(Qt::Checked);

        ui->calibrationListWidget->addItem(item);

        QCoreApplication::processEvents();

        if(progressDialog.wasCanceled())

            return;

    }

    // Set enabled checkmark

    if(calibrationData.size() >= 2)

        ui->calibrateButton->setEnabled(true);

}

void SMScanner::on_actionImport_Sequences_triggered(){

    // NOTE: we do not know which algorithm was used!!!