StructLight-Studio/SLTrackerDialog.cpp at master · fangchuan/StructLight-Studio · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
#include "SLTrackerDialog.h"
#include "ui_SLTrackerDialog.h"

SLTrackerDialog::SLTrackerDialog(QWidget *parent) : QDialog(parent), ui(new Ui::SLTrackerDialog), trackerWorkerThread(NULL), trackerWorker(NULL), tracking(false){
    ui->setupUi(this);

    // Create QDockWidget like action associated with dialog
    action = new QAction("Tracker", this);
    action->setCheckable(true);
    connect(action, SIGNAL(toggled(bool)), this, SLOT(setVisible(bool)));

    // Create traces
    ui->translationTraceWidget->setBounds(100, -180, 180);
    ui->translationTraceWidget->addTrace("tx");
    ui->translationTraceWidget->addTrace("ty");
    ui->translationTraceWidget->addTrace("tz");

    ui->rotationTraceWidget->setBounds(100, -0.5, 1.5);
    ui->rotationTraceWidget->addTrace("qw");
    ui->rotationTraceWidget->addTrace("qx");
    ui->rotationTraceWidget->addTrace("qy");
    ui->rotationTraceWidget->addTrace("qz");
}

// QDockWidget like, return a checkable action in sync with visibility
QAction* SLTrackerDialog::toggleViewAction(){
    return action;
}

void SLTrackerDialog::receiveNewPointCloud(PointCloudConstPtr pointCloud){
    emit newPointCloud(pointCloud);
}

SLTrackerDialog::~SLTrackerDialog(){

    delete ui;

}

void SLTrackerDialog::on_startStopPushButton_clicked(){

    if(!tracking){
        // Prepare trackerWorker on separate thread
        trackerWorker = new SLTrackerWorker();
        trackerWorkerThread = new QThread(this);
        trackerWorkerThread->setObjectName("trackerWorkerThread");

        trackerWorker->moveToThread(trackerWorkerThread);
        trackerWorkerThread->start(QThread::LowPriority);

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

        qRegisterMetaType< Eigen::Affine3f >("Eigen::Affine3f");
        connect(this, SIGNAL(newPointCloud(PointCloudConstPtr)), trackerWorker, SLOT(trackPointCloud(PointCloudConstPtr)));
//        connect(trackerWorker, SIGNAL(newPoseEstimate(Eigen::Affine3f)), ui->poseWidget, SLOT(showPoseEstimate(Eigen::Affine3f)));
        connect(trackerWorker, SIGNAL(newPoseEstimate(Eigen::Affine3f)), this, SLOT(showPoseEstimate(Eigen::Affine3f)));
        connect(trackerWorkerThread, SIGNAL(finished()), trackerWorker, SLOT(deleteLater()));

        tracking = true;
        ui->startStopPushButton->setText("Stop Tracking");

    } else {
        // Terminate tracker worker thread
        if(trackerWorkerThread && trackerWorkerThread->isRunning()){
            trackerWorkerThread->quit();
            trackerWorkerThread->wait();
        }

        disconnect(trackerWorker);
        tracking = false;
        ui->startStopPushButton->setText("Start Tracking");
    }
}

void SLTrackerDialog::showEvent(QShowEvent *){
    if(!action->isChecked())
        action->setChecked(true);
}

void SLTrackerDialog::closeEvent(QCloseEvent *){
    action->setChecked(false);
}

void SLTrackerDialog::showPoseEstimate(Eigen::Affine3f T){

    if(ui->poseTab->isVisible()){
        ui->poseWidget->showPoseEstimate(T);
    } else if(ui->traceTab->isVisible()){
        Eigen::Vector3f t(T.translation());
        Eigen::Quaternionf q(T.rotation());

        ui->translationTraceWidget->addMeasurement("tx", t(0));
        ui->translationTraceWidget->addMeasurement("ty", t(1));
        ui->translationTraceWidget->addMeasurement("tz", t(2));
        ui->translationTraceWidget->draw();

        ui->rotationTraceWidget->addMeasurement("qw", q.w());
        ui->rotationTraceWidget->addMeasurement("qx", q.x());
        ui->rotationTraceWidget->addMeasurement("qy", q.y());
        ui->rotationTraceWidget->addMeasurement("qz", q.z());
        ui->rotationTraceWidget->draw();
    }
}