Python visualizer OpenGL improvements (#15)

* Python visualizer OpenGL improvements

* Remove comment

Author: kaatrasa

python/cli/visualization/visualizer.py

@@ -248,8 +248,6 @@ def __init__(self, args=VisualizerArgs()):
         self.outputQueue = []
         self.outputQueueMutex = Lock()
         self.clock = pygame.time.Clock()
-        self.projectionMatrix = None
-        self.viewMatrix = None
 
         # Window
         self.fullScreen = args.fullScreen
@@ -376,18 +374,20 @@ def __render(self, cameraPose, width, height, image=None, colorFormat=None):
             top = 25.0 * self.cameraControls2D.zoom / self.aspectRatio
             projectionMatrix = getOrthographicProjectionMatrixOpenGL(left, right, bottom, top, -1000.0, 1000.0)
 
-        self.projectionMatrix = projectionMatrix
-        self.viewMatrix = viewMatrix
+        glMatrixMode(GL_PROJECTION)
+        glLoadMatrixf(projectionMatrix.transpose())
+        glMatrixMode(GL_MODELVIEW)
+        glLoadMatrixf(viewMatrix.transpose())
 
         self.map.render(cameraPose.getPosition(), viewMatrix, projectionMatrix)
-        if self.showGrid: self.grid.render(viewMatrix, projectionMatrix)
-        if self.showPoseTrail: self.poseTrail.render(viewMatrix, projectionMatrix)
+        if self.showGrid: self.grid.render()
+        if self.showPoseTrail: self.poseTrail.render()
         if self.args.customRenderCallback: self.args.customRenderCallback()
 
         if self.cameraMode in [CameraMode.THIRD_PERSON, CameraMode.TOP_VIEW]:
             modelMatrix = cameraPose.getCameraToWorldMatrix()
-            if self.showCameraModel: self.cameraModelRenderer.render(modelMatrix, viewMatrix, projectionMatrix)
-            if self.showCameraFrustum: self.cameraFrustumRenderer.render(modelMatrix, viewMatrix, projectionMatrix, self.cameraMode is CameraMode.TOP_VIEW)
+            if self.showCameraModel: self.cameraModelRenderer.render(modelMatrix)
+            if self.showCameraFrustum: self.cameraFrustumRenderer.render(modelMatrix, self.cameraMode is CameraMode.TOP_VIEW)
 
         if self.recorder: self.recorder.recordFrame()
         pygame.display.flip()
@@ -548,12 +548,6 @@ def run(self):
 
         self.__close()
 
-    def getProjectionMatrix(self):
-        return self.projectionMatrix
-
-    def getViewMatrix(self):
-        return self.viewMatrix
-
     def printHelp(self):
         print("Control using the keyboard:")
         print("* Q: Quit")

python/cli/visualization/visualizer_renderers/renderers.py

@@ -33,7 +33,10 @@ def __init__(self, color=np.array(DEFAULT_CAMERA_RGBA), scale=0.05, lineWidth=2)
             [4, 1],
         ]
 
-    def render(self, modelMatrix, viewMatrix, projectionMatrix):
+    def render(self, modelMatrix):
+        glPushMatrix()
+        glMultMatrixf(modelMatrix.transpose())
+
         glLineWidth(self.lineWidth)
         glEnable(GL_BLEND)
         glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
@@ -42,12 +45,6 @@ def render(self, modelMatrix, viewMatrix, projectionMatrix):
         glEnable(GL_DEPTH_TEST)
         glColor4fv(self.color)
 
-        modelView = viewMatrix @ modelMatrix
-        glMatrixMode(GL_MODELVIEW)
-        glLoadMatrixf(modelView.transpose())
-        glMatrixMode(GL_PROJECTION)
-        glLoadMatrixf(projectionMatrix.transpose())
-
         glBegin(GL_LINES)
         for edge in self.edges:
             p0 = self.vertices[edge[0]]
