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