(args)
| 99 | }) |
| 100 | |
| 101 | def smooth(args): |
| 102 | import spectacularAI |
| 103 | import numpy as np |
| 104 | |
| 105 | # Globals |
| 106 | visualizer = None |
| 107 | isTracking = False |
| 108 | finalMapWritten = False |
| 109 | poseTrails = [] |
| 110 | outputToCam = None |
| 111 | |
| 112 | def process_mapping_output(output): |
| 113 | nonlocal visualizer |
| 114 | nonlocal finalMapWritten |
| 115 | |
| 116 | if visualizer is not None: |
| 117 | visualizer.onMappingOutput(output) |
| 118 | |
| 119 | if output.finalMap: |
| 120 | trajectory = compute_full_trajectory(output.map.keyFrames, poseTrails, outputToCam) |
| 121 | |
| 122 | with open(args.output, "w") as outFile: |
| 123 | for output in trajectory: |
| 124 | outFile.write(json.dumps(output) + "\n") |
| 125 | finalMapWritten = True |
| 126 | |
| 127 | elif len(output.updatedKeyFrames) > 0: |
| 128 | frameId = output.updatedKeyFrames[-1] |
| 129 | keyFrame = output.map.keyFrames.get(frameId) |
| 130 | if not keyFrame: return |
| 131 | |
| 132 | frameSet = keyFrame.frameSet |
| 133 | targetFrame = frameSet.rgbFrame |
| 134 | if not targetFrame: targetFrame = frameSet.primaryFrame |
| 135 | if not targetFrame or not targetFrame.image: return |
| 136 | img = targetFrame.image.toArray() |
| 137 | |
| 138 | if args.preview: |
| 139 | import cv2 |
| 140 | bgrImage = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) |
| 141 | cv2.imshow("Frame", bgrImage) |
| 142 | cv2.setWindowTitle("Frame", "Key frame candidate #{}".format(frameId)) |
| 143 | cv2.waitKey(1) |
| 144 | |
| 145 | def on_vio_output(vioOutput): |
| 146 | nonlocal visualizer, isTracking, outputToCam |
| 147 | wasTracking = isTracking |
| 148 | isTracking = vioOutput.status == spectacularAI.TrackingStatus.TRACKING |
| 149 | if wasTracking and not isTracking: |
| 150 | print('warning: Lost tracking!') |
| 151 | |
| 152 | if outputToCam is None: |
| 153 | # TODO: hacky... improve API |
| 154 | primaryCam = vioOutput.getCameraPose(0) |
| 155 | camToWorld = primaryCam.pose.asMatrix() |
| 156 | outToWorld = vioOutput.pose.asMatrix() |
| 157 | outputToCam = np.linalg.inv(camToWorld) @ outToWorld |
| 158 |
no test coverage detected