@@ -59,6 +56,7 @@ def render(self, modelMatrix, viewMatrix, projectionMatrix):
         glDisable(GL_BLEND)
         glDisable(GL_LINE_SMOOTH)
         glDisable(GL_DEPTH_TEST)
+        glPopMatrix()
 
 class KeyFrameRenderer:
     def __init__(self, color=np.array(DEFAULT_KEYFRAME_RGBA), scale=0.05, lineWidth=2):
@@ -75,11 +73,9 @@ def render(self, keyFrameCameraToWorldMatrices, viewMatrix, projectionMatrix):
 
         for kfId in keyFrameCameraToWorldMatrices:
             modelMatrix = keyFrameCameraToWorldMatrices[kfId]
-            modelView = viewMatrix @ modelMatrix
-            glMatrixMode(GL_MODELVIEW)
-            glLoadMatrixf(modelView.transpose())
-            glMatrixMode(GL_PROJECTION)
-            glLoadMatrixf(projectionMatrix.transpose())
+
+            glPushMatrix()
+            glMultMatrixf(modelMatrix.transpose())
 
             glBegin(GL_LINES)
             for edge in self.cameraWireframe.edges:
@@ -88,6 +84,7 @@ def render(self, keyFrameCameraToWorldMatrices, viewMatrix, projectionMatrix):
                 glVertex3f(p0[0], p0[1], p0[2])
                 glVertex3f(p1[0], p1[1], p1[2])
             glEnd()
+            glPopMatrix()
 
         glDisable(GL_BLEND)
         glDisable(GL_LINE_SMOOTH)
@@ -255,15 +252,8 @@ def append(self, position):
         if self.maxLength is not None and len(self.poseTrail) > self.maxLength:
             self.poseTrail.pop(0)
 
-    def render(self, viewMatrix, projectionMatrix):
-        modelView = viewMatrix # pose trail is already in world coordinates -> model matrix is identity
-
+    def render(self):
         glLineWidth(self.lineWidth)
-        glMatrixMode(GL_MODELVIEW)
-        glLoadMatrixf(modelView.transpose())
-        glMatrixMode(GL_PROJECTION)
-        glLoadMatrixf(projectionMatrix.transpose())
-
         glEnable(GL_BLEND)
         glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
         glEnable(GL_LINE_SMOOTH)
@@ -289,13 +279,7 @@ def __init__(self, radius=20, length=1.0, color=np.array(DEFAULT_GRID_RGBA), ori
         self.lineWidth = lineWidth
         self.bounds = [-radius * length, radius * length]
 
-    def render(self, viewMatrix, projectionMatrix):
-        modelView = viewMatrix # grid is defined in world coordinates
-        glMatrixMode(GL_MODELVIEW)
-        glLoadMatrixf(modelView.transpose())
-        glMatrixMode(GL_PROJECTION)
-        glLoadMatrixf(projectionMatrix.transpose())
-
+    def render(self):
         glLineWidth(self.lineWidth)
         glEnable(GL_BLEND)
         glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
@@ -389,15 +373,11 @@ def __init__(self, projectionMatrix, color=np.array(DEFAULT_FRUSTUM_RGBA)):
             ]
         ]
 
-    def render(self, modelMatrix, viewMatrix, projectionMatrix, render2d):
-        modelView = viewMatrix @ modelMatrix
+    def render(self, modelMatrix, render2d):
+        glPushMatrix()
+        glMultMatrixf(modelMatrix.transpose())
 
         glLineWidth(1)
-        glMatrixMode(GL_MODELVIEW)
-        glLoadMatrixf(modelView.transpose())
-        glMatrixMode(GL_PROJECTION)
-        glLoadMatrixf(projectionMatrix.transpose())
-
         glEnable(GL_DEPTH_TEST)
         glEnable(GL_BLEND)
         glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
@@ -413,6 +393,7 @@ def render(self, modelMatrix, viewMatrix, projectionMatrix, render2d):
             glEnd()
 
         glDisable(GL_DEPTH_TEST)
+        glPopMatrix()
 
 def createPlaneMesh(scale, position, color):
     # 3 ---- 2