Statistics
| Branch: | Revision:

root / rgbdslam / src / glviewer.cpp @ 9240aaa3

History | View | Annotate | Download (21.3 KB)

1
/* This file is part of RGBDSLAM.
2
 * 
3
 * RGBDSLAM is free software: you can redistribute it and/or modify
4
 * it under the terms of the GNU General Public License as published by
5
 * the Free Software Foundation, either version 3 of the License, or
6
 * (at your option) any later version.
7
 * 
8
 * RGBDSLAM is distributed in the hope that it will be useful,
9
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11
 * GNU General Public License for more details.
12
 * 
13
 * You should have received a copy of the GNU General Public License
14
 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.
15
 */
16

    
17

    
18
#include "ros/ros.h"
19
#include <QtGui>
20
#include <QtOpenGL>
21
#include <QThread>
22
#include <GL/gl.h>
23
#include <cmath>
24

    
25
#include "glviewer.h"
26

    
27
#ifndef GL_MULTISAMPLE
28
#define GL_MULTISAMPLE  0x809D
29
#endif
30

    
31
const double pi= 3.14159265358979323846;
32

    
33
GLViewer::GLViewer(QWidget *parent)
34
    : QGLWidget(QGLFormat(QGL::SampleBuffers), parent),
35
      xRot(180*16.0),
36
      yRot(0),
37
      zRot(0),
38
      xTra(0),
39
      yTra(0),
40
      zTra(-220),
41
      polygon_mode(GL_FILL),
42
      cloud_list_indices(),
43
      edge_list_(NULL),
44
      cloud_matrices(new QList<QMatrix4x4>()),
45
      show_axis_(ParameterServer::instance()->get<bool>("show_axis")),
46
      follow_mode_(true){
47
    setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); //can make good use of more space
48
    viewpoint_tf_.setToIdentity();
