working

venkateshbs · Oct 17, 2014 · 9f1a46b · 9f1a46b
1 parent ed45a6d
commit 9f1a46b
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 9 deletions.
diff --git a/lsd_slam_core/src/GUI.cpp b/lsd_slam_core/src/GUI.cpp
@@ -79,6 +79,7 @@ void GUI::addKeyframe(Keyframe * newFrame)
     }
     else
     {
+        newFrame->initId = keyframes.getReference().size();
         keyframes.getReference()[newFrame->id] = newFrame;
     }
 
@@ -91,11 +92,16 @@ void GUI::drawKeyframes()
 
     for(std::map<int, Keyframe *>::iterator i = keyframes.getReference().begin(); i != keyframes.getReference().end(); ++i)
     {
-        if(!i->second->hasVbo || i->second->needsUpdate)
+        //Don't render first five, according to original code
+        if(i->second->initId >= 5)
         {
-            i->second->computeVbo();
+            if(!i->second->hasVbo || i->second->needsUpdate)
+            {
+                i->second->computeVbo();
+            }
+            i->second->drawPoints();
+            i->second->drawCamera();
         }
-        i->second->draw();
     }
 
     lock.unlock();

diff --git a/lsd_slam_core/src/IOWrapper/Pangolin/Keyframe.h b/lsd_slam_core/src/IOWrapper/Pangolin/Keyframe.h
@@ -72,10 +72,10 @@ class Keyframe
 
             memcpy(camToWorld.data(), &camToWorldRaw[0], 7*sizeof(float));
 
-            float my_scaledTH = 1;
-            float my_absTH = 1;
+            float my_scaledTH = 1e-3;
+            float my_absTH = 1e-1;
             float my_scale = camToWorld.scale();
-            int my_minNearSupport = 5;
+            int my_minNearSupport = 9;
             int my_sparsifyFactor = 1;
 
             InputPointDense * originalInput = (InputPointDense *)pointData;
@@ -154,7 +154,7 @@ class Keyframe
             needsUpdate = false;
         }
 
-        void draw()
+        void drawPoints()
         {
             assert(hasVbo);
 
@@ -180,7 +180,36 @@ class Keyframe
             glPopMatrix();
         }
 
+        void drawCamera()
+        {
+            glPushMatrix();
+            Sophus::Matrix4f m = camToWorld.matrix();
+            glMultMatrixf((GLfloat*) m.data());
+            glColor3f(1, 0, 0);
+            glBegin(GL_LINES);
+                glVertex3f(0, 0, 0);
+                glVertex3f(0.05 * (0 - cx) / fx, 0.05 * (0 - cy) / fy, 0.05);
+                glVertex3f(0, 0, 0);
+                glVertex3f(0.05 * (0 - cx) / fx, 0.05 * (height - 1 - cy) / fy, 0.05);
+                glVertex3f(0, 0, 0);
+                glVertex3f(0.05 * (width - 1 - cx) / fx, 0.05 * (height - 1 - cy) / fy, 0.05);
+                glVertex3f(0, 0, 0);
+                glVertex3f(0.05 * (width - 1 - cx) / fx, 0.05 * (0 - cy) / fy, 0.05);
+                glVertex3f(0.05 * (width - 1 - cx) / fx, 0.05 * (0 - cy) / fy, 0.05);
+                glVertex3f(0.05 * (width - 1 - cx) / fx, 0.05 * (height - 1 - cy) / fy, 0.05);
+                glVertex3f(0.05 * (width - 1 - cx) / fx, 0.05 * (height - 1 - cy) / fy, 0.05);
+                glVertex3f(0.05 * (0 - cx) / fx, 0.05 * (height - 1 - cy) / fy, 0.05);
+                glVertex3f(0.05 * (0 - cx) / fx, 0.05 * (height - 1 - cy) / fy, 0.05);
+                glVertex3f(0.05 * (0 - cx) / fx, 0.05 * (0 - cy) / fy, 0.05);
+                glVertex3f(0.05 * (0 - cx) / fx, 0.05 * (0 - cy) / fy, 0.05);
+                glVertex3f(0.05 * (width - 1 - cx) / fx, 0.05 * (0 - cy) / fy, 0.05);
+            glEnd();
+            glPopMatrix();
+            glColor3f(1, 1, 1);
+        }
+
         int id;
+        int initId;
         uint64_t time;
         bool isKeyframe;
 

diff --git a/lsd_slam_core/src/util/settings.cpp b/lsd_slam_core/src/util/settings.cpp
@@ -74,8 +74,8 @@ float freeDebugParam3 = 1;
 float freeDebugParam4 = 1;
 float freeDebugParam5 = 1;
 
-float KFDistWeight = 4;
-float KFUsageWeight = 3;
+float KFUsageWeight = 4;
+float KFDistWeight = 3;
 
 float minUseGrad = 5;
 float cameraPixelNoise2 = 4*4;