StructLight-Studio/SLPoseWidget.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
104
105
106
107
108
109
110
111
#include "SLPoseWidget.h"
#include <pcl/geometry/quad_mesh.h>

#include <vtkRenderWindow.h>

SLPoseWidget::SLPoseWidget(QWidget *parent) : QVTKWidget(parent) {

    visualizer = new pcl::visualization::PCLVisualizer("PCLVisualizer", false);
    this->SetRenderWindow(visualizer->getRenderWindow());

    visualizer->setShowFPS(false);
    this->setMouseTracking(false);

    // Create pose viewport
    visualizer->setBackgroundColor(0.3, 0.3, 0.3);

    // Add ply model
    bool success = visualizer->addModelFromPLYFile("/home/jakw/Code/Repos/SLStudio/src/resources/head.ply");

    if(!success){
        // Show cube as a fallback
        pcl::PointCloud<pcl::PointXYZ> square;
        square.resize(4);
        square[0] = pcl::PointXYZ(-10,-10,-10);
        square[1] = pcl::PointXYZ(10,-10,-10);
        square[2] = pcl::PointXYZ(10,10,-10);
        square[3] = pcl::PointXYZ(-10,10,-10);
        pcl::PlanarPolygon<pcl::PointXYZ> anterior;
        anterior.setContour(square);
        visualizer->addPolygon(anterior, 1.0, 0.0, 0.0, "anterior");

        square[0] = pcl::PointXYZ(-10,-10,10);
        square[1] = pcl::PointXYZ(10,-10,10);
        square[2] = pcl::PointXYZ(10,10,10);
        square[3] = pcl::PointXYZ(-10,10,10);
        pcl::PlanarPolygon<pcl::PointXYZ> posterior;
        posterior.setContour(square);
        visualizer->addPolygon(posterior, 1.0, 0.0, 0.0, "posterior");

        square[0] = pcl::PointXYZ(-10,-10,-10);
        square[1] = pcl::PointXYZ(-10,10,-10);
        square[2] = pcl::PointXYZ(-10,10,10);
        square[3] = pcl::PointXYZ(-10,-10,10);
        pcl::PlanarPolygon<pcl::PointXYZ> left;
        left.setContour(square);
        visualizer->addPolygon(left, 0.0, 1.0, 0.0, "left");

        square[0] = pcl::PointXYZ(10,-10,-10);
        square[1] = pcl::PointXYZ(10,10,-10);
        square[2] = pcl::PointXYZ(10,10,10);
        square[3] = pcl::PointXYZ(10,-10,10);
        pcl::PlanarPolygon<pcl::PointXYZ> right;
        right.setContour(square);
        visualizer->addPolygon(right, 0.0, 1.0, 0.0, "right");

        square[0] = pcl::PointXYZ(-10,10,-10);
        square[1] = pcl::PointXYZ(10,10,-10);
        square[2] = pcl::PointXYZ(10,10,10);
        square[3] = pcl::PointXYZ(-10,10,10);
        pcl::PlanarPolygon<pcl::PointXYZ> superior;
        superior.setContour(square);
        visualizer->addPolygon(superior, 0.0, 0.0, 1.0, "superior");

        square[0] = pcl::PointXYZ(-10,-10,-10);
        square[1] = pcl::PointXYZ(10,-10,-10);
        square[2] = pcl::PointXYZ(10,-10,10);
        square[3] = pcl::PointXYZ(-10,-10,10);
        pcl::PlanarPolygon<pcl::PointXYZ> inferior;
        inferior.setContour(square);
        visualizer->addPolygon(inferior, 0.0, 0.0, 1.0, "inferior");
    }

//    visualizer->setRepresentationToSurfaceForAllActors();
//#ifndef __APPLE__
    visualizer->setCameraPosition(0,0,-150,0,0,0,0,-1,0);
//#endif


}

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

    //SEGFAULT!!
//    visualizer->updateShapePose("cube", T);

//    // Show only rotation
//    T.translation() = Eigen::Vector3f(0.0, 0.0, 0.0);

    // Workaround
    Eigen::Vector3f posVector = T.rotation() * Eigen::Vector3f(0,0,-150);
    Eigen::Vector3f lookAtVector = -posVector;
    Eigen::Vector3f upVector = T.rotation() * Eigen::Vector3f(0, -1, 0);
    visualizer->setCameraPosition(posVector[0], posVector[1], posVector[2],
                                lookAtVector[0], lookAtVector[1], lookAtVector[2],
                                upVector[0], upVector[1], upVector[2]);

//    // Workaround 2
//    double dummy;
//    if(!visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, dummy, "pointCloudPose"))
//        visualizer->addPointCloud(pointCloudPCL, *colorHandler, "pointCloudPose", viewPortPose);
//    visualizer->updatePointCloudPose("pointCloudPose", T);

    this->update();
}


SLPoseWidget::~SLPoseWidget(){

    //delete visualizer;

}