/* 3dv-client/display.cc
*
* Copyright (C) 2013 VisLab
*
* This file is part of lib3dv; you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation; either version 3 of the License, or (at
* your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, see .
*/
#include "display.h"
#include "interactor_style_3dv.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#ifdef _MSC_VER
#else
#include
#endif
void on_keyboard_event(const pcl::visualization::KeyboardEvent& event, void* data)
{
display* _this = static_cast(data);
if(event.getKeySym() == "p" && event.keyDown())
{
_this->toggle_points_display();
}
else if(event.getKeySym() == "t" && event.keyDown())
{
_this->toggle_terrain_display();
}
else if(event.getKeySym() == "m" && event.keyDown())
{
_this->toggle_terrain_gm_display();
}
else if(event.getKeySym() == "o" && event.keyDown())
{
_this->toggle_obstacles_display();
}
else if(event.getKeySym() == "v" && event.keyDown())
{
_this->toggle_motion_display();
}
}
void addCubeCloud(pcl::PointCloud::Ptr cloud_in, const lib3dv::stixel& stixel, uint8_t r, uint8_t g, uint8_t b){
//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(o) + "-" + boost::lexical_cast(s));
float step_stix=0.02;
pcl::PointXYZRGB padd(r,g,b);
for(padd.x=stixel.m_x;padd.xpoints.push_back(padd);
padd.y = stixel.m_y+stixel.m_dy;
cloud_in->points.push_back(padd);
//up
padd.z = stixel.m_height+stixel.m_z;
cloud_in->points.push_back(padd);
padd.y = stixel.m_y;
cloud_in->points.push_back(padd);
}
for(padd.y=stixel.m_y;padd.ypoints.push_back(padd);
padd.x = stixel.m_x+stixel.m_dx;
cloud_in->points.push_back(padd);
//up
padd.z = stixel.m_height+stixel.m_z;
cloud_in->points.push_back(padd);
padd.x = stixel.m_x;
cloud_in->points.push_back(padd);
}
for(padd.z = stixel.m_z;padd.zpoints.push_back(padd);
padd.x = stixel.m_x+stixel.m_dx;
cloud_in->points.push_back(padd);
padd.y = stixel.m_y+stixel.m_dy;
cloud_in->points.push_back(padd);
padd.x = stixel.m_x;
cloud_in->points.push_back(padd);
}
}
display::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) :
m_downsample(downsample),
m_area_step(area_step),
m_log_level(log_level),
m_image_viewers_closed(0),
m_3d_viewer_closed(false),
m_show_points(true),
m_show_terrain(true),
m_show_terrain_gm(true),
m_show_obstacles(true),
m_show_motion(true),
m_show_classification(true),
m_stopped(false),
m_updated(false)
{
lib3dv::calibration::camera_intrinsics downsampled_intrinsics = intrinsics;
downsampled_intrinsics.m_u0 /= downsample;
downsampled_intrinsics.m_v0 /= downsample;
downsampled_intrinsics.m_ku /= downsample;
downsampled_intrinsics.m_kv /= downsample;
m_ipm = boost::shared_ptr(new lib3dv::inverse_perspective_mapping(downsampled_intrinsics, position, orientation, baseline));
m_pose_utils = boost::shared_ptr(new lib3dv::pose_utils(position, orientation));
m_me_orientations.first.m_yaw = m_me_orientations.first.m_pitch = m_me_orientations.first.m_roll = 0;
m_me_orientations.second.m_yaw = m_me_orientations.second.m_pitch = m_me_orientations.second.m_roll = 0;
m_td_orientations.first = m_td_orientations.second = boost::posix_time::time_duration(0,0,0);
#ifdef _MSC_VER
screen_width=GetSystemMetrics(SM_CXSCREEN);
screen_height=GetSystemMetrics(SM_CYSCREEN);
#else
Display* disp = XOpenDisplay(NULL);
Screen* scrn = DefaultScreenOfDisplay(disp);
screen_height = scrn->height;
screen_width = scrn->width;
XCloseDisplay(disp);
#endif
}
void display::update(boost::shared_ptr image)
{
boost::mutex::scoped_lock l(m_update_mutex);
m_image_buffers[image->m_type - 1].second = image;
m_updated = true;
m_update_condition.notify_all();
}
void display::update(boost::shared_ptr terrain)
{
boost::mutex::scoped_lock l(m_update_mutex);
m_terrain_buffers.second = terrain;
m_updated = true;
m_update_condition.notify_all();
}
void display::update(boost::shared_ptr > obstacles)
{
boost::mutex::scoped_lock l(m_update_mutex);
m_obstacles_buffers.second = obstacles;
m_updated = true;
m_update_condition.notify_all();
}
void display::update(boost::shared_ptr motion)
{
boost::mutex::scoped_lock l(m_update_mutex);
m_motion_buffers.second = motion;
m_updated = true;
m_update_condition.notify_all();
}
void display::update(boost::shared_ptr classification)
{
boost::mutex::scoped_lock l(m_update_mutex);
m_classification_buffers.second = classification;
m_updated = true;
m_update_condition.notify_all();
}
float r_coefficeints[] = {1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 1.0f};
float g_coefficeints[] = {0.0f, 0.5f, 1.0f, 1.0f, 0.5f, 0.0f, 0.5f, 0.0f, 0.0f};
float b_coefficeints[] = {0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.5f};
std::string image_names[] = {"Left rectified", "Right rectified", "Left raw", "Right raw", "DSI"};
void display::run()
{
do
{
boost::this_thread::interruption_point();
{
boost::mutex::scoped_lock l(m_update_mutex);
while(!m_updated && !m_stopped)
m_update_condition.wait(l);
if(m_stopped)
break;
m_updated = false;
for(unsigned int i = 0; i < lib3dv::image::type::NUM - 1; ++i)
if(m_image_buffers[i].second)
{
std::swap(m_image_buffers[i].first, m_image_buffers[i].second);
m_image_buffers[i].second.reset();
}
if(m_terrain_buffers.second)
{
std::swap(m_terrain_buffers.first, m_terrain_buffers.second);
m_terrain_buffers.second.reset();
}
if(m_obstacles_buffers.second)
{
std::swap(m_obstacles_buffers.first, m_obstacles_buffers.second);
m_obstacles_buffers.second.reset();
}
if(m_motion_buffers.second)
{
std::swap(m_motion_buffers.first, m_motion_buffers.second);
m_motion_buffers.second.reset();
}
if(m_classification_buffers.second)
{
std::swap(m_classification_buffers.first, m_classification_buffers.second);
m_classification_buffers.second.reset();
}
}
for(unsigned int i = 0; i < lib3dv::image::type::NUM - 1; ++i)
if(m_image_buffers[i].first && !m_image_viewers_closed[i])
{
if(m_image_buffers[i].first->m_type == lib3dv::image::type::RIGHT_RECTIFIED)
m_right_rectified = m_image_buffers[i].first;
if(m_image_buffers[i].first->m_type == lib3dv::image::type::DSI)
m_dsi = m_image_buffers[i].first;
if(!m_image_viewers[i])
m_image_viewers[i] = boost::shared_ptr(new pcl::visualization::ImageViewer());
m_image_viewers[i]->setWindowTitle(image_names[i] + " - " + boost::posix_time::to_simple_string(m_image_buffers[i].first->m_timestamp));
switch(m_image_buffers[i].first->m_format)
{
case (lib3dv::image::format::MONO8) :
case (lib3dv::image::format::BAYER8_BGGR) :
case (lib3dv::image::format::BAYER8_GRBG) :
case (lib3dv::image::format::BAYER8_RGGB) :
case (lib3dv::image::format::BAYER8_GBRG) :
{
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);
if(m_classification_buffers.first && m_show_classification && m_image_buffers[i].first->m_type == lib3dv::image::type::RIGHT_RECTIFIED)
{
m_image_viewers[i]->removeLayer("rectangles");
m_image_viewers[i]->removeLayer("car");
m_image_viewers[i]->removeLayer("pedestrian");
int offset=m_image_buffers[i].first->m_height+27-screen_height;
if (offset<0)
offset=0;
for(int s=0; s < m_classification_buffers.first->m_candidates.size(); s++)
{
const lib3dv::candidate& cand = m_classification_buffers.first->m_candidates[s];
if (cand.m_category==lib3dv::category_type::PEDESTRIAN)
{
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);
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);
}
else if (cand.m_category==lib3dv::category_type::CAR)
{
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);
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);
}
}
}
break;
}
case (lib3dv::image::format::MONO16) :
case (lib3dv::image::format::BAYER16_BGGR) :
case (lib3dv::image::format::BAYER16_GRBG) :
case (lib3dv::image::format::BAYER16_RGGB) :
case (lib3dv::image::format::BAYER16_GBRG) :
{
if(m_image_buffers[i].first->m_type == lib3dv::image::type::DSI)
m_image_viewers[i]->showShortImage(reinterpret_cast(m_image_buffers[i].first->m_buffer.data()), m_image_buffers[i].first->m_width, m_image_buffers[i].first->m_height, 0, 0x7F00);
else
m_image_viewers[i]->showShortImage(reinterpret_cast(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);
break;
}
case (lib3dv::image::format::RGB24) :
{
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);
if(m_classification_buffers.first && m_show_classification && m_image_buffers[i].first->m_type == lib3dv::image::type::RIGHT_RECTIFIED)
{
m_image_viewers[i]->removeLayer("rectangles");
m_image_viewers[i]->removeLayer("car");
m_image_viewers[i]->removeLayer("pedestrian");
int offset=m_image_buffers[i].first->m_height+27-screen_height;
if (offset<0)
offset=0;
for(int s=0; s < m_classification_buffers.first->m_candidates.size(); s++)
{
const lib3dv::candidate& cand = m_classification_buffers.first->m_candidates[s];
if (cand.m_category==lib3dv::category_type::PEDESTRIAN)
{
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);
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);
}
else if (cand.m_category==lib3dv::category_type::CAR)
{
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);
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);
}
}
}
break;
}
}
m_image_viewers[i]->spinOnce();
if(m_image_viewers[i]->wasStopped())
{
m_image_viewers[i]->close();
m_image_viewers[i].reset();
m_image_viewers_closed[i] = true;
}
boost::mutex::scoped_lock l(m_update_mutex);
m_image_buffers[i].first.reset();
}
if((m_terrain_buffers.first || m_obstacles_buffers.first || m_dsi) && !m_3d_viewer_closed)
{
if(!m_3d_viewer)
{
int argc = 0;
m_3d_viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer(argc, (char**)NULL, std::string("Point Cloud"), interactor_style_3dv::New()));
//FIXME: get camera params from device calibration
m_3d_viewer->setBackgroundColor (0, 0, 0);
m_3d_viewer->addCoordinateSystem (1.0);
m_3d_viewer->initCameraParameters();
m_3d_viewer->setCameraPosition(-3.0, 0.0, 6.0, 1.03093, -0.140037, 1.86098, 0.715357, 0.012124, 0.698653);
m_3d_viewer->setCameraFieldOfView(0.8575);
m_3d_viewer->setCameraClipDistances(-100, 300);
m_3d_viewer->setShowFPS(false);
m_3d_viewer->registerKeyboardCallback(on_keyboard_event, (void*)this);
}
if(m_dsi && m_show_points)
{
std::vector world_points;
world_points.reserve(m_dsi->m_width * m_dsi->m_height);
(*m_ipm)(m_dsi, world_points);
if(m_right_rectified)
{
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
cloud->reserve(world_points.size());
if(m_right_rectified->m_format == lib3dv::image::format::MONO8)
{
const uint8_t* buffer = m_right_rectified->m_buffer.data();
for(unsigned int i = 0; i < m_dsi->m_height; ++i)
for(unsigned int j = 0; j < m_dsi->m_width; ++j)
{
const uint32_t dsi_px = i * m_dsi->m_width + j;
const uint32_t right_px = m_downsample * (i * (m_dsi->m_width * m_downsample) + j);
const lib3dv::point3& wp = world_points[dsi_px];
if(wp.m_x < std::numeric_limits::max() && wp.m_y < std::numeric_limits::max() && wp.m_z < std::numeric_limits::max())
{
uint8_t val = buffer[right_px];
pcl::PointXYZRGB p(val, val, val);
p.x = wp.m_x;
p.y = wp.m_y;
p.z = wp.m_z;
cloud->points.push_back(p);
}
}
}
if(m_right_rectified->m_format == lib3dv::image::format::RGB24)
{
const uint8_t* buffer = m_right_rectified->m_buffer.data();
for(unsigned int i = 0; i < m_dsi->m_height; ++i)
for(unsigned int j = 0; j < m_dsi->m_width; ++j)
{
const uint32_t dsi_px = i * m_dsi->m_width + j;
const uint32_t right_px = m_downsample * (i * (m_dsi->m_width * 3 * m_downsample) + j * 3);
const lib3dv::point3& wp = world_points[dsi_px];
if(wp.m_x < std::numeric_limits::max() && wp.m_y < std::numeric_limits::max() && wp.m_z < std::numeric_limits::max())
{
uint8_t r = buffer[right_px];
uint8_t g = buffer[right_px + 1];
uint8_t b = buffer[right_px + 2];
pcl::PointXYZRGB p(r, g, b);
p.x = wp.m_x;
p.y = wp.m_y;
p.z = wp.m_z;
cloud->points.push_back(p);
}
}
}
if(!m_3d_viewer->updatePointCloud(cloud, "cloud"))
{
m_3d_viewer->addPointCloud(cloud, "cloud");
m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
}
m_right_rectified.reset();
}
else
{
// pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
// cloud->reserve(world_points.size());
//
// for(unsigned int w = 0; w < world_points.size(); ++w)
// {
// const lib3dv::point3& wp = world_points[w];
//
// if(wp.m_x < std::numeric_limits::max() && wp.m_y < std::numeric_limits::max() && wp.m_z < std::numeric_limits::max())
// {
// pcl::PointXYZ p;
//
// p.x = wp.m_x;
// p.y = wp.m_y;
// p.z = wp.m_z;
//
// cloud->points.push_back(p);
// }
// }
//
// if(!m_3d_viewer->updatePointCloud(cloud, "cloud"))
// {
// m_3d_viewer->addPointCloud(cloud, "cloud");
// m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
// }
}
m_dsi.reset();
}
else if(!m_show_points)
m_3d_viewer->removePointCloud("cloud");
//m_3d_viewer->removeAllShapes();
std::vector< std::vector > tcolumn_points;
if(m_terrain_buffers.first && m_show_terrain)
{
unsigned int dim_x;
unsigned int dim_y;
std::vector matrix_terrain_index;
if(m_show_terrain_gm && m_terrain_buffers.first->m_data.size()>4)
{
//std::cout << "terrain_size: "<< m_terrain_buffers.first->m_data.size() << std::endl;
float x_min=std::numeric_limits::max(),x_max=-std::numeric_limits::max(),y_min=std::numeric_limits::max(),y_max=-std::numeric_limits::max();
for(unsigned int t = 0; t < m_terrain_buffers.first->m_data.size(); ++t){
//std::cout<<"(x,y)"<m_data[t].m_x<<" "<m_data[t].m_y<m_data[t].m_x>x_max)
x_max = m_terrain_buffers.first->m_data[t].m_x;
if(m_terrain_buffers.first->m_data[t].m_xm_data[t].m_x;
if(m_terrain_buffers.first->m_data[t].m_y>y_max)
y_max = m_terrain_buffers.first->m_data[t].m_y;
if(m_terrain_buffers.first->m_data[t].m_ym_data[t].m_y;
}
dim_x=(x_max-x_min)/m_area_step + 1;
dim_y=(y_max-y_min)/m_area_step + 1;
//std::cout << "x_min:"<::max());
for(unsigned int t = 0; t < m_terrain_buffers.first->m_data.size(); ++t)
{
lib3dv::point3 ptt = m_terrain_buffers.first->m_data[t];
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);
if(index < dim_x*dim_y)
matrix_terrain_index[index] = t;
}
/*
for(int w=0;w-100){
std::cout<<"("<::Ptr cloud(new pcl::PointCloud);
cloud->reserve(m_terrain_buffers.first->m_data.size());
for(unsigned int t = 0; t < m_terrain_buffers.first->m_data.size(); ++t)
{
pcl::PointXYZRGB p(0,255,0);
p.x = m_terrain_buffers.first->m_data[t].m_x;
p.y = m_terrain_buffers.first->m_data[t].m_y;
p.z = m_terrain_buffers.first->m_data[t].m_z;
cloud->points.push_back(p);
}
if (m_show_terrain_gm && m_terrain_buffers.first->m_data.size()>4)
{
for (int c = 0; c < dim_x; c++)
{
for (int cc = 0; cc < dim_y; cc++)
{
pcl::PointXYZRGB p1,p2;
const std::vector& terr_data= m_terrain_buffers.first->m_data;
if(matrix_terrain_index[cc+c*dim_y]!=std::numeric_limits::max())
{
//valid point
unsigned int p1_index = matrix_terrain_index[cc+c*dim_y];
p1.x = terr_data[p1_index].m_x;
p1.y = terr_data[p1_index].m_y;
p1.z = terr_data[p1_index].m_z;
if(cc::max() )
{
int p2_index=matrix_terrain_index[cc+1+c*dim_y];
p2.x = terr_data[p2_index].m_x;
p2.y = terr_data[p2_index].m_y;
p2.z = terr_data[p2_index].m_z;
std::string tid = "TRLH-"+ boost::lexical_cast(c)+"-"+ boost::lexical_cast(cc);
pcl::PointXYZRGB pmed(0,255,0);
float step_terr = 0.02;
pmed.x=p1.x;
pmed.z=p1.z;
float dz = step_terr*(p2.z-p1.z)/(p2.y-p1.y);
for(pmed.y=p1.y+step_terr, pmed.z=p1.z+dz;pmed.ypoints.push_back(pmed);
}
}
if(c::max() )
{
int p2_index=matrix_terrain_index[cc+(c+1)*dim_y];
std::string tid = "TRLV-"+ boost::lexical_cast(c)+"-"+ boost::lexical_cast(cc);
p2.x = terr_data[p2_index].m_x;
p2.y = terr_data[p2_index].m_y;
p2.z = terr_data[p2_index].m_z;
pcl::PointXYZRGB pmed(0,255,0);
float step_terr = 0.02;
pmed.y=p1.y;
pmed.z=p1.z;
float dz = step_terr*(p2.z-p1.z)/(p2.x-p1.x);
for(pmed.x=p1.x+step_terr, pmed.z=p1.z+dz;pmed.xpoints.push_back(pmed);
}
}
}
}
}
}
if(!m_3d_viewer->updatePointCloud(cloud, "terrain"))
{
m_3d_viewer->addPointCloud(cloud, "terrain");
m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "terrain");
}
} else if(!m_show_terrain)
{
m_3d_viewer->removePointCloud("terrain");
}
if(m_obstacles_buffers.first && m_show_obstacles)
{
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
for(unsigned int o = 0; o < m_obstacles_buffers.first->size(); ++o)
{
const lib3dv::obstacle& obstacle = (*m_obstacles_buffers.first)[o];
uint8_t c = obstacle.m_guid % 9;
float r = r_coefficeints[c];
float g = g_coefficeints[c];
float b = b_coefficeints[c];
for(unsigned int s = 0; s < obstacle.m_data.size(); ++s)
{
const lib3dv::stixel& stixel = obstacle.m_data[s];
//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(o) + "-" + boost::lexical_cast(s));
addCubeCloud(cloud,stixel,r*255,g*255,b*255);
}
}
if(!m_3d_viewer->updatePointCloud(cloud, "obstacle"))
{
m_3d_viewer->addPointCloud(cloud, "obstacle");
m_3d_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "obstacle");
}
} else if(!m_show_obstacles){
m_3d_viewer->removePointCloud("obstacle");
}
if(m_motion_buffers.first && m_show_motion)
{
if(m_motion_buffers.first->m_poses.size()==0)
{
m_me_orientations.first.m_yaw = m_me_orientations.first.m_pitch = m_me_orientations.first.m_roll = 0;
m_td_orientations.first = boost::posix_time::time_duration(0,0,0);
m_3d_viewer->removeShape("orientation");
m_3d_viewer->removeShape("orientation_rate");
m_3d_viewer->removeShape("orientation_acc");
}
else
{
std::swap(m_td_orientations.first, m_td_orientations.second);
std::swap(m_me_orientations.first, m_me_orientations.second);
boost::posix_time::time_duration& prev_pose_td = m_td_orientations.first;
boost::posix_time::time_duration& pose_td = m_td_orientations.second;
float & yaw = m_me_orientations.second.m_yaw;
float & pitch = m_me_orientations.second.m_pitch;
float & roll = m_me_orientations.second.m_roll;
float & prev_yaw = m_me_orientations.first.m_yaw;
float & prev_pitch = m_me_orientations.first.m_pitch;
float & prev_roll = m_me_orientations.first.m_roll;
for (std::vector::const_iterator p = m_motion_buffers.first->m_poses.begin(); p != m_motion_buffers.first->m_poses.end(); p++)
if ( p->m_type == lib3dv::pose::type::RELATIVE_POSE )
{
m_pose_utils->extract_orientation(*p,yaw,pitch,roll);
pose_td = p->m_timestamp - boost::posix_time::ptime(boost::gregorian::date(1970,1,1));
}
float dt = std::fabs((double)pose_td.total_microseconds() - (double)prev_pose_td.total_microseconds())/1e6;
std::string orientation_acc = "yaw_acc: " + boost::lexical_cast((yaw - prev_yaw)/pow(dt,2.0f))
+ ", pitch_acc: " + boost::lexical_cast((pitch - prev_pitch)/pow(dt,2.0f)) +
+ ", roll_acc: " + boost::lexical_cast((roll - prev_roll)/pow(dt,2.0f)) + " [rad/s^2]";
std::string orientation_rate = "yaw rate: " + boost::lexical_cast((yaw - prev_yaw)/dt)
+ ", pitch rate: " + boost::lexical_cast((pitch - prev_pitch)/dt) +
+ ", roll rate: " + boost::lexical_cast((roll - prev_roll)/dt) + " [rad/s]";
std::string orientation = "yaw: " + boost::lexical_cast(yaw)
+ ", pitch: " + boost::lexical_cast(pitch) +
+ ", roll: " + boost::lexical_cast(roll) + " [rad]";
m_3d_viewer->removeShape("orientation");
m_3d_viewer->removeShape("orientation_rate");
m_3d_viewer->removeShape("orientation_acc");
if (!m_3d_viewer->updateText(orientation, 5, 25, 1.0, 1.0, 1.0, "orientation"))
m_3d_viewer->addText(orientation, 5, 25, 1.0, 1.0, 1.0, "orientation");
if (!m_3d_viewer->updateText(orientation_rate, 5, 15, 1.0, 1.0, 1.0, "orientation_rate"))
m_3d_viewer->addText(orientation_rate, 5, 15, 1.0, 1.0, 1.0, "orientation_rate");
if (!m_3d_viewer->updateText(orientation_acc, 5, 5, 1.0, 1.0, 1.0, "orientation_acc"))
m_3d_viewer->addText(orientation_acc, 5, 5, 1.0, 1.0, 1.0, "orientation_acc");
}
}
else if (!m_show_motion)
{
m_3d_viewer->removeShape("angles");
m_3d_viewer->removeShape("orientation");
m_3d_viewer->removeShape("orientation_rate");
m_3d_viewer->removeShape("orientation_acc");
}
m_3d_viewer->spinOnce();
if(m_3d_viewer->wasStopped())
{
m_3d_viewer->close();
m_3d_viewer.reset();
m_3d_viewer_closed = true;
}
boost::mutex::scoped_lock l(m_update_mutex);
m_terrain_buffers.first.reset();
m_obstacles_buffers.first.reset();
m_motion_buffers.first.reset();
}
} while(true);
for(unsigned int i = 0; i < lib3dv::image::type::NUM - 1; ++i)
if(m_image_viewers[i])
{
m_image_viewers[i]->close();
m_image_viewers[i].reset();
}
if(m_3d_viewer)
{
m_3d_viewer->close();
m_3d_viewer.reset();
}
}
void display::stop()
{
boost::mutex::scoped_lock l(m_update_mutex);
m_stopped = true;
m_update_condition.notify_all();
}