source: pacpussensors/trunk/Vislab/20150310_lib3dv-1.2.0/src/display.cc@ 149

Last change on this file since 149 was 136, checked in by ldecherf, 7 years ago

Doc
File size: 37.0 KB
Line 
1/* 3dv-client/display.cc
2 *
3 * Copyright (C) 2013 VisLab
4 *
5 * This file is part of lib3dv; you can redistribute it and/or modify
6 * it under the terms of the GNU Lesser General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU Lesser General Public License
16 * along with this program; if not, see <http://www.gnu.org/licenses/>.
17 */
18
19#include "display.h"
20#include "interactor_style_3dv.h"
21
22#include <lib3dv/ipm.h>
23#include <lib3dv/pose_utils.h>
24
25#include <pcl/visualization/image_viewer.h>
26#include <pcl/visualization/pcl_visualizer.h>
27
28#include <boost/lexical_cast.hpp>
29
30#include <limits>
31#include <stdint.h>
32#include <string>
33#include <boost/format.hpp>
34
35#ifdef _MSC_VER
36#else
37#include <X11/Xlib.h>
38#endif
39
40void on_keyboard_event(const pcl::visualization::KeyboardEvent& event, void* data)
41{
42 display* _this = static_cast<display*>(data);
43
44 if(event.getKeySym() == "p" && event.keyDown())
45 {
46 _this->toggle_points_display();
47 }
48 else if(event.getKeySym() == "t" && event.keyDown())
49 {
50 _this->toggle_terrain_display();
51 }
52 else if(event.getKeySym() == "m" && event.keyDown())
53 {
54 _this->toggle_terrain_gm_display();
55 }
56 else if(event.getKeySym() == "o" && event.keyDown())
57 {
58 _this->toggle_obstacles_display();
59 }
60 else if(event.getKeySym() == "v" && event.keyDown())
61 {
62 _this->toggle_motion_display();
63 }
64}
65
66void addCubeCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in, const lib3dv::stixel& stixel, uint8_t r, uint8_t g, uint8_t b){
67 //m_3d_viewer->addCube(stixel.m_x - stixel.m_dx, stixel.m_x + stixel.m_dx, stixel.m_y - stixel.m_dy, stixel.m_y + stixel.m_dy, stixel.m_z, stixel.m_z + stixel.m_height, r, g, b, boost::lexical_cast<std::string>(o) + "-" + boost::lexical_cast<std::string>(s));
68
69 float step_stix=0.02;
70 pcl::PointXYZRGB padd(r,g,b);
71
72 for(padd.x=stixel.m_x;padd.x<stixel.m_dx+stixel.m_x;padd.x+=step_stix){
73 //base
74 padd.y = stixel.m_y;
75 padd.z = stixel.m_z;
76 cloud_in->points.push_back(padd);
77 padd.y = stixel.m_y+stixel.m_dy;
78 cloud_in->points.push_back(padd);
79 //up
80 padd.z = stixel.m_height+stixel.m_z;
81 cloud_in->points.push_back(padd);
82 padd.y = stixel.m_y;
83 cloud_in->points.push_back(padd);
84 }
85 for(padd.y=stixel.m_y;padd.y<stixel.m_dy+stixel.m_y;padd.y+=step_stix){
86 //base
87 padd.x = stixel.m_x;
88 padd.z = stixel.m_z;
89 cloud_in->points.push_back(padd);
90 padd.x = stixel.m_x+stixel.m_dx;
91 cloud_in->points.push_back(padd);
92 //up
93 padd.z = stixel.m_height+stixel.m_z;
94 cloud_in->points.push_back(padd);
95 padd.x = stixel.m_x;
96 cloud_in->points.push_back(padd);
97 }
98 for(padd.z = stixel.m_z;padd.z<stixel.m_height+stixel.m_z;padd.z+=step_stix){
99 //base
100 padd.x = stixel.m_x;
101 padd.y = stixel.m_y;
102 cloud_in->points.push_back(padd);
103 padd.x = stixel.m_x+stixel.m_dx;
104 cloud_in->points.push_back(padd);
105 padd.y = stixel.m_y+stixel.m_dy;
106 cloud_in->points.push_back(padd);
107 padd.x = stixel.m_x;
108 cloud_in->points.push_back(padd);
109 }
110}
111
112
113display::display(const lib3dv::calibration::camera_intrinsics& intrinsics, const lib3dv::calibration::position& position, const lib3dv::calibration::orientation& orientation, float baseline, uint8_t downsample, double area_step, uint8_t log_level) :
114 m_downsample(downsample),
115 m_area_step(area_step),
116 m_log_level(log_level),
117 m_image_viewers_closed(0),
118 m_3d_viewer_closed(false),
119 m_show_points(true),
120 m_show_terrain(true),
121 m_show_terrain_gm(true),
122 m_show_obstacles(true),
123 m_show_motion(true),
124 m_show_classification(true),
125 m_stopped(false),
126 m_updated(false)
127{
128
129
130 lib3dv::calibration::camera_intrinsics downsampled_intrinsics = intrinsics;
131
132 downsampled_intrinsics.m_u0 /= downsample;
133 downsampled_intrinsics.m_v0 /= downsample;
134 downsampled_intrinsics.m_ku /= downsample;
135 downsampled_intrinsics.m_kv /= downsample;
136
137 m_ipm = boost::shared_ptr<lib3dv::inverse_perspective_mapping>(new lib3dv::inverse_perspective_mapping(downsampled_intrinsics, position, orientation, baseline));
138 m_pose_utils = boost::shared_ptr<lib3dv::pose_utils>(new lib3dv::pose_utils(position, orientation));
139
140 m_me_orientations.first.m_yaw = m_me_orientations.first.m_pitch = m_me_orientations.first.m_roll = 0;
141 m_me_orientations.second.m_yaw = m_me_orientations.second.m_pitch = m_me_orientations.second.m_roll = 0;
142
143 m_td_orientations.first = m_td_orientations.second = boost::posix_time::time_duration(0,0,0);
144
145 #ifdef _MSC_VER
146 screen_width=GetSystemMetrics(SM_CXSCREEN);
147 screen_height=GetSystemMetrics(SM_CYSCREEN);
148 #else
149 Display* disp = XOpenDisplay(NULL);
150 Screen* scrn = DefaultScreenOfDisplay(disp);
151 screen_height = scrn->height;
152 screen_width = scrn->width;
153 XCloseDisplay(disp);
154 #endif
155}
156
157void display::update(boost::shared_ptr<const lib3dv::image > image)
158{
159 boost::mutex::scoped_lock l(m_update_mutex);
160 m_image_buffers[image->m_type - 1].second = image;
161 m_updated = true;
162 m_update_condition.notify_all();
163}
164
165void display::update(boost::shared_ptr<const lib3dv::terrain> terrain)
166{
167 boost::mutex::scoped_lock l(m_update_mutex);
168 m_terrain_buffers.second = terrain;
169 m_updated = true;
170 m_update_condition.notify_all();
171}
172
173void display::update(boost::shared_ptr<const std::vector<lib3dv::obstacle> > obstacles)
174{
175 boost::mutex::scoped_lock l(m_update_mutex);
176 m_obstacles_buffers.second = obstacles;
177 m_updated = true;
178 m_update_condition.notify_all();
179}
180
181void display::update(boost::shared_ptr<const lib3dv::motion> motion)
182{
183 boost::mutex::scoped_lock l(m_update_mutex);
184 m_motion_buffers.second = motion;
185 m_updated = true;
186 m_update_condition.notify_all();
187}
188
189void display::update(boost::shared_ptr<const lib3dv::classification> classification)
190{
191 boost::mutex::scoped_lock l(m_update_mutex);
192 m_classification_buffers.second = classification;
193 m_updated = true;
194 m_update_condition.notify_all();
195}
196
197float r_coefficeints[] = {1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 1.0f};
198float g_coefficeints[] = {0.0f, 0.5f, 1.0f, 1.0f, 0.5f, 0.0f, 0.5f, 0.0f, 0.0f};
199float b_coefficeints[] = {0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.5f};
200
201std::string image_names[] = {"Left rectified", "Right rectified", "Left raw", "Right raw", "DSI"};
202
203void display::run()
204{
205 do
206 {
207 boost::this_thread::interruption_point();
208
209 {
210 boost::mutex::scoped_lock l(m_update_mutex);
211
212 while(!m_updated && !m_stopped)
213 m_update_condition.wait(l);
214
215 if(m_stopped)
216 break;
217 m_updated = false;
218
219 for(unsigned int i = 0; i < lib3dv::image::type::NUM - 1; ++i)
220 if(m_image_buffers[i].second)
221 {
222 std::swap(m_image_buffers[i].first, m_image_buffers[i].second);
223 m_image_buffers[i].second.reset();
224 }
225
226 if(m_terrain_buffers.second)
227 {
228 std::swap(m_terrain_buffers.first, m_terrain_buffers.second);
229 m_terrain_buffers.second.reset();
230 }
231
232 if(m_obstacles_buffers.second)
233 {
234 std::swap(m_obstacles_buffers.first, m_obstacles_buffers.second);
235 m_obstacles_buffers.second.reset();
236 }
237
238 if(m_motion_buffers.second)
239 {
240 std::swap(m_motion_buffers.first, m_motion_buffers.second);
241 m_motion_buffers.second.reset();
242 }
243
244 if(m_classification_buffers.second)
245 {
246 std::swap(m_classification_buffers.first, m_classification_buffers.second);
247 m_classification_buffers.second.reset();
248 }
249
250 }
251
252 for(unsigned int i = 0; i < lib3dv::image::type::NUM - 1; ++i)
253 if(m_image_buffers[i].first && !m_image_viewers_closed[i])
254 {
255 if(m_image_buffers[i].first->m_type == lib3dv::image::type::RIGHT_RECTIFIED)
256 m_right_rectified = m_image_buffers[i].first;
257
258 if(m_image_buffers[i].first->m_type == lib3dv::image::type::DSI)
259 m_dsi = m_image_buffers[i].first;
260
261 if(!m_image_viewers[i])
262 m_image_viewers[i] = boost::shared_ptr<pcl::visualization::ImageViewer>(new pcl::visualization::ImageViewer());
263
264 m_image_viewers[i]->setWindowTitle(image_names[i] + " - " + boost::posix_time::to_simple_string(m_image_buffers[i].first->m_timestamp));
265
266 switch(m_image_buffers[i].first->m_format)
267 {
268 case (lib3dv::image::format::MONO8) :
269 case (lib3dv::image::format::BAYER8_BGGR) :
270 case (lib3dv::image::format::BAYER8_GRBG) :
271 case (lib3dv::image::format::BAYER8_RGGB) :
272 case (lib3dv::image::format::BAYER8_GBRG) :
273 {
274 m_image_viewers[i]->showMonoImage(m_image_buffers[i].first->m_buffer.data(), m_image_buffers[i].first->m_width, m_image_buffers[i].first->m_height);
275 if(m_classification_buffers.first && m_show_classification && m_image_buffers[i].first->m_type == lib3dv::image::type::RIGHT_RECTIFIED)
276 {
277 m_image_viewers[i]->removeLayer("rectangles");
278 m_image_viewers[i]->removeLayer("car");
279 m_image_viewers[i]->removeLayer("pedestrian");
280 int offset=m_image_buffers[i].first->m_height+27-screen_height;
281 if (offset<0)
282 offset=0;
283 for(int s=0; s < m_classification_buffers.first->m_candidates.size(); s++)
284 {
285 const lib3dv::candidate& cand = m_classification_buffers.first->m_candidates[s];
286 if (cand.m_category==lib3dv::category_type::PEDESTRIAN)
287 {
288 m_image_viewers[i]->addRectangle(cand.m_x0,cand.m_x1,m_image_buffers[i].first->m_height-cand.m_y0,m_image_buffers[i].first->m_height-cand.m_y1,0.0,0.0,1.0,"rectangles",1.0);
289 m_image_viewers[i]->addText(cand.m_x0+2, cand.m_y1-offset,"Conf="+(boost::format("%0.2f") %cand.m_confidence).str(), 0.0, 0.0, 1.0, "pedestrian",1.0);
290 }
291 else if (cand.m_category==lib3dv::category_type::CAR)
292 {
293 m_image_viewers[i]->addRectangle(cand.m_x0,cand.m_x1,m_image_buffers[i].first->m_height-cand.m_y0,m_image_buffers[i].first->m_height-cand.m_y1,1.0,0.0,0.0,"rectangles",1.0);
294 m_image_viewers[i]->addText(cand.m_x0+2, cand.m_y1-offset,"Conf="+(boost::format("%0.2f") %cand.m_confidence).str(), 1.0, 0.0, 0.0, "car",1.0);
295 }
296 }
297
298 }
299 break;
300 }
301 case (lib3dv::image::format::MONO16) :
302 case (lib3dv::image::format::BAYER16_BGGR) :
303 case (lib3dv::image::format::BAYER16_GRBG) :
304 case (lib3dv::image::format::BAYER16_RGGB) :
305 case (lib3dv::image::format::BAYER16_GBRG) :
306 {
307 if(m_image_buffers[i].first->m_type == lib3dv::image::type::DSI)
308 m_image_viewers[i]->showShortImage(reinterpret_cast<const uint16_t*>(m_image_buffers[i].first->m_buffer.data()), m_image_buffers[i].first->m_width, m_image_buffers[i].first->m_height, 0, 0x7F00);
309 else
310 m_image_viewers[i]->showShortImage(reinterpret_cast<const uint16_t*>(m_image_buffers[i].first->m_buffer.data()), m_image_buffers[i].first->m_width, m_image_buffers[i].first->m_height, 0, (1 << m_image_buffers[i].first->m_bpp) - 1, true);
311 break;
312 }
313
314 case (lib3dv::image::format::RGB24) :
315 {
316 m_image_viewers[i]->showRGBImage(m_image_buffers[i].first->m_buffer.data(), m_image_buffers[i].first->m_width, m_image_buffers[i].first->m_height);
317 if(m_classification_buffers.first && m_show_classification && m_image_buffers[i].first->m_type == lib3dv::image::type::RIGHT_RECTIFIED)
318 {
319 m_image_viewers[i]->removeLayer("rectangles");
320 m_image_viewers[i]->removeLayer("car");
321 m_image_viewers[i]->removeLayer("pedestrian");
322 int offset=m_image_buffers[i].first->m_height+27-screen_height;
323 if (offset<0)
324 offset=0;
325 for(int s=0; s < m_classification_buffers.first->m_candidates.size(); s++)
326 {
327 const lib3dv::candidate& cand = m_classification_buffers.first->m_candidates[s];
328 if (cand.m_category==lib3dv::category_type::PEDESTRIAN)
329 {
330 m_image_viewers[i]->addRectangle(cand.m_x0,cand.m_x1,m_image_buffers[i].first->m_height-cand.m_y0,m_image_buffers[i].first->m_height-cand.m_y1,0.0,0.0,1.0,"rectangles",1.0);
331 m_image_viewers[i]->addText(cand.m_x0+2, cand.m_y1-offset,"Conf="+(boost::format("%0.2f") %cand.m_confidence).str(), 0.0, 0.0, 1.0, "pedestrian",1.0);
332 }
333 else if (cand.m_category==lib3dv::category_type::CAR)
334 {
335 m_image_viewers[i]->addRectangle(cand.m_x0,cand.m_x1,m_image_buffers[i].first->m_height-cand.m_y0,m_image_buffers[i].first->m_height-cand.m_y1,1.0,0.0,0.0,"rectangles",1.0);
336 m_image_viewers[i]->addText(cand.m_x0+2, cand.m_y1-offset,"Conf="+(boost::format("%0.2f") %cand.m_confidence).str(), 1.0, 0.0, 0.0, "car",1.0);
337 }
338 }
339
340 }
341 break;
342 }
343 }
344
345 m_image_viewers[i]->spinOnce();
346
347 if(m_image_viewers[i]->wasStopped())
348 {
349 m_image_viewers[i]->close();
350 m_image_viewers[i].reset();
351 m_image_viewers_closed[i] = true;
352 }
353
354 boost::mutex::scoped_lock l(m_update_mutex);
355 m_image_buffers[i].first.reset();
356 }
357
358 if((m_terrain_buffers.first || m_obstacles_buffers.first || m_dsi) && !m_3d_viewer_closed)
359 {
360 if(!m_3d_viewer)
361 {
362 int argc = 0;
363
364 m_3d_viewer = boost::shared_ptr<pcl::visualization::PCLVisualizer>(new pcl::visualization::PCLVisualizer(argc, (char**)NULL, std::string("Point Cloud"), interactor_style_3dv::New()));
365
366 //FIXME: get camera params from device calibration
367 m_3d_viewer->setBackgroundColor (0, 0, 0);
368 m_3d_viewer->addCoordinateSystem (1.0);
369 m_3d_viewer->initCameraParameters();
370 m_3d_viewer->setCameraPosition(-3.0, 0.0, 6.0, 1.03093, -0.140037, 1.86098, 0.715357, 0.012124, 0.698653);
371 m_3d_viewer->setCameraFieldOfView(0.8575);
372 m_3d_viewer->setCameraClipDistances(-100, 300);
373 m_3d_viewer->setShowFPS(false);
374 m_3d_viewer->registerKeyboardCallback(on_keyboard_event, (void*)this);
375 }
376
377 if(m_dsi && m_show_points)
378 {
379
380 std::vector<lib3dv::point3> world_points;
381 world_points.reserve(m_dsi->m_width * m_dsi->m_height);
382
383 (*m_ipm)(m_dsi, world_points);
384
385 if(m_right_rectified)
386 {
387 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
388 cloud->reserve(world_points.size());
389
390 if(m_right_rectified->m_format == lib3dv::image::format::MONO8)
391 {
392 const uint8_t* buffer = m_right_rectified->m_buffer.data();
393
394 for(unsigned int i = 0; i < m_dsi->m_height; ++i)
395 for(unsigned int j = 0; j < m_dsi->m_width; ++j)
396 {
397 const uint32_t dsi_px = i * m_dsi->m_width + j;
398 const uint32_t right_px = m_downsample * (i * (m_dsi->m_width * m_downsample) + j);
399
400 const lib3dv::point3& wp = world_points[dsi_px];
401
402 if(wp.m_x < std::numeric_limits<float>::max() && wp.m_y < std::numeric_limits<float>::max() && wp.m_z < std::numeric_limits<float>::max())
403 {
404 uint8_t val = buffer[right_px];
405 pcl::PointXYZRGB p(val, val, val);
406
407 p.x = wp.m_x;
408 p.y = wp.m_y;
409 p.z = wp.m_z;
410
411 cloud->points.push_back(p);
412 }
413 }
414 }
415
416 if(m_right_rectified->m_format == lib3dv::image::format::RGB24)
417 {
418 const uint8_t* buffer = m_right_rectified->m_buffer.data();
419
420 for(unsigned int i = 0; i < m_dsi->m_height; ++i)
421 for(unsigned int j = 0; j < m_dsi->m_width; ++j)
422 {
423 const uint32_t dsi_px = i * m_dsi->m_width + j;
424 const uint32_t right_px = m_downsample * (i * (m_dsi->m_width * 3 * m_downsample) + j * 3);
425
426 const lib3dv::point3& wp = world_points[dsi_px];
427
428 if(wp.m_x < std::numeric_limits<float>::max() && wp.m_y < std::numeric_limits<float>::max() && wp.m_z < std::numeric_limits<float>::max())
429 {
430 uint8_t r = buffer[right_px];
431 uint8_t g = buffer[right_px + 1];
432 uint8_t b = buffer[right_px + 2];
433
434 pcl::PointXYZRGB p(r, g, b);
435
436 p.x = wp.m_x;
437 p.y = wp.m_y;
438 p.z = wp.m_z;
439
440 cloud->points.push_back(p);
441 }
442 }
443 }
444
445 if(!m_3d_viewer->updatePointCloud(cloud, "cloud"))
446 {
447 m_3d_viewer->addPointCloud(cloud, "cloud");
448 m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
449 }
450
451 m_right_rectified.reset();
452 }
453 else
454 {
455// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
456// cloud->reserve(world_points.size());
457//
458// for(unsigned int w = 0; w < world_points.size(); ++w)
459// {
460// const lib3dv::point3& wp = world_points[w];
461//
462// if(wp.m_x < std::numeric_limits<float>::max() && wp.m_y < std::numeric_limits<float>::max() && wp.m_z < std::numeric_limits<float>::max())
463// {
464// pcl::PointXYZ p;
465//
466// p.x = wp.m_x;
467// p.y = wp.m_y;
468// p.z = wp.m_z;
469//
470// cloud->points.push_back(p);
471// }
472// }
473//
474// if(!m_3d_viewer->updatePointCloud(cloud, "cloud"))
475// {
476// m_3d_viewer->addPointCloud(cloud, "cloud");
477// m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
478// }
479 }
480
481 m_dsi.reset();
482 }
483 else if(!m_show_points)
484 m_3d_viewer->removePointCloud("cloud");
485
486 //m_3d_viewer->removeAllShapes();
487
488 std::vector< std::vector<lib3dv::point3> > tcolumn_points;
489
490 if(m_terrain_buffers.first && m_show_terrain)
491 {
492
493 unsigned int dim_x;
494 unsigned int dim_y;
495 std::vector<unsigned long> matrix_terrain_index;
496
497 if(m_show_terrain_gm && m_terrain_buffers.first->m_data.size()>4)
498 {
499 //std::cout << "terrain_size: "<< m_terrain_buffers.first->m_data.size() << std::endl;
500 float x_min=std::numeric_limits<float>::max(),x_max=-std::numeric_limits<float>::max(),y_min=std::numeric_limits<float>::max(),y_max=-std::numeric_limits<float>::max();
501 for(unsigned int t = 0; t < m_terrain_buffers.first->m_data.size(); ++t){
502 //std::cout<<"(x,y)"<<m_terrain_buffers.first->m_data[t].m_x<<" "<<m_terrain_buffers.first->m_data[t].m_y<<std::endl;
503 if(m_terrain_buffers.first->m_data[t].m_x>x_max)
504 x_max = m_terrain_buffers.first->m_data[t].m_x;
505 if(m_terrain_buffers.first->m_data[t].m_x<x_min)
506 x_min = m_terrain_buffers.first->m_data[t].m_x;
507 if(m_terrain_buffers.first->m_data[t].m_y>y_max)
508 y_max = m_terrain_buffers.first->m_data[t].m_y;
509 if(m_terrain_buffers.first->m_data[t].m_y<y_min)
510 y_min = m_terrain_buffers.first->m_data[t].m_y;
511 }
512 dim_x=(x_max-x_min)/m_area_step + 1;
513 dim_y=(y_max-y_min)/m_area_step + 1;
514 //std::cout << "x_min:"<<x_min<< " | x_max:"<<x_max << " | y_min:"<<y_min<<" | y_max:"<<y_max<<std::endl;
515 //std::cout << "dim_x:"<<dim_x<< " dim_y:"<<dim_y<<std::endl;
516 matrix_terrain_index.resize(dim_x*dim_y,std::numeric_limits<unsigned long>::max());
517
518 for(unsigned int t = 0; t < m_terrain_buffers.first->m_data.size(); ++t)
519 {
520 lib3dv::point3 ptt = m_terrain_buffers.first->m_data[t];
521 unsigned int index=((unsigned int)((ptt.m_x-x_min)/m_area_step))*dim_y + (unsigned int)((ptt.m_y-y_min)/m_area_step);
522 if(index < dim_x*dim_y)
523 matrix_terrain_index[index] = t;
524 }
525 /*
526 for(int w=0;w<dim_x;w++){
527 for(int q=0;q<dim_y;q++){
528 unsigned int index=q+w*dim_y;
529 if(matrix_terrain[index].m_z>-100){
530 std::cout<<"("<<matrix_terrain[index].m_x<<","<<matrix_terrain[index].m_y<<")\t";
531 //std::cout <<"o\t";
532 }
533 else
534 std::cout <<"x\t\t";
535 }
536 std::cout << std::endl;
537 }*/
538 }
539 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
540
541 cloud->reserve(m_terrain_buffers.first->m_data.size());
542
543 for(unsigned int t = 0; t < m_terrain_buffers.first->m_data.size(); ++t)
544 {
545
546 pcl::PointXYZRGB p(0,255,0);
547
548 p.x = m_terrain_buffers.first->m_data[t].m_x;
549 p.y = m_terrain_buffers.first->m_data[t].m_y;
550 p.z = m_terrain_buffers.first->m_data[t].m_z;
551
552 cloud->points.push_back(p);
553 }
554
555
556
557 if (m_show_terrain_gm && m_terrain_buffers.first->m_data.size()>4)
558 {
559 for (int c = 0; c < dim_x; c++)
560 {
561 for (int cc = 0; cc < dim_y; cc++)
562 {
563 pcl::PointXYZRGB p1,p2;
564 const std::vector<lib3dv::point3>& terr_data= m_terrain_buffers.first->m_data;
565 if(matrix_terrain_index[cc+c*dim_y]!=std::numeric_limits<unsigned long>::max())
566 {
567 //valid point
568 unsigned int p1_index = matrix_terrain_index[cc+c*dim_y];
569 p1.x = terr_data[p1_index].m_x;
570 p1.y = terr_data[p1_index].m_y;
571 p1.z = terr_data[p1_index].m_z;
572
573 if(cc<dim_y-1)
574 if(matrix_terrain_index[cc+1+c*dim_y]!=std::numeric_limits<unsigned long>::max() )
575 {
576 int p2_index=matrix_terrain_index[cc+1+c*dim_y];
577 p2.x = terr_data[p2_index].m_x;
578 p2.y = terr_data[p2_index].m_y;
579 p2.z = terr_data[p2_index].m_z;
580 std::string tid = "TRLH-"+ boost::lexical_cast<std::string>(c)+"-"+ boost::lexical_cast<std::string>(cc);
581 pcl::PointXYZRGB pmed(0,255,0);
582 float step_terr = 0.02;
583 pmed.x=p1.x;
584 pmed.z=p1.z;
585 float dz = step_terr*(p2.z-p1.z)/(p2.y-p1.y);
586
587 for(pmed.y=p1.y+step_terr, pmed.z=p1.z+dz;pmed.y<p2.y;pmed.y+=step_terr,pmed.z+=dz){
588 cloud->points.push_back(pmed);
589 }
590 }
591
592 if(c<dim_x-1)
593 if(matrix_terrain_index[cc+(c+1)*dim_y]!=std::numeric_limits<unsigned long>::max() )
594 {
595 int p2_index=matrix_terrain_index[cc+(c+1)*dim_y];
596 std::string tid = "TRLV-"+ boost::lexical_cast<std::string>(c)+"-"+ boost::lexical_cast<std::string>(cc);
597 p2.x = terr_data[p2_index].m_x;
598 p2.y = terr_data[p2_index].m_y;
599 p2.z = terr_data[p2_index].m_z;
600 pcl::PointXYZRGB pmed(0,255,0);
601 float step_terr = 0.02;
602 pmed.y=p1.y;
603 pmed.z=p1.z;
604 float dz = step_terr*(p2.z-p1.z)/(p2.x-p1.x);
605 for(pmed.x=p1.x+step_terr, pmed.z=p1.z+dz;pmed.x<p2.x;pmed.x+=step_terr,pmed.z+=dz){
606 cloud->points.push_back(pmed);
607 }
608 }
609
610 }
611 }
612 }
613
614
615 }
616 if(!m_3d_viewer->updatePointCloud(cloud, "terrain"))
617 {
618 m_3d_viewer->addPointCloud(cloud, "terrain");
619 m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "terrain");
620 }
621
622 } else if(!m_show_terrain)
623 {
624 m_3d_viewer->removePointCloud("terrain");
625 }
626
627
628
629 if(m_obstacles_buffers.first && m_show_obstacles)
630 {
631 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
632 for(unsigned int o = 0; o < m_obstacles_buffers.first->size(); ++o)
633 {
634 const lib3dv::obstacle& obstacle = (*m_obstacles_buffers.first)[o];
635
636 uint8_t c = obstacle.m_guid % 9;
637 float r = r_coefficeints[c];
638 float g = g_coefficeints[c];
639 float b = b_coefficeints[c];
640
641 for(unsigned int s = 0; s < obstacle.m_data.size(); ++s)
642 {
643 const lib3dv::stixel& stixel = obstacle.m_data[s];
644 //m_3d_viewer->addCube(stixel.m_x - stixel.m_dx, stixel.m_x + stixel.m_dx, stixel.m_y - stixel.m_dy, stixel.m_y + stixel.m_dy, stixel.m_z, stixel.m_z + stixel.m_height, r, g, b, boost::lexical_cast<std::string>(o) + "-" + boost::lexical_cast<std::string>(s));
645 addCubeCloud(cloud,stixel,r*255,g*255,b*255);
646 }
647 }
648 if(!m_3d_viewer->updatePointCloud(cloud, "obstacle"))
649 {
650 m_3d_viewer->addPointCloud(cloud, "obstacle");
651 m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "obstacle");
652 }
653
654 } else if(!m_show_obstacles){
655 m_3d_viewer->removePointCloud("obstacle");
656 }
657
658
659 if(m_motion_buffers.first && m_show_motion)
660 {
661 if(m_motion_buffers.first->m_poses.size()==0)
662 {
663 m_me_orientations.first.m_yaw = m_me_orientations.first.m_pitch = m_me_orientations.first.m_roll = 0;
664 m_td_orientations.first = boost::posix_time::time_duration(0,0,0);
665 m_3d_viewer->removeShape("orientation");
666 m_3d_viewer->removeShape("orientation_rate");
667 m_3d_viewer->removeShape("orientation_acc");
668 }
669 else
670 {
671 std::swap(m_td_orientations.first, m_td_orientations.second);
672 std::swap(m_me_orientations.first, m_me_orientations.second);
673
674 boost::posix_time::time_duration& prev_pose_td = m_td_orientations.first;
675 boost::posix_time::time_duration& pose_td = m_td_orientations.second;
676
677 float & yaw = m_me_orientations.second.m_yaw;
678 float & pitch = m_me_orientations.second.m_pitch;
679 float & roll = m_me_orientations.second.m_roll;
680
681 float & prev_yaw = m_me_orientations.first.m_yaw;
682 float & prev_pitch = m_me_orientations.first.m_pitch;
683 float & prev_roll = m_me_orientations.first.m_roll;
684
685
686 for (std::vector<lib3dv::pose>::const_iterator p = m_motion_buffers.first->m_poses.begin(); p != m_motion_buffers.first->m_poses.end(); p++)
687 if ( p->m_type == lib3dv::pose::type::RELATIVE_POSE )
688 {
689 m_pose_utils->extract_orientation(*p,yaw,pitch,roll);
690 pose_td = p->m_timestamp - boost::posix_time::ptime(boost::gregorian::date(1970,1,1));
691 }
692
693
694 float dt = std::fabs((double)pose_td.total_microseconds() - (double)prev_pose_td.total_microseconds())/1e6;
695
696 std::string orientation_acc = "yaw_acc: " + boost::lexical_cast<std::string>((yaw - prev_yaw)/pow(dt,2.0f))
697 + ", pitch_acc: " + boost::lexical_cast<std::string>((pitch - prev_pitch)/pow(dt,2.0f)) +
698 + ", roll_acc: " + boost::lexical_cast<std::string>((roll - prev_roll)/pow(dt,2.0f)) + " [rad/s^2]";
699
700 std::string orientation_rate = "yaw rate: " + boost::lexical_cast<std::string>((yaw - prev_yaw)/dt)
701 + ", pitch rate: " + boost::lexical_cast<std::string>((pitch - prev_pitch)/dt) +
702 + ", roll rate: " + boost::lexical_cast<std::string>((roll - prev_roll)/dt) + " [rad/s]";
703
704 std::string orientation = "yaw: " + boost::lexical_cast<std::string>(yaw)
705 + ", pitch: " + boost::lexical_cast<std::string>(pitch) +
706 + ", roll: " + boost::lexical_cast<std::string>(roll) + " [rad]";
707 m_3d_viewer->removeShape("orientation");
708 m_3d_viewer->removeShape("orientation_rate");
709 m_3d_viewer->removeShape("orientation_acc");
710 if (!m_3d_viewer->updateText(orientation, 5, 25, 1.0, 1.0, 1.0, "orientation"))
711 m_3d_viewer->addText(orientation, 5, 25, 1.0, 1.0, 1.0, "orientation");
712
713 if (!m_3d_viewer->updateText(orientation_rate, 5, 15, 1.0, 1.0, 1.0, "orientation_rate"))
714 m_3d_viewer->addText(orientation_rate, 5, 15, 1.0, 1.0, 1.0, "orientation_rate");
715
716 if (!m_3d_viewer->updateText(orientation_acc, 5, 5, 1.0, 1.0, 1.0, "orientation_acc"))
717 m_3d_viewer->addText(orientation_acc, 5, 5, 1.0, 1.0, 1.0, "orientation_acc");
718 }
719 }
720 else if (!m_show_motion)
721 {
722 m_3d_viewer->removeShape("angles");
723 m_3d_viewer->removeShape("orientation");
724 m_3d_viewer->removeShape("orientation_rate");
725 m_3d_viewer->removeShape("orientation_acc");
726 }
727 m_3d_viewer->spinOnce();
728
729 if(m_3d_viewer->wasStopped())
730 {
731 m_3d_viewer->close();
732 m_3d_viewer.reset();
733 m_3d_viewer_closed = true;
734 }
735
736 boost::mutex::scoped_lock l(m_update_mutex);
737 m_terrain_buffers.first.reset();
738 m_obstacles_buffers.first.reset();
739 m_motion_buffers.first.reset();
740 }
741 } while(true);
742
743 for(unsigned int i = 0; i < lib3dv::image::type::NUM - 1; ++i)
744 if(m_image_viewers[i])
745 {
746 m_image_viewers[i]->close();
747 m_image_viewers[i].reset();
748 }
749
750 if(m_3d_viewer)
751 {
752 m_3d_viewer->close();
753 m_3d_viewer.reset();
754 }
755}
756
757
758void display::stop()
759{
760 boost::mutex::scoped_lock l(m_update_mutex);
761 m_stopped = true;
762 m_update_condition.notify_all();
763}
Note: See TracBrowser for help on using the repository browser.