| 504 | print("Warning: Dropping mapper output in visualizer (processing too slow!)") |
| 505 | |
| 506 | def run(self): |
| 507 | vioOutput = None |
| 508 | prevVioOutput = None |
| 509 | wasTracking = False |
| 510 | |
| 511 | def getNextOutput(): |
| 512 | vioOutputTime = None if len(self.vioOutputQueue) == 0 else self.vioOutputQueue[0]["time"] |
| 513 | mapperOutputTime = None if len(self.mapperOutputQueue) == 0 else self.mapperOutputQueue[0]["time"] |
| 514 | if vioOutputTime is None: |
| 515 | if mapperOutputTime is None: return None |
| 516 | return self.mapperOutputQueue.pop(0) |
| 517 | if mapperOutputTime is None: |
| 518 | return self.vioOutputQueue.pop(0) |
| 519 | return self.vioOutputQueue.pop(0) if vioOutputTime < mapperOutputTime else self.mapperOutputQueue.pop(0) |
| 520 | |
| 521 | def processVioOutput(output): |
| 522 | nonlocal vioOutput, prevVioOutput, wasTracking |
| 523 | vioOutput = output |
| 524 | if vioOutput['isTracking']: |
| 525 | wasTracking = True |
| 526 | cameraPose = vioOutput["cameraPose"] |
| 527 | self.poseTrail.append(cameraPose.getPosition()) |
| 528 | else: |
| 529 | vioOutput = None |
| 530 | if wasTracking: |
| 531 | self.__resetAfterLost() |
| 532 | wasTracking = False |
| 533 | |
| 534 | def processMapperOutput(output): |
| 535 | nonlocal vioOutput, prevVioOutput, wasTracking |
| 536 | |
| 537 | mapperOutput = output["mapperOutput"] |
| 538 | if wasTracking: # Don't render if not tracking. Messes up this visualization easily |
| 539 | self.map.onMappingOutput(mapperOutput) |
| 540 | if mapperOutput.finalMap: |
| 541 | if self.args.keepOpenAfterFinalMap: |
| 542 | self.showCameraFrustum = False |
| 543 | self.showCameraModel = False |
| 544 | if self.args.targetFps == 0: self.args.targetFps = 30 # No vio outputs -> set 30fps mode instead |
| 545 | if self.cameraSmooth: self.cameraSmooth.reset() # Stop camera moving automatically |
| 546 | vioOutput = prevVioOutput |
| 547 | else: |
| 548 | self.shouldQuit = True |
| 549 | |
| 550 | while not self.shouldQuit: |
| 551 | self.__processUserInput() |
| 552 | |
| 553 | # Process VIO & Mapping API outputs |
| 554 | while True: |
| 555 | if self.shouldPause: break |
| 556 | |
| 557 | output = getNextOutput() |
| 558 | if output is None: break |
| 559 | |
| 560 | if output["type"] == "vio": |
| 561 | processVioOutput(output) |
| 562 | |
| 563 | # Render on all outputs if using target fps 0 (i.e. render on vio output mode) |