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 |
|