CSetOfObjectsPtr stock_objects::Hokuyo_URG()
{
CSetOfObjectsPtr ret = CSetOfObjects::Create();
{
CBoxPtr base = CBox::Create(TPoint3D(-0.025,-0.025,-0.0575),TPoint3D(0.025,0.025,-0.0185));
base->setColor(0.7,0.7,0.7);
ret->insert(base);
}
{
CCylinderPtr cyl1 = CCylinder::Create(0.02,0.02,0.01);
cyl1->setColor(0,0,0);
cyl1->setPose(CPoint3D(0,0,-0.014));
ret->insert(cyl1);
}
{
CCylinderPtr cyl2 = CCylinder::Create(0.02,0.0175,0.01);
cyl2->setColor(0,0,0);
cyl2->setPose(CPoint3D(0,0,-0.004));
ret->insert(cyl2);
}
{
CCylinderPtr cyl3 = CCylinder::Create(0.0175,0.0175,0.01);
cyl3->setColor(0,0,0);
cyl3->setPose(CPoint3D(0,0,0.004));
ret->insert(cyl3);
}
return ret;
}
CSetOfObjectsPtr stock_objects::Hokuyo_UTM()
{
CSetOfObjectsPtr ret = CSetOfObjects::Create();
{
CBoxPtr base = CBox::Create(TPoint3D(-0.03,-0.03,-0.055),TPoint3D(0.03,0.03,-0.014));
base->setColor(0,0,0);
ret->insert(base);
}
{
CCylinderPtr cyl1 = CCylinder::Create(0.028,0.024,0.028);
cyl1->setColor(0,0,0);
cyl1->setPose(CPose3D(0,0,-0.014));
ret->insert(cyl1);
}
{
CCylinderPtr cyl2 = CCylinder::Create(0.028,0.028,0.01);
cyl2->setColor(1,69/255.0,0);
cyl2->setPose(CPoint3D(0,0,0.014));
ret->insert(cyl2);
}
{
CCylinderPtr cyl3 = CCylinder::Create(0.028,0.028,0.01);
cyl3->setColor(0,0,0);
cyl3->setPose(CPoint3D(0,0,0.024));
ret->insert(cyl3);
}
return ret;
}
CSetOfObjectsPtr stock_objects::CornerXYZEye()
{
CSetOfObjectsPtr ret = CSetOfObjects::Create();
CPose3D rotation;
CArrowPtr obj = CArrow::Create(
0,0,0,
1.0,0,0,
0.25f,0.02f,0.05f);
obj->setColor(1,0,0);
ret->insert( obj );
obj = CArrow::Create(
0,0,0,
0,1.0,0,
0.25f,0.02f,0.05f);
obj->setColor(0,1,0);
ret->insert( obj );
obj = CArrow::Create(
0,0,-1.0,
0,0,0,
0.25f,0.02f,0.05f);
obj->setColor(0,0,1);
ret->insert( obj );
return ret;
}
/** Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call
* mrpt::poses::CPose3DQuatPDF::getAs3DObject */
CSetOfObjectsPtr CSetOfObjects::posePDF2opengl(const CPose3DQuatPDF &o)
{
CSetOfObjectsPtr outObj = CSetOfObjects::Create();
if (IS_CLASS(&o,CPose3DQuatPDFGaussian))
{
const CPose3DQuatPDFGaussian *p = static_cast<const CPose3DQuatPDFGaussian*>(&o);
opengl::CEllipsoidPtr obj = opengl::CEllipsoid::Create();
obj->setPose( p->mean);
obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2,2)==0 ? 2:3);
obj->setQuantiles(3);
obj->enableDrawSolid3D(false);
obj->setColor(POSE_COLOR);
outObj->insert( obj );
opengl::CSetOfObjectsPtr axes = opengl::stock_objects::CornerXYZ();
axes->setPose(CPose3D(p->mean));
axes->setScale(POSE_AXIS_SCALE);
outObj->insert(axes);
}
return outObj;
}
CSetOfObjectsPtr stock_objects::CornerXYZSimple(float scale, float lineWidth)
{
CSetOfObjectsPtr ret = CSetOfObjects::Create();
{
CSimpleLinePtr lin = CSimpleLine::Create();
lin->setLineWidth(lineWidth);
lin->setColor(1,0,0);
lin->setLineCoords(0,0,0, scale,0,0);
ret->insert(lin);
}
{
CSimpleLinePtr lin = CSimpleLine::Create();
lin->setLineWidth(lineWidth);
lin->setColor(0,1,0);
lin->setLineCoords(0,0,0, 0,scale,0);
ret->insert(lin);
}
{
CSimpleLinePtr lin = CSimpleLine::Create();
lin->setLineWidth(lineWidth);
lin->setColor(0,0,1);
lin->setLineCoords(0,0,0, 0,0,scale);
ret->insert(lin);
}
return ret;
}
/*---------------------------------------------------------------
CornerXYZ
---------------------------------------------------------------*/
CSetOfObjectsPtr stock_objects::CornerXYZ(float scale)
{
CSetOfObjectsPtr ret = CSetOfObjects::Create();
CArrowPtr obj = CArrow::Create(
0,0,0,
scale,0,0,
0.25f*scale,0.02f*scale,0.05f*scale);
obj->setColor(1,0,0);
ret->insert( obj );
obj = CArrow::Create(
0,0,0,
0,scale,0,
0.25f*scale,0.02f*scale,0.05f*scale);
obj->setColor(0,1,0);
ret->insert( obj );
obj = CArrow::Create(
0,0,0,
0,0,scale,
0.25f*scale,0.02f*scale,0.05f*scale);
obj->setColor(0,0,1);
ret->insert( obj );
return ret;
}
CSetOfObjectsPtr stock_objects::Househam_Sprayer()
{
CSetOfObjectsPtr ret = CSetOfObjects::Create();
{
CBoxPtr cabin = CBox::Create(TPoint3D(0.878,0.723,-0.12),TPoint3D(-0.258,-0.723,-1.690));
cabin->setColor(0.7,0.7,0.7);
ret->insert(cabin);
}
{
CBoxPtr back = CBox::Create(TPoint3D(-0.258,0.723,-0.72),TPoint3D(-5.938,-0.723,-1.690));
back->setColor(1,1,1);
ret->insert(back);
}
{
CBoxPtr boomAxis = CBox::Create(TPoint3D(-5.938,0.723,-1.0),TPoint3D(-6.189,-0.723,-1.690));
boomAxis->setColor(0,0,0);
ret->insert(boomAxis);
}
{
CBoxPtr boom1 = CBox::Create(TPoint3D(-5.938,0.723,-1.0),TPoint3D(-6.189,11.277,-1.620));
boom1->setColor(0,1,0);
ret->insert(boom1);
}
{
CBoxPtr boom2 = CBox::Create(TPoint3D(-5.938,-0.723,-1.0),TPoint3D(-6.189,-11.277,-1.620));
boom2->setColor(0,1,0);
ret->insert(boom2);
}
{
CCylinderPtr cyl1 = CCylinder::Create(0.716,0.716,0.387,30);
cyl1->setColor(0,0,0);
cyl1->setPose(CPose3D(-0.710,0.923,-2.480,0,0,DEG2RAD(90)));
ret->insert(cyl1);
}
{
CCylinderPtr cyl2 = CCylinder::Create(0.716,0.716,0.387,30);
cyl2->setColor(0,0,0);
cyl2->setPose(CPose3D(-3.937,0.923,-2.480,0,0,DEG2RAD(90)));
ret->insert(cyl2);
}
{
CCylinderPtr cyl1 = CCylinder::Create(0.716,0.716,0.387,30);
cyl1->setColor(0,0,0);
cyl1->setPose(CPose3D(-0.710,-0.423,-2.480,0,0,DEG2RAD(90)));
ret->insert(cyl1);
}
{
CCylinderPtr cyl2 = CCylinder::Create(0.716,0.716,0.387,30);
cyl2->setColor(0,0,0);
cyl2->setPose(CPose3D(-3.937,-0.423,-2.480,0,0,DEG2RAD(90)));
ret->insert(cyl2);
}
return ret;
}
/** Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call
* mrpt::poses::CPointPDF::getAs3DObject */
CSetOfObjectsPtr CSetOfObjects::posePDF2opengl(const CPointPDF &o)
{
CSetOfObjectsPtr outObj = CSetOfObjects::Create();
if (IS_CLASS(&o,CPointPDFSOG))
{
const CPointPDFSOG *p = static_cast<const CPointPDFSOG*>(&o);
// For each gaussian node
for (CPointPDFSOG::CListGaussianModes::const_iterator it = p->begin(); it!= p->end();++it)
{
opengl::CEllipsoidPtr obj = opengl::CEllipsoid::Create();
obj->setPose( it->val.mean);
obj->setCovMatrix(it->val.cov, it->val.cov(2,2)==0 ? 2:3);
obj->setQuantiles(3);
obj->enableDrawSolid3D(false);
obj->setColor(POINT_COLOR);
outObj->insert( obj );
} // end for each gaussian node
}
else if (IS_CLASS(&o, CPointPDFGaussian))
{
const CPointPDFGaussian *p = static_cast<const CPointPDFGaussian*>(&o);
CEllipsoidPtr obj = CEllipsoid::Create();
obj->setLocation(p->mean);
obj->setCovMatrix(p->cov, p->cov(2,2)==0 ? 2:3);
obj->setColor(POINT_COLOR);
obj->setQuantiles(3);
obj->enableDrawSolid3D(false);
outObj->insert( obj );
}
else if (IS_CLASS(&o, CPointPDFParticles))
{
const CPointPDFParticles *p = static_cast<const CPointPDFParticles*>(&o);
mrpt::opengl::CPointCloudPtr obj = mrpt::opengl::CPointCloud::Create();
const size_t N=p->size();
obj->resize(N);
obj->setColor(POINT_COLOR);
for (size_t i=0;i<N;i++)
obj->setPoint(
i,
p->m_particles[i].d->x,
p->m_particles[i].d->y,
p->m_particles[i].d->z );
outObj->insert( obj );
}
return outObj;
}
void generateObjects(CSetOfObjectsPtr &world)
{
CSpherePtr sph=CSphere::Create(0.5);
sph->setLocation(0,0,0);
sph->setColor(1,0,0);
world->insert(sph);
CDiskPtr pln= opengl::CDisk::Create();
pln->setDiskRadius(2);
pln->setPose(CPose3D(0,0,0,0,DEG2RAD(5),DEG2RAD(5)));
pln->setColor(0.8,0,0);
world->insert(pln);
{
CDiskPtr pln= opengl::CDisk::Create();
pln->setDiskRadius(2);
pln->setPose(CPose3D(0,0,0,DEG2RAD(30),DEG2RAD(-20),DEG2RAD(-2)));
pln->setColor(0.9,0,0);
world->insert(pln);
}
}
/*---------------------------------------------------------------
BumblebeeCamera
---------------------------------------------------------------*/
CSetOfObjectsPtr stock_objects::BumblebeeCamera()
{
CSetOfObjectsPtr camera = opengl::CSetOfObjects::Create();
CPolyhedronPtr rect = opengl::CPolyhedron::CreateCubicPrism( -0.02, 0.14, -0.02, 0.02, 0, -0.04 );
rect->setColor( 1, 0.8, 0 );
camera->insert( rect );
CCylinderPtr lCam = opengl::CCylinder::Create( 0.01,0.01, 0.003, 10, 10 );
lCam->setColor( 1,0,0 );
CCylinderPtr rCam = opengl::CCylinder::Create( 0.01,0.01, 0.003, 10, 10 );
rCam->setPose( CPose3D(0.12,0,0) );
rCam->setColor( 0,0,0 );
camera->insert( lCam );
camera->insert( rCam );
return camera;
}
/*---------------------------------------------------------------
RobotRhodon
---------------------------------------------------------------*/
CSetOfObjectsPtr stock_objects::RobotRhodon()
{
CSetOfObjectsPtr ret = CSetOfObjects::Create();
float height = 0;
vector<TPoint2D> level1;
level1.push_back(TPoint2D(0.31, 0));
level1.push_back(TPoint2D(0.22, 0.24));
level1.push_back(TPoint2D(-0.22, 0.24));
level1.push_back(TPoint2D(-0.31, 0));
level1.push_back(TPoint2D(-0.22, -0.24));
level1.push_back(TPoint2D(0.22, -0.24));
CPolyhedronPtr obj1 = opengl::CPolyhedron::CreateCustomPrism(level1, 0.38);
obj1->setLocation(0,0,height);
height+=0.38;
obj1->setColor(0.6,0.6,0.6);
ret->insert( obj1 );
vector<TPoint2D> level2;
level2.push_back(TPoint2D(0.16, 0.21));
level2.push_back(TPoint2D(-0.16, 0.21));
level2.push_back(TPoint2D(-0.16, -0.21));
level2.push_back(TPoint2D(0.16, -0.21));
CPolyhedronPtr obj2 = opengl::CPolyhedron::CreateCustomPrism(level2, 0.35);
obj2->setLocation(0,0,height);
height+=0.35;
obj2->setColor(0.2,0.2,0.2);
ret->insert( obj2 );
vector<TPoint2D> level3;
level3.push_back(TPoint2D(-0.12, 0.12));
level3.push_back(TPoint2D(-0.16, 0.12));
level3.push_back(TPoint2D(-0.16, -0.12));
level3.push_back(TPoint2D(-0.12, -0.12));
CPolyhedronPtr obj3 = opengl::CPolyhedron::CreateCustomPrism(level3, 1);
obj3->setLocation(0,0,height);
//height+=1;
obj3->setColor(0.6,0.6,0.6);
ret->insert( obj3 );
opengl::CCylinderPtr obj4 = opengl::CCylinder::Create(0.05, 0.05, 0.4, 20, 20);
obj4->setLocation(0,0,0.73);
obj4->setColor(0,0,0.9);
ret->insert( obj4 );
opengl::CCylinderPtr obj5 = opengl::CCylinder::Create(0.05, 0.05, 0.4, 20, 20);
obj5->setPose(CPose3D(0.32,0,0.89,0,-1,0));
obj5->setColor(0,0,0.9);
ret->insert( obj5 );
return ret;
}
pair<CPolyhedronPtr,CPolyhedronPtr> addPairOfPolys(CPolyhedronPtr p1,CPolyhedronPtr p2,CSetOfObjectsPtr &objs,double x,double y) {
p1->makeConvexPolygons();
p2->makeConvexPolygons();
#ifdef PAIRED_RANDOM_POSES
CPose3D pose=CPose3D(x,y,randomZ(),randomAngle(),randomAngle(),randomAngle());
p1->setPose(pose);
p2->setPose(pose);
#else
CPose3D pose1=CPose3D(x,y,randomZ(),randomAngle(),randomAngle(),randomAngle());
CPose3D pose2=CPose3D(x,y,randomZ(),randomAngle(),randomAngle(),randomAngle());
p1->setPose(pose1);
p2->setPose(pose2);
#endif
randomColor(p1,0.5);
randomColor(p2,0.5);
objs->insert(p1);
objs->insert(p2);
return make_pair(p1,p2);
}
// 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";
}
/** Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call
* mrpt::poses::CPosePDF::getAs3DObject */
CSetOfObjectsPtr CSetOfObjects::posePDF2opengl(const CPosePDF &o)
{
CSetOfObjectsPtr outObj = CSetOfObjects::Create();
if (IS_CLASS(&o,CPosePDFSOG))
{
const CPosePDFSOG *p = static_cast<const CPosePDFSOG*>(&o);
opengl::CSetOfLinesPtr lins = opengl::CSetOfLines::Create();
lins->setColor(0,0,1,0.6);
lins->setLineWidth(POSE_TAIL_WIDTH);
for (CPosePDFSOG::const_iterator it=p->begin();it!=p->end();++it)
{
opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
ellip->setPose( CPose3D((it)->mean.x(), (it)->mean.y(), 0) );
ellip->setCovMatrix((it)->cov, 2 /* x y */ );
ellip->setColor(POSE_COLOR,0.6);
ellip->setQuantiles(3);
ellip->enableDrawSolid3D(false);
outObj->insert(ellip);
lins->appendLine(
(it)->mean.x(), (it)->mean.y(), 0,
(it)->mean.x() + POSE_TAIL_LENGTH * cos((it)->mean.phi()) , (it)->mean.y() + POSE_TAIL_LENGTH * sin((it)->mean.phi()) , 0
);
}
outObj->insert(lins);
} else
if (IS_CLASS(&o,CPosePDFGaussian))
{
const CPosePDFGaussian *p = static_cast<const CPosePDFGaussian*>(&o);
opengl::CSetOfLinesPtr lins = opengl::CSetOfLines::Create();
lins->setColor(POSE_COLOR,0.6);
lins->setLineWidth(POSE_TAIL_WIDTH);
opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
ellip->setPose( CPose3D(p->mean.x(), p->mean.y(), 0) );
ellip->setCovMatrix(p->cov, 2 /* x y */ );
ellip->setColor(POSE_COLOR,0.6);
ellip->setQuantiles(3);
ellip->enableDrawSolid3D(false);
outObj->insert(ellip);
lins->appendLine(
p->mean.x(), p->mean.y(), 0,
p->mean.x() + POSE_TAIL_LENGTH * cos(p->mean.phi()) , p->mean.y() + POSE_TAIL_LENGTH * sin(p->mean.phi()) , 0
);
outObj->insert(lins);
} else
if (IS_CLASS(&o,CPosePDFParticles))
{
const CPosePDFParticles *p = static_cast<const CPosePDFParticles*>(&o);
opengl::CPointCloudPtr pnts = opengl::CPointCloud::Create();
pnts->setColor(POSE_COLOR,0.6);
pnts->setPointSize(POSE_POINT_SIZE);
opengl::CSetOfLinesPtr lins = opengl::CSetOfLines::Create();
lins->setColor(POSE_COLOR,0.6);
lins->setLineWidth(POSE_TAIL_WIDTH);
for (size_t i=0;i<p->size();++i)
{
const mrpt::poses::CPose2D *po = p->m_particles[i].d;
pnts->insertPoint(po->x(), po->y(), 0);
lins->appendLine(
po->x(), po->y(), 0,
po->x() + POSE_TAIL_LENGTH * cos(po->phi()), po->y() + POSE_TAIL_LENGTH * sin(po->phi()), 0
);
}
outObj->insert(pnts);
outObj->insert(lins);
}
return outObj;
}
/*---------------------------------------------------------------
RobotGiraff
---------------------------------------------------------------*/
CSetOfObjectsPtr stock_objects::RobotGiraff()
{
CSetOfObjectsPtr ret = CSetOfObjects::Create();
float height = 0;
//Base
vector<TPoint2D> level1;
level1.push_back(TPoint2D(0.31, 0));
level1.push_back(TPoint2D(0.22, 0.24));
level1.push_back(TPoint2D(-0.22, 0.24));
level1.push_back(TPoint2D(-0.31, 0));
level1.push_back(TPoint2D(-0.22, -0.24));
level1.push_back(TPoint2D(0.22, -0.24));
CPolyhedronPtr obj1 = opengl::CPolyhedron::CreateCustomPrism(level1, 0.23);
obj1->setLocation(0,0,height);
height+=0.23;
obj1->setColor(1.0,0.6,0.0);
ret->insert( obj1 );
//Electronic's cage
vector<TPoint2D> level2;
level2.push_back(TPoint2D(0.13, 0.1));
level2.push_back(TPoint2D(-0.13, 0.1));
level2.push_back(TPoint2D(-0.13, -0.1));
level2.push_back(TPoint2D(0.13, -0.1));
CPolyhedronPtr obj2 = opengl::CPolyhedron::CreateCustomPrism(level2, 0.45);
obj2->setLocation(0,0,height);
height+=0.45;
obj2->setColor(1.0,0.6,0.2);
ret->insert( obj2 );
//Neck
vector<TPoint2D> level3;
level3.push_back(TPoint2D(0.03, 0.03));
level3.push_back(TPoint2D(-0.03, 0.03));
level3.push_back(TPoint2D(-0.03, -0.03));
level3.push_back(TPoint2D(0.03, -0.03));
CPolyhedronPtr obj3 = opengl::CPolyhedron::CreateCustomPrism(level3, 0.55);
obj3->setLocation(0,0,height);
height+=0.55;
obj3->setColor(0.6,0.6,0.6);
ret->insert( obj3 );
//Screen
vector<TPoint2D> level4;
level4.push_back(TPoint2D(0.03, 0.11));
level4.push_back(TPoint2D(-0.03, 0.11));
level4.push_back(TPoint2D(-0.03, -0.11));
level4.push_back(TPoint2D(0.03, -0.11));
CPolyhedronPtr obj4 = opengl::CPolyhedron::CreateCustomPrism(level4, 0.4);
obj4->setLocation(0,0,height);
height+=0.4;
obj4->setColor(1.0,0.6,0.0);
ret->insert( obj4 );
return ret;
}
/** Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call
* mrpt::poses::CPose3DPDF::getAs3DObject */
CSetOfObjectsPtr CSetOfObjects::posePDF2opengl(const CPose3DPDF &o)
{
CSetOfObjectsPtr outObj = CSetOfObjects::Create();
if (IS_CLASS(&o,CPose3DPDFSOG))
{
const CPose3DPDFSOG *p = static_cast<const CPose3DPDFSOG*>(&o);
// For each gaussian node
for (CPose3DPDFSOG::const_iterator it = p->begin(); it!= p->end();++it)
{
opengl::CEllipsoidPtr obj = opengl::CEllipsoid::Create();
obj->setPose( it->val.mean);
obj->setCovMatrix(CMatrixDouble(it->val.cov), it->val.cov(2,2)==0 ? 2:3);
obj->setQuantiles(3);
obj->enableDrawSolid3D(false);
obj->setColor(POSE_COLOR);
outObj->insert( obj );
opengl::CSetOfObjectsPtr axes = opengl::stock_objects::CornerXYZ();
axes->setPose(it->val.mean);
axes->setScale(POSE_AXIS_SCALE);
outObj->insert(axes);
} // end for each gaussian node
}
else
if (IS_CLASS(&o,CPose3DPDFGaussian))
{
const CPose3DPDFGaussian *p = static_cast<const CPose3DPDFGaussian*>(&o);
opengl::CEllipsoidPtr obj = opengl::CEllipsoid::Create();
obj->setPose( p->mean);
obj->setCovMatrix(CMatrixDouble(p->cov), p->cov(2,2)==0 ? 2:3);
obj->setQuantiles(3);
obj->enableDrawSolid3D(false);
obj->setColor(POSE_COLOR);
outObj->insert( obj );
opengl::CSetOfObjectsPtr axes = opengl::stock_objects::CornerXYZ();
axes->setPose(p->mean);
axes->setScale(POSE_AXIS_SCALE);
outObj->insert(axes);
}
else
if (IS_CLASS(&o,CPose3DPDFParticles))
{
const CPose3DPDFParticles *p = static_cast<const CPose3DPDFParticles*>(&o);
for (size_t i=0;i<p->size();i++)
{
opengl::CSetOfObjectsPtr axes = opengl::stock_objects::CornerXYZSimple(POSE_AXIS_SCALE);
axes->setPose(*p->m_particles[i].d);
outObj->insert(axes);
}
}
return outObj;
}
/*-------------------------------------------------------------
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
/*---------------------------------------------------------------
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;
}
