예제 #1
0
/*-------------------------------------------------------------
					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
예제 #2
0
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