void MapView::plotCam(TooN::SE3<> droneToGlobal, bool xyCross, float thick, float len, float alpha)
{
glEnable(GL_DEPTH_TEST);
// draw cam
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glScaled(0.1,0.1,0.1);
CVD::glMultMatrix(mse3ViewerFromWorld * droneToGlobal);
SetupFrustum();
glLineWidth(thick*lineWidthFactor);
if(alpha < 1)
{
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
}
else
glDisable(GL_BLEND);
glBegin(GL_LINES);
glColor4f(1,0,0,alpha);
glVertex3f(-len, 0.0f, 0.0f);
glVertex3f(len, 0.0f, 0.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, -len, 0.0f);
glColor4f(0,1,0,alpha);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, len, 0.0f);
glColor4f(1,1,1,alpha);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, 0.0f, len);
glEnd();
if(xyCross)
{
glLineWidth(1*lineWidthFactor);
glColor4f(1,1,1, alpha);
SetupModelView();
TooN::Vector<2> v2CamPosXY = droneToGlobal.get_translation().slice<0,2>();
glBegin(GL_LINES);
glColor4f(1,1,1, alpha);
glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] + 0.04);
glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] - 0.04);
glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] - 0.04);
glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] + 0.04);
glEnd();
}
glDisable(GL_BLEND);
glDisable(GL_DEPTH_TEST);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
void MapView::drawTrail()
{
glEnable(GL_DEPTH_TEST);
// draw cam
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glScaled(0.1,0.1,0.1);
CVD::glMultMatrix(mse3ViewerFromWorld);
SetupFrustum();
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glLineWidth(3*lineWidthFactor);
glBegin(GL_LINES);
glColor4f(0,1,0,0.6);
TooN::Vector<3> PTAMScales = filter->getCurrentScales();
TooN::Vector<3> PTAMOffsets = filter->getCurrentOffsets().slice<0,3>();
for(unsigned int i=1;i<trailPoints.size();i++)
{
if(trailPoints[i].PTAMValid)
{
trailPoints[i].pointFilter = trailPoints[i].pointPTAM;
trailPoints[i].pointFilter[0] *= PTAMScales[0];
trailPoints[i].pointFilter[1] *= PTAMScales[1];
trailPoints[i].pointFilter[2] *= PTAMScales[2];
trailPoints[i].pointFilter += PTAMOffsets;
}
if(i > 1 && i < trailPoints.size()-1)
glVertex3f((float)trailPoints[i].pointFilter[0], (float)trailPoints[i].pointFilter[1], (float)trailPoints[i].pointFilter[2]);
glVertex3f((float)trailPoints[i].pointFilter[0], (float)trailPoints[i].pointFilter[1], (float)trailPoints[i].pointFilter[2]);
}
glEnd();
glDisable(GL_DEPTH_TEST);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
void MapView::plotMapPoints()
{
glEnable(GL_DEPTH_TEST);
// draw cam
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glScaled(0.1,0.1,0.1);
CVD::glMultMatrix(mse3ViewerFromWorld);
SetupFrustum();
float width = 3.0f;
float len = 0.04f;
glLineWidth(width*lineWidthFactor);
glBegin(GL_LINES);
glColor3f(1,0,0);
glLineWidth(5.0f);
std::vector<tvec3>* mpl = &(ptamWrapper->mapPointsTransformed);
for(unsigned int i=0;i<mpl->size();i++)
{
TooN::Vector<3> pos = (*mpl)[i];
glVertex3f((float)pos[0]-len, (float)pos[1], (float)pos[2]);
glVertex3f((float)pos[0]+len, (float)pos[1], (float)pos[2]);
glVertex3f((float)pos[0], (float)pos[1]-len, (float)pos[2]);
glVertex3f((float)pos[0], (float)pos[1]+len, (float)pos[2]);
glVertex3f((float)pos[0], (float)pos[1], (float)pos[2]-len);
glVertex3f((float)pos[0], (float)pos[1], (float)pos[2]+len);
}
glEnd();
glDisable(GL_DEPTH_TEST);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
void MapView::plotGrid()
{
// Update viewer position according to mouse input:
std::pair<TooN::Vector<6>, TooN::Vector<6> > pv6 = myGLWindow->GetMousePoseUpdate();
TooN::SE3<> se3CamFromMC;
se3CamFromMC.get_translation() = mse3ViewerFromWorld * TooN::makeVector(0,0,0);
mse3ViewerFromWorld = TooN::SE3<>::exp(pv6.first) *
se3CamFromMC * TooN::SE3<>::exp(pv6.second).inverse() * se3CamFromMC.inverse() * mse3ViewerFromWorld;
myGLWindow->SetupViewport();
glClearColor(0,0,0,0);
glClearDepth(1);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glEnable(GL_POINT_SMOOTH);
//glEnable(GL_LINE_SMOOTH);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColorMask(1,1,1,1);
glDisable(GL_DEPTH_TEST);
// draw grid
SetupFrustum();
SetupModelView();
glLineWidth(1*lineWidthFactor);
double gridBigSpacing = 1;
double gridSmallSpacing = 0.2;
double gridBigMax = 30;
double gridSmallMax = 5;
// fine grid
glBegin(GL_LINES);
for(double x=-gridSmallMax; x<=gridSmallMax;x+=gridSmallSpacing)
{
if(x != 0 && (x-floor(x)) > 0.1)
{
glColor3f(0.2f,0.2f,0.2f);
glVertex3d(x, -gridSmallMax, 0.0);
glVertex3d(x, gridSmallMax, 0.0);
}
}
for(double y=-gridSmallMax; y<=gridSmallMax;y+=gridSmallSpacing)
{
if(y != 0 && (y-floor(y)) > 0.1)
{
glColor3f(0.25f,0.25f,0.25f);
glVertex3d(-gridSmallMax,y, 0.0);
glVertex3d(gridSmallMax,y, 0.0);
}
}
glEnd();
//big grid
glLineWidth(2*lineWidthFactor);
glBegin(GL_LINES);
for(double x=-gridBigMax; x<=gridBigMax;x+=gridBigSpacing)
{
if(x != 0)
{
glColor3f(0.6f,0.6f,0.6f);
glVertex3d(x, -gridBigMax, 0.0);
glVertex3d(x, gridBigMax, 0.0);
}
}
for(double y=-gridBigMax; y<=gridBigMax;y+=gridBigSpacing)
{
if(y != 0)
{
glColor3f(0.6f,0.6f,0.6f);
glVertex3d(-gridBigMax,y, 0.0);
glVertex3d(gridBigMax,y, 0.0);
}
}
glEnd();
//xy lines
glLineWidth(2.5*lineWidthFactor);
glBegin(GL_LINES);
glColor3f(1,1,1);
glVertex3d(-gridBigMax,0, 0.0);
glVertex3d(gridBigMax,0, 0.0);
glVertex3d(0,-gridBigMax, 0.0);
glVertex3d(0,gridBigMax, 0.0);
// colored xy lines
glColor3f(1,0,0);
glVertex3d(0,0,0);
glVertex3d(1,0,0);
glColor3f(0,1,0);
glVertex3d(0,0,0);
glVertex3d(0,1,0);
glColor3f(1,1,1);
glVertex3d(0,0,0);
glVertex3d(0,0,1);
glEnd();
}
