root / rgbdslam / src / glviewer.cpp @ 9240aaa3
History | View | Annotate | Download (21.3 KB)
1 | 9240aaa3 | Alex | /* 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 | } |