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 paintthread %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.0nor*nor); 
198 
glColor3f(1.0ratio, 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 pointcloud 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 < hstep; y+=step){ //go through every point and make two triangles 
316 
for( int x = 0; x < wstep; 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 rightdown (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,*(ulstep))/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,*(ulstep))/depth > mesh_thresh or

407 
squaredEuclideanDistance(*ll,*(llstep))/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 pointcloud 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 < w1; x++){ 
469 
for(unsigned int y = 0; y < h1; 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 rightdown 
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 