Visualizer tweaks: handle lost tracking status

Author: oseiskar

python/cli/visualization/visualizer.py

@@ -287,6 +287,10 @@ def __init__(self, args=VisualizerArgs()):
         # Recording
         self.recorder = None
 
+    def __resetAfterLost(self):
+        self.map.reset()
+        self.poseTrail.reset()
+
     def __initDisplay(self):
         from pygame.locals import DOUBLEBUF, OPENGL, FULLSCREEN, GL_MULTISAMPLEBUFFERS, GL_MULTISAMPLESAMPLES
 
@@ -430,29 +434,26 @@ def __processUserInput(self):
                 if self.cameraMode is CameraMode.THIRD_PERSON: self.cameraControls3D.update(event)
                 if self.cameraMode is CameraMode.TOP_VIEW: self.cameraControls2D.update(event)
 
-    def onVioOutput(self, cameraPose, image=None, width=None, height=None, colorFormat=None):
+    def onVioOutput(self, cameraPose, image=None, width=None, height=None, colorFormat=None, status=None):
         if self.shouldQuit: return
+        import spectacularAI
 
-        if image is None:
-            output = {
-                "type": "vio",
-                "cameraPose" : cameraPose,
-                "image" : None,
-                "width" : self.targetResolution[0],
-                "height" : self.targetResolution[1],
-                "colorFormat" : None
-            }
-        else:
+        output = {
+            "type": "vio",
+            "isTracking": status == spectacularAI.TrackingStatus.TRACKING,
+            "cameraPose" : cameraPose,
+            "image" : None,
+            "width" : self.targetResolution[0],
+            "height" : self.targetResolution[1],
+            "colorFormat" : None
+        }
+        if image is not None:
             # Flip the image upside down for OpenGL.
             if not self.args.flip: image = np.ascontiguousarray(np.flipud(image))
-            output = {
-                "type" : "vio",
-                "cameraPose" : cameraPose,
-                "image" : image,
-                "width" : width,
-                "height" : height,
-                "colorFormat" : colorFormat
-            }
+            output['width'] = width
+            output['height'] = height
+            output['colorFormat'] = colorFormat
+
 
         if self.outputQueueMutex:
             self.outputQueue.append(output)
@@ -475,24 +476,37 @@ def onMappingOutput(self, mapperOutput):
 
     def run(self):
         vioOutput = None
+        wasTracking = False
 
         while not self.shouldQuit:
             self.__processUserInput()
 
             # Process VIO & Mapping API outputs
-            while self.outputQueueMutex and len(self.outputQueue) > 0:
+            while True:
                 if self.shouldPause: break
 
-                output = self.outputQueue.pop(0)
+                if self.outputQueueMutex and len(self.outputQueue) > 0:
+                    output = self.outputQueue.pop(0)
+                else:
+                    break
+
                 if output["type"] == "vio":
                     vioOutput = output
-                    cameraPose = vioOutput["cameraPose"]
-                    self.poseTrail.append(cameraPose.getPosition())
+                    if vioOutput['isTracking']:
+                        wasTracking = True
+                        cameraPose = vioOutput["cameraPose"]
+                        self.poseTrail.append(cameraPose.getPosition())
+                    else:
+                        vioOutput = None
+                        if wasTracking:
+                            self.__resetAfterLost()
+                            wasTracking = False
                     # Drop vio outputs if target fps is too low
                     if self.args.targetFps == 0: break
                 elif output["type"] == "slam":
                     mapperOutput = output["mapperOutput"]
-                    self.map.onMappingOutput(mapperOutput)
+                    if wasTracking: # Don't render if not tracking. Messes up this visualization easily
+                        self.map.onMappingOutput(mapperOutput)
                     if mapperOutput.finalMap and not self.args.keepOpenAfterFinalMap:
                         self.shouldQuit = True
                 else:

python/cli/visualization/visualizer_renderers/renderers.py

@@ -77,12 +77,7 @@ def __init__(
             renderSparsePointCloud='auto',
             renderKeyFrames=True,
             renderMesh=False):
-        self.keyFrameCameraToWorldMatrices = {}
-        self.pointCloudRenderers = {}
-        self.sparsePointCloudRenderer = None
         self.renderSparsePointCloud = renderSparsePointCloud
-        self.keyFrameRenderer = KeyFrameRenderer()
-        # self.meshRenderer = MeshRenderer() # TODO: fix
 
         # constant for entire visualization
         self.voxelSize = 0 if voxelSize is None else voxelSize
@@ -99,6 +94,8 @@ def __init__(
         self.maxRenderKeyFrames = keyFrameCount
         self.colorMode = colorMode
 
+        self.reset()
+
     def onMappingOutput(self, mapperOutput):
         for kfId in mapperOutput.updatedKeyFrames:
             keyFrame = mapperOutput.map.keyFrames.get(kfId)
@@ -177,6 +174,13 @@ def setColorMode(self, mode):
     def setMaxRenderKeyFrames(self, n):
         self.maxRenderKeyFrames = n
 
+    def reset(self):
+        self.keyFrameCameraToWorldMatrices = {}
+        self.pointCloudRenderers = {}
+        self.sparsePointCloudRenderer = None
+        self.keyFrameRenderer = KeyFrameRenderer()
+        # self.meshRenderer = MeshRenderer() # TODO: fix
+
     def render(self, cameraPositionWorld, viewMatrix, projectionMatrix):
         n = len(self.pointCloudRenderers) if self.maxRenderKeyFrames is None else self.maxRenderKeyFrames
 
@@ -203,10 +207,13 @@ def renderPointCloud():
 
 class PoseTrailRenderer:
     def __init__(self, maxLength=None, color=np.array(DEFAULT_POSE_TRAIL_RGBA), lineWidth=2.0):
-        self.poseTrail = []
         self.color = color
         self.maxLength = maxLength
         self.lineWidth = lineWidth
+        self.reset()
+
+    def reset(self):
+        self.poseTrail = []
 
     def append(self, position):
         position = np.array([position.x, position.y, position.z])