// The thread in charge of creating and updating the GUI windows.
void CStereoOdometryEstimator::thread_gui()
{
VERBOSE_LEVEL(1) << "[CStereoOdometryEstimator::thread_gui] Thread alive.\n";
mrpt::gui::CDisplayWindow3DPtr win;
CPose3D current_pose(0,0,0,DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-100) );
try
{
// Initialize a 3D view with 2 panels for L & R images:
win = mrpt::gui::CDisplayWindow3D::Create("Stereo Odometry",1200,465);
// Subscribe to key events:
TObserverWindowEvents my_observer( m_win_keyhit );
my_observer.observeBegin(*win);
vector<COpenGLViewportPtr> gl_views(3);
{
COpenGLScenePtr &theScene = win->get3DSceneAndLock();
gl_views[0] = theScene->getViewport("main");
ASSERT_(gl_views[0])
gl_views[1] = theScene->createViewport("right_image");
ASSERT_(gl_views[1])
gl_views[2] = theScene->createViewport("pose3D");
// Assign sizes:
gl_views[0]->setViewportPosition( 0, 0, .33,1.);
gl_views[1]->setViewportPosition(.33, 0, .33,1.);
gl_views[2]->setViewportPosition(.66, 0, .33,1.);
// Prepare 3D view
CSetOfObjectsPtr cam = stock_objects::BumblebeeCamera();
cam->setPose(CPose3D());
cam->setName("bumblebee");
CSetOfLinesPtr path = CSetOfLines::Create();
path->setName("path");
path->setColor(0,1,0);
path->appendLine(0,0,0,0,0,0);
gl_views[2]->insert( stock_objects::CornerXYZ() );
gl_views[2]->insert( CGridPlaneXY::Create(-100,100,-100,100) );
gl_views[2]->insert( cam );
gl_views[2]->insert( path );
theScene->enableFollowCamera(true);
// IMPORTANT!!! IF NOT UNLOCKED, THE WINDOW WILL NOT BE UPDATED!
win->unlockAccess3DScene();
}
mrpt::system::TTimeStamp last_timestamp = INVALID_TIMESTAMP;
// Main loop in this GUI thread
while (!m_threads_must_close)
{
// Check if we have new stuff to show:
m_gui_info_cs.enter();
if (m_gui_info->timestamp==last_timestamp || m_gui_info->timestamp==INVALID_TIMESTAMP)
{ // Not a new or valid structure, keep waiting.
m_gui_info_cs.leave();
mrpt::system::sleep(2);
continue;
}
last_timestamp = m_gui_info->timestamp;
// We have new stuff:
// Firstly, make a quick copy of the data to release the critical section ASAP and without
// letting any chance of raising an exception in the way:
TInfoForTheGUI info;
info.swap(*m_gui_info);
m_gui_info_cs.leave();
// From now on, we can safely work on our copy "info"
//----------------------------------------------------
// Set the image to color mode
// ----------------------------------------------------
info.img_left.colorImageInPlace();
info.img_right.colorImageInPlace();
// Update camera position
current_pose += info.inc_pose;
// Draw optional stuff on the base images:
// ----------------------------------------------------
m_profiler.enter("gui.draw_lr_pairings");
if (!info.draw_pairings_all.empty())
{
const size_t nFeats = info.draw_pairings_all.size();
static TColor colors[8] = {
TColor(0xFF,0x00,0x00),
TColor(0xFF,0xFF,0x00),
TColor(0x00,0x00,0xFF),
TColor(0x00,0xFF,0xFF),
TColor(0x00,0x00,0x00),
TColor(0xFF,0xFF,0xFF),
TColor(0xFF,0x00,0xFF),
TColor(0xFF,0x80,0x80)
};
const int rect_w = 2;
for(size_t i=0;i<nFeats;++i)
{
// (X,Y) coordinates in the left/right images for this pairing:
const TPixelCoord &ptL = info.draw_pairings_all[i].first;
const TPixelCoord &ptR = info.draw_pairings_all[i].second;
TColor thisColor;
if( params_detect.detect_method == TDetectParams::dmORB )
thisColor = colors[info.draw_pairings_ids[i]&0x07];
else
thisColor = colors[i&0x07];
info.img_left.rectangle( ptL.x-rect_w, ptL.y-rect_w, ptL.x+rect_w, ptL.y+rect_w, thisColor );
info.img_right.rectangle( ptR.x-rect_w, ptR.y-rect_w, ptR.x+rect_w, ptR.y+rect_w, thisColor );
}
}
m_profiler.leave("gui.draw_lr_pairings");
// Draw tracked feats as lines in L/R
m_profiler.enter("gui.draw_tracking");
if (!info.stats_tracked_feats.empty())
{
static TColor colors[8] = {
TColor(0xFF,0x00,0x00),
TColor(0xFF,0xFF,0x00),
TColor(0x00,0x00,0xFF),
TColor(0x00,0xFF,0xFF),
TColor(0x00,0x00,0x00),
TColor(0xFF,0xFF,0xFF),
TColor(0xFF,0x00,0xFF),
TColor(0xFF,0x80,0x80)
};
const size_t N = info.stats_tracked_feats.size();
for (size_t i=0;i<N;i++)
{
info.img_left.line(
info.stats_tracked_feats[i].px_pL.x,
info.stats_tracked_feats[i].px_pL.y,
info.stats_tracked_feats[i].px_cL.x,
info.stats_tracked_feats[i].px_cL.y,
colors[i&0x07] );
info.img_right.line(
info.stats_tracked_feats[i].px_pR.x,
info.stats_tracked_feats[i].px_pR.y,
info.stats_tracked_feats[i].px_cR.x,
info.stats_tracked_feats[i].px_cR.y,
colors[i&0x07] );
}
}
m_profiler.leave("gui.draw_tracking");
// ------------------------------------------------------------
// LOCK 3D scene & update images & text labels:
m_profiler.enter("gui.locked-update");
win->get3DSceneAndLock();
gl_views[0]->setImageView_fast(info.img_left); // "_fast()" has "move semantics" and destroy origin objects
gl_views[1]->setImageView_fast(info.img_right);
CRenderizablePtr obj = gl_views[2]->getByName("bumblebee");
if(obj)
{
CSetOfObjectsPtr bb = static_cast<CSetOfObjectsPtr>(obj);
bb->setPose( current_pose ); // Update camera position:
CCamera &theCam = gl_views[2]->getCamera();
theCam.setPointingAt( bb->getPoseX(), bb->getPoseY(), bb->getPoseZ() );
}
obj = gl_views[2]->getByName("path");
if(obj)
{
CSetOfLinesPtr p = static_cast<CSetOfLinesPtr>(obj);
p->appendLineStrip( current_pose.x(), current_pose.y(), current_pose.z() ); // Update camera path
}
// Text:
win->addTextMessage(
5,5,
mrpt::system::dateTimeLocalToString(info.timestamp),
TColorf(1,1,1),"mono",10, mrpt::opengl::FILL, 1
);
// Text:
/** /
{
string sStatsFeats = string("Raw FAST features per octave: ");
string sThresholds;
for (size_t i=0;i<info.stats_feats_per_octave.size();i++) {
sStatsFeats += mrpt::format("%u/",static_cast<unsigned int>(info.stats_feats_per_octave[i]) );
sThresholds += mrpt::format("%i/",static_cast<int>(info.stats_FAST_thresholds_per_octave[i]) );
}
sStatsFeats += string(" | Thresholds: ") + sThresholds;
win->addTextMessage(
5,5+1*15,
sStatsFeats,
TColorf(1,1,1),"mono",10, mrpt::opengl::FILL, 2
);
}
/ * */
// Text:
win->addTextMessage(
5,5+1*15,
"Detected features: " + info.text_msg_from_detect + "\n",
TColorf(1,1,1),"mono",10, mrpt::opengl::FILL, 2
);
// Text:
win->addTextMessage(
5,5+2*15,
info.text_msg_from_lr_match,
TColorf(1,1,1),"mono",10, mrpt::opengl::FILL, 3
);
// Text:
win->addTextMessage(
5,5+3*15,
info.text_msg_from_conseq_match,
TColorf(1,1,1),"mono",10, mrpt::opengl::FILL, 4
);
// Text:
win->addTextMessage(
5,5+5*15,
mrpt::format("Current pose (x,y,z,yaw,pitch,roll) = (%.2f,%.2f,%.2f,%.1fd,%.1fd,%.1fd)",
current_pose.x(), current_pose.y(), current_pose.z(),
RAD2DEG(current_pose.yaw()), RAD2DEG(current_pose.pitch()), RAD2DEG(current_pose.roll()))
+ info.text_msg_from_optimization,
TColorf(1,1,1),"mono",10, mrpt::opengl::FILL, 5
);
win->unlockAccess3DScene();
m_profiler.leave("gui.locked-update");
win->repaint();
} // end while() main loop
}
catch(std::exception &e)
{
cerr << "[CStereoOdometryEstimator::thread_gui] Thread exit for exception:\n" << e.what() << endl;
}
if (win) {
win.clear();
mrpt::system::sleep(20); // Leave time to the wxWidgets thread to clean up
}
VERBOSE_LEVEL(1) << "[CStereoOdometryEstimator::thread_gui] Thread closed.\n";
}
void CSkeletonTracker::processPreviewNone()
{
using namespace mrpt::opengl;
// show laser scan
if( m_showPreview )
{
if( !m_win )
{
string caption = string("Preview of ") + m_sensorLabel;
m_win = mrpt::gui::CDisplayWindow3D::Create( caption, 800, 600 );
COpenGLScenePtr & scene = m_win->get3DSceneAndLock();
scene->insert( CGridPlaneXZ::Create(-3,3,0,5,-1.5 ) );
// set camera parameters
m_win->setCameraElevationDeg(-90);
m_win->setCameraAzimuthDeg(90);
m_win->setCameraZoom(4);
m_win->setCameraPointingToPoint(0,0,0);
// insert initial body
CSetOfObjectsPtr body = CSetOfObjects::Create();
body->setName("body");
for(int i = 0; i < NUM_JOINTS; ++i)
{
CSpherePtr sph = CSphere::Create(0.03);
sph->setColor(0,1,0);
sph->setName( jointNames[i] );
body->insert(sph);
}
// insert initial lines
CSetOfLinesPtr lines = CSetOfLines::Create();
lines->setName("lines");
lines->setColor(0,0,1);
body->insert(lines);
scene->insert(body);
m_win->unlockAccess3DScene();
}
if( m_win && m_win->isOpen() )
{
COpenGLScenePtr & scene = m_win->get3DSceneAndLock();
{
// update joints positions
CSetOfObjectsPtr body = static_cast<CSetOfObjectsPtr>( scene->getByName("body") );
ASSERT_( body )
for(int i = 0; i < NUM_JOINTS; ++i)
{
CSpherePtr s = static_cast<CSpherePtr>( body->getByName( jointNames[i] ) );
CPoint3D sphPos;
if( i == 0 )
sphPos = CPoint3D(0,0,0);
else
{
m_joint_theta[i] += M_2PI/(10*(NUM_JOINTS-1));
sphPos.x( 0.5*cos( m_joint_theta[i] ) );
sphPos.y( 0.5*sin( m_joint_theta[i] ) );
sphPos.z( 0.0 );
}
s->setPose( sphPos );
s->setColor( 1, 0, 0 );
s->setRadius( i == 0 ? 0.07 : 0.03 );
} // end-for
} // end-get3DSceneAndLock
m_win->unlockAccess3DScene();
m_win->forceRepaint();
} // end if
} // end if
/*-------------------------------------------------------------
processPreviewNone
-------------------------------------------------------------*/
void CSkeletonTracker::processPreviewNone()
{
using namespace mrpt::opengl;
// show skeleton data
if( m_showPreview )
{
if( !m_win )
{
string caption = string("Preview of ") + m_sensorLabel;
m_win = mrpt::gui::CDisplayWindow3D::Create( caption, 800, 600 );
COpenGLScenePtr & scene = m_win->get3DSceneAndLock();
scene->insert( CGridPlaneXZ::Create(-3,3,0,5,-1.5 ) );
// set camera parameters
m_win->setCameraElevationDeg(-90);
m_win->setCameraAzimuthDeg(90);
m_win->setCameraZoom(4);
m_win->setCameraPointingToPoint(0,0,0);
// insert initial body
CSetOfObjectsPtr body = CSetOfObjects::Create();
body->setName("body");
for(int i = 0; i < NUM_JOINTS; ++i)
{
CSpherePtr sph = CSphere::Create(0.03f);
sph->setColor(0,1,0);
sph->setName( jointNames[i] );
body->insert(sph);
}
// insert initial lines
CSetOfLinesPtr lines = CSetOfLines::Create();
lines->setName("lines");
lines->setColor(0,0,1);
body->insert(lines);
scene->insert(body);
m_win->unlockAccess3DScene();
}
if( m_win && m_win->isOpen() )
{
COpenGLScenePtr & scene = m_win->get3DSceneAndLock();
{
m_win->addTextMessage( 0.35, 0.9,
"Please, adopt this position",
TColorf(1,1,1),"mono",10, mrpt::opengl::FILL,
0);
// insert translucid dummy and help text (it will go away when measurements are taken)
if( !scene->getByName("dummy") )
{
const double SCALE = 0.8;
const double BODY_RADIUS = 0.22*SCALE;
const double BODY_LENGTH = 0.8*SCALE;
const double ARM_RADIUS = 0.05*SCALE;
const double ARM_LENGTH = 0.4*SCALE;
const double LEG_RADIUS = 0.1*SCALE;
const double LEG_LENGTH = 0.8*SCALE;
const double HEAD_RADIUS = 0.15*SCALE;
const double ALPHA_CH = 0.8;
CSetOfObjectsPtr dummy = CSetOfObjects::Create();
dummy->setName("dummy");
dummy->setPose(math::TPose3D(0,0,0,0,0,DEG2RAD(-90)));
{
// head
CSpherePtr part = CSphere::Create(HEAD_RADIUS);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(0,0,0.5*BODY_LENGTH+HEAD_RADIUS,0,0,0));
dummy->insert(part);
}
{
// body
CCylinderPtr part = CCylinder::Create(BODY_RADIUS,BODY_RADIUS,BODY_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(0,0,-BODY_LENGTH/2,0,0,0));
dummy->insert(part);
}
{
// left arm 0
CCylinderPtr part = CCylinder::Create(ARM_RADIUS,ARM_RADIUS,ARM_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(-BODY_RADIUS,0,0.5*BODY_LENGTH-ARM_RADIUS,0,DEG2RAD(-90),0));
dummy->insert(part);
}
{
// left arm 1
CCylinderPtr part = CCylinder::Create(ARM_RADIUS,ARM_RADIUS,ARM_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(-BODY_RADIUS-ARM_LENGTH+ARM_RADIUS,0,0.5*BODY_LENGTH-ARM_RADIUS,0,0,0));
dummy->insert(part);
}
{
// right arm 0
CCylinderPtr part = CCylinder::Create(ARM_RADIUS,ARM_RADIUS,ARM_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(BODY_RADIUS,0,0.5*BODY_LENGTH-ARM_RADIUS,0,DEG2RAD(90),0));
dummy->insert(part);
}
{
// right arm 1
CCylinderPtr part = CCylinder::Create(ARM_RADIUS,ARM_RADIUS,ARM_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(BODY_RADIUS+ARM_LENGTH-ARM_RADIUS,0,0.5*BODY_LENGTH-ARM_RADIUS,0,0,0));
dummy->insert(part);
}
{
// left leg
CCylinderPtr part = CCylinder::Create(LEG_RADIUS,LEG_RADIUS,LEG_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(-BODY_RADIUS+LEG_RADIUS,0,-(0.5*BODY_LENGTH+LEG_LENGTH),0,0,0));
dummy->insert(part);
}
{
// right leg
CCylinderPtr part = CCylinder::Create(LEG_RADIUS,LEG_RADIUS,LEG_LENGTH);
part->setColor(1,1,1,ALPHA_CH);
part->setPose(math::TPose3D(BODY_RADIUS-LEG_RADIUS,0,-(0.5*BODY_LENGTH+LEG_LENGTH),0,0,0));
dummy->insert(part);
}
scene->insert(dummy);
} // end-if
else
{
CSetOfObjectsPtr dummy = static_cast<CSetOfObjectsPtr>( scene->getByName("dummy") );
dummy->setVisibility(true);
}
// update joints positions
CSetOfObjectsPtr body = static_cast<CSetOfObjectsPtr>( scene->getByName("body") );
ASSERT_( body )
for(int i = 0; i < NUM_JOINTS; ++i)
{
CSpherePtr s = static_cast<CSpherePtr>( body->getByName( jointNames[i] ) );
CPoint3D sphPos;
if( i == 0 )
sphPos = CPoint3D(0,0,0);
else
{
m_joint_theta[i] += M_2PI/(10*(NUM_JOINTS-1));
sphPos.x( 0.5*cos( m_joint_theta[i] ) );
sphPos.y( 0.5*sin( m_joint_theta[i] ) );
sphPos.z( 0.0 );
}
s->setPose( sphPos );
s->setColor( 1, 0, 0 );
s->setRadius( i == 0 ? 0.07 : 0.03 );
} // end-for
} // end-get3DSceneAndLock
m_win->unlockAccess3DScene();
m_win->forceRepaint();
} // end if
} // end if
/*---------------------------------------------------------------
RobotPioneer
---------------------------------------------------------------*/
CSetOfObjectsPtr stock_objects::RobotPioneer()
{
CSetOfObjectsPtr ret = CSetOfObjects::Create();
ret->setName("theRobot");
CSetOfTrianglesPtr obj = CSetOfTriangles::Create();
// Add triangles:
CSetOfTriangles::TTriangle trian;
trian.r[0]=trian.r[1]=trian.r[2]= 1;
trian.g[0]=trian.g[1]=trian.g[2]= 0;
trian.b[0]=trian.b[1]=trian.b[2]= 0;
trian.a[0]=trian.a[1]=trian.a[2]= 1;
trian.x[0] = 0.10f; trian.x[1] =-0.20f; trian.x[2] =-0.20f;
trian.y[0] =-0.10f; trian.y[1] = 0.10f; trian.y[2] =-0.10f;
trian.z[0] = 0.20f; trian.z[1] = 0.25f; trian.z[2] = 0.25f;
obj->insertTriangle( trian ); // 0
trian.x[0] = 0.10f; trian.x[1] = 0.10f; trian.x[2] =-0.20f;
trian.y[0] =-0.10f; trian.y[1] = 0.10f; trian.y[2] = 0.10f;
trian.z[0] = 0.20f; trian.z[1] = 0.20f; trian.z[2] = 0.25f;
obj->insertTriangle( trian ); // 1
//trian.r = 0.9f; trian.g = 0; trian.b = 0; trian.a = 1;
trian.x[0] = 0.10f; trian.x[1] = 0.10f; trian.x[2] = 0.10f;
trian.y[0] =-0.10f; trian.y[1] =-0.10f; trian.y[2] = 0.10f;
trian.z[0] = 0.05f; trian.z[1] = 0.20f; trian.z[2] = 0.20f;
obj->insertTriangle( trian ); // 2
trian.x[0] = 0.10f; trian.x[1] = 0.10f; trian.x[2] = 0.10f;
trian.y[0] =-0.10f; trian.y[1] = 0.10f; trian.y[2] = 0.10f;
trian.z[0] = 0.05f; trian.z[1] = 0.05f; trian.z[2] = 0.20f;
obj->insertTriangle( trian ); // 3
trian.x[0] =-0.20f; trian.x[1] =-0.20f; trian.x[2] =-0.20f;
trian.y[0] =-0.10f; trian.y[1] =-0.10f; trian.y[2] = 0.10f;
trian.z[0] = 0.05f; trian.z[1] = 0.25f; trian.z[2] = 0.25f;
obj->insertTriangle( trian ); // 2b
trian.x[0] =-0.20f; trian.x[1] =-0.20f; trian.x[2] =-0.20f;
trian.y[0] =-0.10f; trian.y[1] = 0.10f; trian.y[2] = 0.10f;
trian.z[0] = 0.05f; trian.z[1] = 0.05f; trian.z[2] = 0.25f;
obj->insertTriangle( trian ); // 3b
//trian.r = 0.8f; trian.g = 0; trian.b = 0; trian.a = 1;
trian.x[0] = 0.10f; trian.x[1] =-0.20f; trian.x[2] =-0.20f;
trian.y[0] =-0.10f; trian.y[1] =-0.10f; trian.y[2] =-0.10f;
trian.z[0] = 0.20f; trian.z[1] = 0.25f; trian.z[2] = 0.05f;
obj->insertTriangle( trian ); // 4
trian.x[0] = 0.10f; trian.x[1] = 0.10f; trian.x[2] =-0.20f;
trian.y[0] =-0.10f; trian.y[1] =-0.10f; trian.y[2] =-0.10f;
trian.z[0] = 0.20f; trian.z[1] = 0.05f; trian.z[2] = 0.05f;
obj->insertTriangle( trian ); // 5
trian.x[0] = 0.10f; trian.x[1] =-0.20f; trian.x[2] =-0.20f;
trian.y[0] = 0.10f; trian.y[1] = 0.10f; trian.y[2] = 0.10f;
trian.z[0] = 0.20f; trian.z[1] = 0.25f; trian.z[2] = 0.05f;
obj->insertTriangle( trian ); // 6
trian.x[0] = 0.10f; trian.x[1] = 0.10f; trian.x[2] =-0.20f;
trian.y[0] = 0.10f; trian.y[1] = 0.10f; trian.y[2] = 0.10f;
trian.z[0] = 0.20f; trian.z[1] = 0.05f; trian.z[2] = 0.05f;
obj->insertTriangle( trian ); // 7
trian.r[0]=trian.r[1]=trian.r[2]= 0.05f;
trian.g[0]=trian.g[1]=trian.g[2]= 0.05f;
trian.b[0]=trian.b[1]=trian.b[2]= 0.05f;
trian.a[0]=trian.a[1]=trian.a[2]= 1;
trian.x[0] = 0.00f; trian.x[1] = 0.00f; trian.x[2] = 0.05f;
trian.y[0] = 0.11f; trian.y[1] = 0.11f; trian.y[2] = 0.11f;
trian.z[0] = 0.00f; trian.z[1] = 0.10f; trian.z[2] = 0.05f;
obj->insertTriangle( trian ); // 8
trian.x[0] = 0.00f; trian.x[1] = 0.00f; trian.x[2] = -0.05f;
trian.y[0] = 0.11f; trian.y[1] = 0.11f; trian.y[2] = 0.11f;
trian.z[0] = 0.00f; trian.z[1] = 0.10f; trian.z[2] = 0.05f;
obj->insertTriangle( trian ); // 9
trian.x[0] = 0.00f; trian.x[1] = 0.00f; trian.x[2] = 0.05f;
trian.y[0] =-0.11f; trian.y[1] =-0.11f; trian.y[2] =-0.11f;
trian.z[0] = 0.00f; trian.z[1] = 0.10f; trian.z[2] = 0.05f;
obj->insertTriangle( trian ); // 10
trian.x[0] = 0.00f; trian.x[1] = 0.00f; trian.x[2] = -0.05f;
trian.y[0] =-0.11f; trian.y[1] =-0.11f; trian.y[2] =-0.11f;
trian.z[0] = 0.00f; trian.z[1] = 0.10f; trian.z[2] = 0.05f;
obj->insertTriangle( trian ); // 11
ret->insert( obj );
return ret;
}
