(self)
| 56 | return pointCloudJson |
| 57 | |
| 58 | def start(self): |
| 59 | shouldQuit = False |
| 60 | jsonPath = os.path.join(self.directory, "json") |
| 61 | with open(jsonPath, 'rb') as file: |
| 62 | while not shouldQuit: |
| 63 | messageHeader = readBytes(file, HEADER_SIZE) |
| 64 | if messageHeader is False: break |
| 65 | |
| 66 | magicBytes, messageId, jsonSize, binarySize = struct.unpack('@4I', messageHeader) |
| 67 | if magicBytes != MAGIC_BYTES: |
| 68 | raise Exception(f"Wrong magic bytes! Expected {MAGIC_BYTES} and received {magicBytes}") |
| 69 | jsonOutput = json.loads(readBytes(file, jsonSize).decode('ascii')) |
| 70 | |
| 71 | if 'cameraPoses' in jsonOutput: # Vio output |
| 72 | assert(binarySize == 0) |
| 73 | if self.onVioOutput: |
| 74 | vioOutput = MockVioOutput(jsonOutput) |
| 75 | self.onVioOutput(vioOutput) |
| 76 | else: # Mapper output |
| 77 | shouldQuit = jsonOutput["finalMap"] |
| 78 | if self.onMappingOutput: |
| 79 | for keyFrameId in jsonOutput["updatedKeyFrames"]: |
| 80 | keyFrame = jsonOutput["map"]["keyFrames"].get(str(keyFrameId)) |
| 81 | if not keyFrame: # Deleted key frame |
| 82 | self.pointClouds.pop(keyFrameId, None) |
| 83 | continue |
| 84 | if "pointCloud" in keyFrame: |
| 85 | pointCloud = keyFrame["pointCloud"] |
| 86 | keyFrame["pointCloud"] = self.__deserializePointCloud(keyFrameId, pointCloud) |
| 87 | mappingOutput = MockMapperOutput(jsonOutput) |
| 88 | self.onMappingOutput(mappingOutput) |
| 89 | |
| 90 | def invert_se3(a): |
| 91 | b = np.eye(4) |
no test coverage detected