MapViewer.cc
Go to the documentation of this file.
00001 #include "ptam/MapViewer.h"
00002 #include "ptam/MapPoint.h"
00003 #include "ptam/KeyFrame.h"
00004 #include "ptam/LevelHelpers.h"
00005 #include <iomanip>
00006 
00007 #include <cvd/gl_helpers.h>
00008 
00009 using namespace CVD;
00010 using namespace std;
00011 
00012 
00013 MapViewer::MapViewer(Map &map, GLWindow2 &glw):
00014       mMap(map), mGLWindow(glw)
00015 {
00016   mse3ViewerFromWorld = 
00017       SE3<>::exp(makeVector(0,0,2,0,0,0)) * SE3<>::exp(makeVector(0,0,0,0.8 * M_PI,0,0));
00018 }
00019 
00020 void MapViewer::DrawMapDots()
00021 {
00022   SetupFrustum();
00023   SetupModelView();
00024 
00025   int nForMass = 0;
00026   glColor3f(0,1,1);
00027   glPointSize(3);
00028   glBegin(GL_POINTS);
00029   mv3MassCenter = Zeros;
00030   for(size_t i=0; i<mMap.vpPoints.size(); i++)
00031   {
00032     Vector<3> v3Pos = mMap.vpPoints[i]->v3WorldPos;
00033     glColor(gavLevelColors[mMap.vpPoints[i]->nSourceLevel]);
00034     if(v3Pos * v3Pos < 10000)
00035     {
00036       nForMass++;
00037       mv3MassCenter += v3Pos;
00038     }
00039     glVertex(v3Pos);
00040   }
00041   glEnd();
00042   mv3MassCenter = mv3MassCenter / (0.1 + nForMass);
00043 }
00044 
00045 
00046 void MapViewer::DrawGrid()
00047 {
00048   SetupFrustum();
00049   SetupModelView();
00050   glLineWidth(1);
00051 
00052   glBegin(GL_LINES);
00053 
00054   // Draw a larger grid around the outside..
00055   double dGridInterval = 0.1;
00056 
00057   double dMin = -100.0 * dGridInterval;
00058   double dMax =  100.0 * dGridInterval;
00059 
00060   for(int x=-10;x<=10;x+=1)
00061   {
00062     if(x==0)
00063       glColor3f(1,1,1);
00064     else
00065       glColor3f(0.3,0.3,0.3);
00066     glVertex3d((double)x * 10 * dGridInterval, dMin, 0.0);
00067     glVertex3d((double)x * 10 * dGridInterval, dMax, 0.0);
00068   }
00069   for(int y=-10;y<=10;y+=1)
00070   {
00071     if(y==0)
00072       glColor3f(1,1,1);
00073     else
00074       glColor3f(0.3,0.3,0.3);
00075     glVertex3d(dMin, (double)y * 10 *  dGridInterval, 0.0);
00076     glVertex3d(dMax, (double)y * 10 * dGridInterval, 0.0);
00077   }
00078 
00079   glEnd();
00080 
00081   glBegin(GL_LINES);
00082   dMin = -10.0 * dGridInterval;
00083   dMax =  10.0 * dGridInterval;
00084 
00085   for(int x=-10;x<=10;x++)
00086   {
00087     if(x==0)
00088       glColor3f(1,1,1);
00089     else
00090       glColor3f(0.5,0.5,0.5);
00091 
00092     glVertex3d((double)x * dGridInterval, dMin, 0.0);
00093     glVertex3d((double)x * dGridInterval, dMax, 0.0);
00094   }
00095   for(int y=-10;y<=10;y++)
00096   {
00097     if(y==0)
00098       glColor3f(1,1,1);
00099     else
00100       glColor3f(0.5,0.5,0.5);
00101     glVertex3d(dMin, (double)y * dGridInterval, 0.0);
00102     glVertex3d(dMax, (double)y * dGridInterval, 0.0);
00103   }
00104 
00105   glColor3f(1,0,0);
00106   glVertex3d(0,0,0);
00107   glVertex3d(1,0,0);
00108   glColor3f(0,1,0);
00109   glVertex3d(0,0,0);
00110   glVertex3d(0,1,0);
00111   glColor3f(1,1,1);
00112   glVertex3d(0,0,0);
00113   glVertex3d(0,0,1);
00114   glEnd();
00115 
00116   //   glColor3f(0.8,0.8,0.8);
00117   //   glRasterPos3f(1.1,0,0);
00118   //   mGLWindow.PrintString("x");
00119   //   glRasterPos3f(0,1.1,0);
00120   //   mGLWindow.PrintString("y");
00121   //   glRasterPos3f(0,0,1.1);
00122   //   mGLWindow.PrintString("z");
00123 }
00124 
00125 void MapViewer::DrawMap(SE3<> se3CamFromWorld)
00126 {
00127   mMessageForUser.str(""); // Wipe the user message clean
00128 
00129   // Update viewer position according to mouse input:
00130   {
00131     pair<Vector<6>, Vector<6> > pv6 = mGLWindow.GetMousePoseUpdate();
00132     SE3<> se3CamFromMC;
00133     se3CamFromMC.get_translation() = mse3ViewerFromWorld * mv3MassCenter;
00134     mse3ViewerFromWorld = SE3<>::exp(pv6.first) * 
00135         se3CamFromMC * SE3<>::exp(pv6.second) * se3CamFromMC.inverse() * mse3ViewerFromWorld;
00136   }
00137 
00138   mGLWindow.SetupViewport();
00139   glClearColor(0,0,0,0);
00140   glClearDepth(1);
00141   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00142   glMatrixMode(GL_PROJECTION);
00143   glLoadIdentity();
00144   glEnable(GL_POINT_SMOOTH);
00145   glEnable(GL_LINE_SMOOTH);
00146   glEnable(GL_BLEND);
00147   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00148   glColorMask(1,1,1,1);
00149 
00150   glEnable(GL_DEPTH_TEST);
00151   DrawGrid();
00152   DrawMapDots();
00153   DrawCamera(se3CamFromWorld);
00154   for(size_t i=0; i<mMap.vpKeyFrames.size(); i++)
00155     DrawCamera(mMap.vpKeyFrames[i]->se3CfromW, true);
00156   glDisable(GL_DEPTH_TEST);
00157   glMatrixMode(GL_MODELVIEW);
00158   glLoadIdentity();
00159 
00160   mMessageForUser << " Map: " << mMap.vpPoints.size() << "P, " << mMap.vpKeyFrames.size() << "KF";
00161   mMessageForUser << setprecision(4);
00162   mMessageForUser << "   Camera Pos: " << se3CamFromWorld.inverse().get_translation();
00163 }
00164 
00165 string MapViewer::GetMessageForUser()
00166 {
00167   return mMessageForUser.str();
00168 }
00169 
00170 void MapViewer::SetupFrustum()
00171 {
00172   glMatrixMode(GL_PROJECTION);  
00173   glLoadIdentity();
00174   double zNear = 0.03;
00175   glFrustum(-zNear, zNear, 0.75*zNear,-0.75*zNear,zNear,50);
00176   glScalef(1,1,-1);
00177   return;
00178 };
00179 
00180 void MapViewer::SetupModelView(SE3<> se3WorldFromCurrent)
00181 {
00182   glMatrixMode(GL_MODELVIEW);  
00183   glLoadIdentity();
00184   glMultMatrix(mse3ViewerFromWorld * se3WorldFromCurrent);
00185   return;
00186 };
00187 
00188 void MapViewer::DrawCamera(SE3<> se3CfromW, bool bSmall)
00189 {
00190 
00191   SetupModelView(se3CfromW.inverse());
00192   SetupFrustum();
00193 
00194   if(bSmall)
00195     glLineWidth(1);
00196   else
00197     glLineWidth(3);
00198 
00199   glBegin(GL_LINES);
00200   glColor3f(1,0,0);
00201   glVertex3f(0.0f, 0.0f, 0.0f);
00202   glVertex3f(0.1f, 0.0f, 0.0f);
00203   glColor3f(0,1,0);
00204   glVertex3f(0.0f, 0.0f, 0.0f);
00205   glVertex3f(0.0f, 0.1f, 0.0f);
00206   glColor3f(1,1,1);
00207   glVertex3f(0.0f, 0.0f, 0.0f);
00208   glVertex3f(0.0f, 0.0f, 0.1f);
00209   glEnd();
00210 
00211 
00212   if(!bSmall)
00213   {
00214     glLineWidth(1);
00215     glColor3f(0.5,0.5,0.5);
00216     SetupModelView();
00217     Vector<2> v2CamPosXY = se3CfromW.inverse().get_translation().slice<0,2>();
00218     glBegin(GL_LINES);
00219     glColor3f(1,1,1);
00220     glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] + 0.04);
00221     glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] - 0.04);
00222     glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] - 0.04);
00223     glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] + 0.04);
00224     glEnd();
00225   }
00226 
00227 }
00228 
00229 


ptam
Author(s): Markus Achtelik , Stephan Weiss , Simon Lynen
autogenerated on Sun Oct 5 2014 23:52:33