49
}
50

    
51
GLViewer::~GLViewer() { }
52

    
53
QSize GLViewer::minimumSizeHint() const {
54
    return QSize(400, 400);
55
}
56

    
57
QSize GLViewer::sizeHint() const {
58
    return QSize(640, 480);
59
}
60

    
61
static void qNormalizeAngle(int &angle) {
62
    while (angle < 0)
63
        angle += 360 * 16;
64
    while (angle > 360 * 16)
65
        angle -= 360 * 16;
66
}
67

    
68
void GLViewer::setXRotation(int angle) { 
69
    qNormalizeAngle(angle);
70
    if (angle != xRot) {
71
        xRot = angle;
72
        updateGL();
73
    }
74
}
75

    
76

    
77
void GLViewer::setYRotation(int angle) {
78
    qNormalizeAngle(angle);
79
    if (angle != yRot) {
80
        yRot = angle;
81
        updateGL();
82
    }
83
}
84

    
85
void GLViewer::setZRotation(int angle) {
86
    qNormalizeAngle(angle);
87
    if (angle != zRot) {
88
        zRot = angle;
89
        updateGL();
90
    }
91
}
92

    
93
void GLViewer::initializeGL() {
94
    //glClearColor(0,0,0,0); 
95
    glEnable (GL_BLEND); 
96
    glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
97
    glEnable(GL_DEPTH_TEST);
98
    glEnable(GL_CULL_FACE);
99
    //glShadeModel(GL_SMOOTH);
100
    //glEnable(GL_LIGHTING);
101
    //glEnable(GL_LIGHT0);
102
    //glEnable(GL_MULTISAMPLE);
103
    //gluPerspective(99.0/180.0*pi, 1.00, 0.01, 1e9); //1.38 = tan(57/2°)/tan(43/2°)
104
    ////gluLookAt(0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0);
105
    //static GLfloat lightPosition[4] = { 0.5, 5.0, 7.0, 1.0 };
106
    //glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
107
}
108
//assumes gl mode for triangles
109
//TODO:Performance, Make inline
110
inline
111
bool GLViewer::drawTriangle(const point_type& p1, const point_type& p2, const point_type& p3){
112
    unsigned char b,g,r;
113
    b = *(  (unsigned char*)(&p1.rgb));
114
    g = *(1+(unsigned char*)(&p1.rgb));
115
    r = *(2+(unsigned char*)(&p1.rgb));
116
    glColor3ub(r,g,b); //glColor3f(1.0,1.0,1.0);
117
    glColor3ub(255,0,0); 
118
    glVertex3f(p1.x, p1.y, p1.z);
119

    
120
    b = *(  (unsigned char*)(&p2.rgb));
121
    g = *(1+(unsigned char*)(&p2.rgb));
122
    r = *(2+(unsigned char*)(&p2.rgb));
123
    glColor3ub(r,g,b); //glColor3f(1.0,1.0,1.0);
124
    glColor3ub(0,255,0); 
125
    glVertex3f(p2.x, p2.y, p2.z);
126

    
127
    b = *(  (unsigned char*)(&p3.rgb));
128
    g = *(1+(unsigned char*)(&p3.rgb));
129
    r = *(2+(unsigned char*)(&p3.rgb));
130
    //glColor3ub(r,g,b); //glColor3f(1.0,1.0,1.0);
131
    glColor3ub(0,0,255); 
132
    glVertex3f(p3.x, p3.y, p3.z);
133
    return true;
134
}
135

    
136
void GLViewer::drawAxis(float scale){
137
    if(!show_axis_) return;
138
    glBegin(GL_LINES);
139
    glColor4f (1, 0, 0, 1.0);
140
    glVertex3f(0, 0, 0);
141
    glColor4f (1, 0, 0, 0.0);
142
    glVertex3f(scale, 0, 0);
143
    glColor4f (0, 1, 0, 1.0);
144
    glVertex3f(0, 0, 0);
145
    glColor4f (0, 1, 0, 0.0);
146
    glVertex3f(0, scale, 0);
147
    glColor4f (0, 0, 1, 1.0);
148
    glVertex3f(0, 0, 0);
149
    glColor4f (0, 0, 1, 0.0);
150
    glVertex3f(0, 0, scale);
151
    glEnd();
152
}
153

    
154
void GLViewer::paintGL() {
155
    if(!this->isVisible()) return;
156
    //ROS_INFO("This is paint-thread %d", (unsigned int)QThread::currentThreadId());
157
    if(follow_mode_){
158
        int id = cloud_matrices->size()-1;
159
        if(id >= 0)setViewPoint((*cloud_matrices)[id]);
160
    }
161
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
162
    glLoadIdentity();
163
    //Camera transformation
164
    glTranslatef(xTra, yTra, zTra);
165
    glRotatef(xRot / 16.0, 1.0, 0.0, 0.0);
166
    glRotatef(yRot / 16.0, 0.0, 1.0, 0.0);
167
    glRotatef(zRot / 16.0, 0.0, 0.0, 1.0);
168
    glMultMatrixd(static_cast<GLdouble*>( viewpoint_tf_.data() ));//works as long as qreal and GLdouble are typedefs to double (might depend on hardware)
169
    drawAxis(0.5);//Show origin as big axis
170
    drawEdges();
171
    ROS_DEBUG("Drawing %i PointClouds", cloud_list_indices.size());
172
    for(int i = 0; i<cloud_list_indices.size() && i<cloud_matrices->size(); i++){
173
        //GLdouble* entry = static_cast<GLdouble*>( cloud_matrices[i].data() );
174
        //for(int j = 0; j < 16; j++, entry++){
175
        //    ROS_INFO("Matrix[%d]: %f", j, *entry);
176
        //}
177
        glPushMatrix();
178
        glMultMatrixd(static_cast<GLdouble*>( (*cloud_matrices)[i].data() ));//works as long as qreal and GLdouble are typedefs to double (might depend on hardware)
179
        glCallList(cloud_list_indices[i]);
180
        drawAxis(0.15);
181
        glPopMatrix();
182
    }
183
}
184
// Shape For Debuging 
185
void GLViewer::drawCoil() {
186
    const float nbSteps = 200.0;
187
    glBegin(GL_QUAD_STRIP);
188
    for (int i=0; i<nbSteps; ++i) {
189
        const float ratio = i/nbSteps;
190
        const float angle = 21.0*ratio;
191
        const float c = cos(angle);
192
        const float s = sin(angle);
193
        const float r1 = 1.0 - 0.8f*ratio;
194
        const float r2 = 0.8f - 0.8f*ratio;
195
        const float alt = ratio - 0.5f;
196
        const float nor = 0.5f;
197
        const float up = sqrt(1.0-nor*nor);
198
        glColor3f(1.0-ratio, 0.2f , ratio);
199
        glNormal3f(nor*c, up, nor*s);
200
        glVertex3f(r1*c, alt, r1*s+2);
201
        glVertex3f(r2*c, alt+0.05f, r2*s+2); }
202
    glEnd();
203
}
204

    
205
void GLViewer::resizeGL(int width, int height)
206
{
207
    //int side = qMin(width, height);
208
    glViewport(0, 0, width, height);
209
    //glViewport((width - side) / 2, (height - side) / 2, side, side);
210

    
211
    glMatrixMode(GL_PROJECTION);
212
    glLoadIdentity();
213
//#ifdef QT_OPENGL_ES_1
214
//    glOrthof(-0.5, +0.5, -0.5, +0.5, 1.0, 15.0);
215
//#else
216
//    glOrtho(-0.5, +0.5, -0.5, +0.5, 1.0, 15.0);
217
//#endif
218
    //gluPerspective(57.0/180.0*pi, 1.38, 0.01, 1e9); //1.38 = tan(57/2°)/tan(43/2°) as kinect has viewing angles 57 and 43
219
    float ratio = (float)width / (float) height;
220
    gluPerspective(pi/4.0, ratio, 0.1, 1e4); 
221
    glMatrixMode(GL_MODELVIEW);
222
}
223

    
224
void GLViewer::mouseDoubleClickEvent(QMouseEvent *event) {
225
    xRot=180*16.0;
226
    yRot=0;
227
    zRot=0;
228
    xTra=0;
229
    yTra=0;
230
    if(cloud_matrices->size()>0){
231
      if (event->buttons() & Qt::LeftButton) {
232
        int id = cloud_matrices->size()-1;
233
        setViewPoint((*cloud_matrices)[id]);
234
      } else if (event->buttons() & Qt::RightButton) {
235
        int id = 0;
236
        setViewPoint((*cloud_matrices)[id]);
237
      } else if (event->buttons() & Qt::MidButton) { 
238
        viewpoint_tf_.setToIdentity();
239
        zTra=-120;
240
      }
241
      setClickedPosition(event->x(), event->y());
242
    }
243
    updateGL();
244
}
245
void GLViewer::toggleShowAxis(bool flag){
246
  show_axis_ = flag;
247
}
248
void GLViewer::toggleFollowMode(bool flag){
249
  follow_mode_ = flag;
250
}
251
void GLViewer::mousePressEvent(QMouseEvent *event) {
252
    lastPos = event->pos();
253
}
254

    
255
void GLViewer::wheelEvent(QWheelEvent *event) {
256
    zTra += ((float)event->delta())/25.0; 
257
    updateGL();
258
}
259
void GLViewer::mouseMoveEvent(QMouseEvent *event) {//TODO: consolidate setRotation methods
260
    int dx = event->x() - lastPos.x();
261
    int dy = event->y() - lastPos.y();
262

    
263
    if (event->buttons() & Qt::LeftButton) {
264
        setXRotation(xRot - 8 * dy);
265
        setYRotation(yRot + 8 * dx);
266
    } else if (event->buttons() & Qt::RightButton) {
267
        setXRotation(xRot - 8 * dy);
268
        setZRotation(zRot + 8 * dx);
269
    } else if (event->buttons() & Qt::MidButton) {
270
        xTra += dx/200.0;
271
        yTra -= dy/200.0;
272
        updateGL();
273
    }
274
    lastPos = event->pos();
275
}
276

    
277
void GLViewer::updateTransforms(QList<QMatrix4x4>* transforms){
278
    ROS_WARN_COND(transforms->size() < cloud_matrices->size(), "Got less transforms than before!");
279
    // This doesn't deep copy, but should work, as qlist maintains a reference count 
280
    delete cloud_matrices;
281
    cloud_matrices = transforms; 
282
    ROS_DEBUG("New Cloud matrices size: %d", cloud_matrices->size());
283
    updateGL();
284
}
285

    
286
void GLViewer::addPointCloud(pointcloud_type * pc, QMatrix4x4 transform){
287
    ROS_DEBUG("pc pointer in addPointCloud: %p (this is %p in thread %d)", pc, this, (unsigned int)QThread::currentThreadId());
288
    pointCloud2GLStrip(pc);
289
    cloud_matrices->push_back(transform); //keep for later
290
    updateGL();
291
}
292

    
293
void GLViewer::pointCloud2GLStrip(pointcloud_type * pc){
294
    struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
295
    ROS_DEBUG("Making GL list from point-cloud pointer %p in thread %d", pc, (unsigned int)QThread::currentThreadId());
296
    GLuint cloud_list_index = glGenLists(1);
297
    if(!cloud_list_index) {
298
        ROS_ERROR("No display list could be created");
299
        return;
300
    }
301
    float mesh_thresh = ParameterServer::instance()->get<double>("squared_meshing_threshold");
302
    glNewList(cloud_list_index, GL_COMPILE);
303
    cloud_list_indices.push_back(cloud_list_index);
304
    //ROS_INFO_COND(!pc->is_dense, "Expected dense cloud for opengl drawing");
305
    point_type origin;
306
    origin.x = 0;
307
    origin.y = 0;
308
    origin.z = 0;
309

    
310
    float depth;
311
    bool strip_on = false, flip = false; //if flip is true, first the lower then the upper is inserted
312
    const int w=pc->width, h=pc->height;
313
    unsigned char b,g,r;
314
    const int step = ParameterServer::instance()->get<int>("visualization_skip_step");
315
    for( int y = 0; y < h-step; y+=step){ //go through every point and make two triangles 
316
        for( int x = 0; x < w-step; x+=step){//for it and its neighbours right and/or down
317
            using namespace pcl;
318
            if(!strip_on){ //Generate vertices for new triangle
319
                const point_type* ll = &pc->points[(x)+(y+step)*w]; //one down (lower left corner)
320
                if(!hasValidXYZ(*ll)) continue; // both new triangles in this step would use this point
321
                const point_type* ur = &pc->points[(x+step)+y*w]; //one right (upper right corner)
322
                if(!hasValidXYZ(*ur)) continue; // both new triangles in this step would use this point
323
          
324
                const point_type* ul = &pc->points[x+y*w]; //current point (upper right)
325
                if(hasValidXYZ(*ul)){ //ul, ur, ll all valid
326
                  depth = squaredEuclideanDistance(*ul,origin);
327
                  if (squaredEuclideanDistance(*ul,*ll)/depth <= mesh_thresh  and 
328
                      squaredEuclideanDistance(*ul,*ll)/depth <= mesh_thresh  and
329
                      squaredEuclideanDistance(*ur,*ll)/depth <= mesh_thresh){
330
                    glBegin(GL_TRIANGLE_STRIP);
331
                    strip_on = true;
332
                    flip = false; //correct order, upper first
333
                    //Prepare the first two vertices of a triangle strip
334
                    //drawTriangle(*ul, *ll, *ur);
335
                    b = *(  (unsigned char*)(&ul->rgb));
336
                    g = *(1+(unsigned char*)(&ul->rgb));
337
                    r = *(2+(unsigned char*)(&ul->rgb));
338
                    glColor3ub(r,g,b);
339

    
340
                    //glColor3ub(255,0,0);
341
                    glVertex3f(ul->x, ul->y, ul->z);
342
                  }
343
                } 
344
                if(!strip_on) { //can't use the point on the upper left, should I still init a triangle?
345
                  const point_type* lr = &pc->points[(x+step)+(y+step)*w]; //one right-down (lower right)
346
                  if(!hasValidXYZ(*lr)) {
347
                    //if this is not valid, there is no way to make a new triangle in the next step
348
                    //and one could have been drawn starting in this step, only if ul had been valid
349
                    x++;
350
                    continue;
351
                  } else { //at least one can be started at the lower left
352
                    depth = squaredEuclideanDistance(*ur,origin);
353
                    if (squaredEuclideanDistance(*ur,*ll)/depth <= mesh_thresh  and 
354
                        squaredEuclideanDistance(*lr,*ll)/depth <= mesh_thresh  and
355
                        squaredEuclideanDistance(*ur,*lr)/depth <= mesh_thresh){
356
                      glBegin(GL_TRIANGLE_STRIP);
357
                      strip_on = true;
358
                      flip = true; //but the lower has to be inserted first, for correct order
359
                    }
360
                  }
361
                }
362
                if(strip_on) { //Be this the second or the first vertex, insert it
363
                  b = *(  (unsigned char*)(&ll->rgb));
364
                  g = *(1+(unsigned char*)(&ll->rgb));
365
                  r = *(2+(unsigned char*)(&ll->rgb));
366
                  glColor3ub(r,g,b);
367

    
368
                  //glColor3ub(0,255,0);
369
                  glVertex3f(ll->x, ll->y, ll->z);
370
                }
371
                continue; //not relevant but demonstrate that nothing else is done in this iteration
372
            } // end strip was off
373
            else 
374
            {//neighbours to the left and left down are already set
375
              const point_type* ul;
376
              if(flip){ ul = &pc->points[(x)+(y+step)*w]; } //one down (lower left corner) 
377
              else { ul = &pc->points[x+y*w]; } //current point (upper right)
378
              if(hasValidXYZ(*ul)){ //Neighbours to the left are prepared
379
                depth = squaredEuclideanDistance(*ul,origin);
380
                if (squaredEuclideanDistance(*ul,*(ul-step))/depth > mesh_thresh){
381
                  glEnd();
382
                  strip_on = false;
383
                  continue;
384
                }
385
                //Complete the triangle with both leftern neighbors
386
                //drawTriangle(*ul, *ll, *ur);
387
                b = *(  (unsigned char*)(&ul->rgb));
388
                g = *(1+(unsigned char*)(&ul->rgb));
389
                r = *(2+(unsigned char*)(&ul->rgb));
390
                glColor3ub(r,g,b);
391

    
392
                //glColor3ub(255,0,0);
393
                glVertex3f(ul->x, ul->y, ul->z);
394
              } else {
395
                glEnd();
396
                strip_on = false;
397
                continue; //TODO: Could restart with next point instead
398
              }
399
              //The following point connects one to the left with the other on this horizontal level
400
              const point_type* ll;
401
              if(flip){ ll = &pc->points[x+y*w]; } //current point (upper right)
402
              else { ll = &pc->points[(x)+(y+step)*w]; } //one down (lower left corner) 
403
              if(hasValidXYZ(*ll)){ 
404
                depth = squaredEuclideanDistance(*ll,origin);
405
                if (squaredEuclideanDistance(*ul,*ll)/depth > mesh_thresh or
406
                    squaredEuclideanDistance(*ul,*(ul-step))/depth > mesh_thresh or
407
                    squaredEuclideanDistance(*ll,*(ll-step))/depth > mesh_thresh){
408
                  glEnd();
409
                  strip_on = false;
410
                  continue;
411
                }
412
                b = *(  (unsigned char*)(&ll->rgb));
413
                g = *(1+(unsigned char*)(&ll->rgb));
414
                r = *(2+(unsigned char*)(&ll->rgb));
415
                glColor3ub(r,g,b);
416

    
417
                glVertex3f(ll->x, ll->y, ll->z);
418
              } else {
419
                glEnd();
420
                strip_on = false;
421
                continue;
422
              }
423
            }//completed triangles if strip is running
424
        }
425
        if(strip_on) glEnd();
426
        strip_on = false;
427
    }
428
    ROS_DEBUG("Compiled pointcloud into list %i",  cloud_list_index);
429
    glEndList();
430
    //pointcloud_type pc_empty;
431
    //pc_empty.points.swap(pc->points);
432
    //pc->width = 0;
433
    //pc->height = 0;
434
    Q_EMIT cloudRendered(pc);
435
    clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
436
}
437

    
438
void GLViewer::deleteLastNode(){
439
  if(cloud_list_indices.size() <= 1){
440
    this->reset();
441
    return;
442
  }
443
        GLuint nodeId = cloud_list_indices.back();
444
        cloud_list_indices.pop_back();
445
        glDeleteLists(nodeId,1);
446
}
447

    
448
void GLViewer::pointCloud2GLList(pointcloud_type const * pc){
449
    struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
450
    ROS_DEBUG("Making GL list from point-cloud pointer %p in thread %d", pc, (unsigned int)QThread::currentThreadId());
451
    GLuint cloud_list_index = glGenLists(1);
452
    if(!cloud_list_index) {
453
        ROS_ERROR("No display list could be created");
454
        return;
455
    }
456
    float mesh_thresh = ParameterServer::instance()->get<double>("squared_meshing_threshold");
457
    cloud_list_indices.push_back(cloud_list_index);
458
    glNewList(cloud_list_index, GL_COMPILE);
459
    glBegin(GL_TRIANGLES);
460
    //ROS_INFO_COND(!pc->is_dense, "Expected dense cloud for opengl drawing");
461
    point_type origin;
462
    origin.x = 0;
463
    origin.y = 0;
464
    origin.z = 0;
465

    
466
    float depth;
467
    unsigned int w=pc->width, h=pc->height;
468
    for(unsigned int x = 0; x < w-1; x++){
469
        for(unsigned int y = 0; y < h-1; y++){
470
            using namespace pcl;
471

    
472
            const point_type* pi = &pc->points[x+y*w]; //current point
473

    
474
            if(!(hasValidXYZ(*pi))) continue;
475
            depth = squaredEuclideanDistance(*pi,origin);
476

    
477
            const point_type* pl = &pc->points[(x+1)+(y+1)*w]; //one right-down
478
            if(!(hasValidXYZ(*pl)) or squaredEuclideanDistance(*pi,*pl)/depth > mesh_thresh)  
479
              continue;
480

    
481
            const point_type* pj = &pc->points[(x+1)+y*w]; //one right
482
            if(hasValidXYZ(*pj)
483
               and squaredEuclideanDistance(*pi,*pj)/depth <= mesh_thresh  
484
               and squaredEuclideanDistance(*pj,*pl)/depth <= mesh_thresh){
485
              drawTriangle(*pi, *pj, *pl);
486
            }
487
            const point_type* pk = &pc->points[(x)+(y+1)*w]; //one down
488
            
489
            if(hasValidXYZ(*pk)
490
               and squaredEuclideanDistance(*pi,*pk)/depth <= mesh_thresh  
491
               and squaredEuclideanDistance(*pk,*pl)/depth <= mesh_thresh){
492
              drawTriangle(*pi, *pk, *pl);
493
            }
494
        }
495
    }
496
    glEnd();
497
    ROS_DEBUG("Compiled pointcloud into list %i",  cloud_list_index);
498
    glEndList();
499
    clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
500
}
501

    
502
void GLViewer::reset(){
503
    glDeleteLists(1,cloud_list_indices.back());
504
    cloud_list_indices.clear();
505
    cloud_matrices->clear();
506
    if(edge_list_) {
507
      delete edge_list_;
508
      edge_list_ = NULL;
509
    }
510
    updateGL();
511
}
512
QImage GLViewer::renderList(QMatrix4x4 transform, int list_id){
513
    return QImage();
514
}
515

    
516
void GLViewer::setEdges(QList<QPair<int, int> >* edge_list){
517
  if(edge_list_) delete edge_list_;
518
  edge_list_ = edge_list;
519
}
520

    
521
void GLViewer::drawEdges(){
522
  if(!ParameterServer::instance()->get<bool>("show_axis")) return;
523
  if(edge_list_ == NULL) return;
524
  glBegin(GL_LINES);
525
  glLineWidth(2);
526
  glColor4f(1,1,1,0.4);
527
  for(int i = 0; i < edge_list_->size(); i++){
528
    int id = (*edge_list_)[i].first;
529
    if(cloud_matrices->size() > id){//only happens in weird circumstances
530
      float x = (*cloud_matrices)[id](0,3);
531
      float y = (*cloud_matrices)[id](1,3);
532
      float z = (*cloud_matrices)[id](2,3);
533
      glVertex3f(x,y,z);
534
      id = (*edge_list_)[i].second;
535
      x = (*cloud_matrices)[id](0,3);
536
      y = (*cloud_matrices)[id](1,3);
537
      z = (*cloud_matrices)[id](2,3);
538
      glVertex3f(x,y,z);
539
    }
540
  }
541
  glEnd();
542
}
543

    
544

    
545
void GLViewer::setViewPoint(QMatrix4x4 new_vp){
546
    ///Moving the camera is inverse to moving the points to draw
547
    viewpoint_tf_ = new_vp.inverted();
548
}
549

    
550
void GLViewer::setClickedPosition(int x, int y) {
551
    GLint viewport[4];
552
    GLdouble modelview[16];
553
    GLdouble projection[16];
554
    GLfloat winX, winY, winZ;
555
    GLdouble posX, posY, posZ;
556

    
557
    glGetDoublev( GL_MODELVIEW_MATRIX, modelview );
558
    glGetDoublev( GL_PROJECTION_MATRIX, projection );
559
    glGetIntegerv( GL_VIEWPORT, viewport );
560

    
561
    winX = (float)x;
562
    winY = (float)viewport[3] - (float)y;
563
    glReadPixels( x, int(winY), 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &winZ );
564
    if(winZ != 1){ //default value, where nothing was rendered
565
      gluUnProject( winX, winY, winZ, modelview, projection, viewport, &posX, &posY, &posZ);
566
      ROS_INFO_STREAM((float)winZ << ", " << posX << "," << posY << "," << posZ);
567
      viewpoint_tf_(0,3) = -posX;
568
      viewpoint_tf_(1,3) = -posY;
569
      viewpoint_tf_(2,3) = -posZ;
570
    }
571
}
572

    
573
void GLViewer::toggleTriangulation() {
574
    ROS_INFO("Toggling Triangulation");
575
    if(polygon_mode == GL_FILL){ // Turn on Pointcloud mode
576
        polygon_mode = GL_POINT;
577
    } else if(polygon_mode == GL_POINT){ // Turn on Wireframe mode
578
        polygon_mode = GL_LINE;
579
    } else { // Turn on Surface mode
580
        polygon_mode = GL_FILL;
581
    }
582
    glPolygonMode(GL_FRONT_AND_BACK, polygon_mode);
583
    updateGL();
584
}
585