def test_pose_inv() -> None: p = Pose(Position(1, 1, 1), Orientation(0.707, 0, 0, 0.707)) pi = p.inversed() assert pi == Pose(Position(-1, -1, 1), Orientation(-0.707, 0, 0, 0.707)) assert p == pi.inversed()
def get_calibration() -> RespT: """Get calibration (camera pose wrt. marker) --- put: description: Returns camera pose with respect to the origin. tags: - Camera parameters: - in: query name: inverse schema: type: boolean description: When set, the method returns pose of the origin wrt. the camera. - in: query name: fx schema: type: number format: float required: true description: unique ID - in: query name: fy schema: type: number format: float required: true description: unique ID - in: query name: cx schema: type: number format: float required: true description: unique ID - in: query name: cy schema: type: number format: float required: true description: unique ID - in: query name: distCoefs schema: type: array items: type: number format: float required: true description: unique ID requestBody: content: multipart/form-data: schema: type: object required: - image properties: # 'image' will be the field name in this multipart request image: type: string format: binary responses: 200: description: Ok content: application/json: schema: $ref: EstimatedPose 404: description: No marker found. content: application/json: schema: type: string """ file = request.files["image"] camera_matrix = camera_matrix_from_request() image = Image.open(file.stream) dist_matrix = dist_matrix_from_request() if _mock: time.sleep(0.5) quality = random.uniform(0, 1) pose = Pose( Position(random.uniform(-0.5, 0.5), random.uniform(-0.5, 0.5), random.uniform(0.2, 1))) else: poses = estimate_camera_pose(camera_matrix, dist_matrix, image, MARKER_SIZE) if not poses: return jsonify("No marker detected."), 404 quality_dict: dict[int, float] = {k: 0.0 for k, v in MARKERS.items()} known_markers: list[tuple[Pose, float]] = [] # apply configured marker offset from origin to the detected poses for marker_id in poses.keys(): try: cpose = MARKERS[marker_id] except KeyError: logger.debug(f"Detected un-configured marker id {marker_id}.") continue mpose = poses[marker_id] dist = math.sqrt(mpose.position.x**2 + mpose.position.y**2 + mpose.position.z**2) # the closer the theta is to pi, the higher quality we get theta = quaternion.as_spherical_coords( mpose.orientation.as_quaternion())[0] ori_marker_quality = normalize(abs(theta), MIN_THETA, MAX_THETA) # the closer the marker is, the higher quality we get dist_marker_quality = 1.0 - normalize(dist, MIN_DIST, MAX_DIST) marker_quality = (ori_marker_quality + dist_marker_quality) / 2 quality_dict[marker_id] = marker_quality known_markers.append((tr.make_pose_abs(cpose, mpose), marker_quality)) logger.debug(f"Known marker : {marker_id}") logger.debug(f"...original pose : {poses[marker_id]}") logger.debug(f"...transformed pose: {poses[marker_id]}") logger.debug(f"...dist quality : {dist_marker_quality:.3f}") logger.debug(f"...ori quality : {ori_marker_quality:.3f}") logger.debug(f"...overall quality : {marker_quality:.3f}") if not known_markers: return jsonify("No known marker detected."), 404 weights = [marker[1] for marker in known_markers] wsum = sum(weights) if wsum <= 0: logger.warning(f"Got invalid weights, probably bad input data.\n" f"Camera matrix: {camera_matrix}\n" f"Dist matrix: {dist_matrix}") return jsonify("Invalid input data."), 500 # combine all detections pose = Pose() for mpose, weight in known_markers: pose.position += mpose.position * weight pose.position *= 1.0 / wsum quaternions = np.array([ quaternion.as_float_array(km[0].orientation.as_quaternion()) for km in known_markers ]) pose.orientation.set_from_quaternion( quaternion.from_float_array( weighted_average_quaternions(quaternions, np.array(weights)))) quality = np.mean(list(quality_dict.values())) inverse = request.args.get("inverse", default="false") == "true" if inverse: logger.debug("Inverting the output pose.") pose = pose.inversed() return jsonify(EstimatedPose(pose, quality)), 200
def get_calibration() -> RespT: """Get calibration (camera pose wrt. marker) --- put: description: Returns camera pose with respect to the origin. tags: - Camera parameters: - in: query name: inverse schema: type: boolean description: When set, the method returns pose of the origin wrt. the camera. - in: query name: fx schema: type: number format: float required: true description: unique ID - in: query name: fy schema: type: number format: float required: true description: unique ID - in: query name: cx schema: type: number format: float required: true description: unique ID - in: query name: cy schema: type: number format: float required: true description: unique ID - in: query name: distCoefs schema: type: array items: type: number format: float required: true description: unique ID requestBody: content: multipart/form-data: schema: type: object required: - image properties: # 'image' will be the field name in this multipart request image: type: string format: binary responses: 200: description: Ok content: application/json: schema: $ref: Pose """ file = request.files["image"] camera_matrix = camera_matrix_from_request() image = Image.open(file.stream) dist_matrix = dist_matrix_from_request() if _mock: time.sleep(0.5) pose = Pose( Position(random.uniform(-0.5, 0.5), random.uniform(-0.5, 0.5), random.uniform(0.2, 1))) else: poses = estimate_camera_pose(camera_matrix, dist_matrix, image, MARKER_SIZE) if not poses: return jsonify("No marker detected."), 404 known_markers: List[Tuple[Pose, float]] = [] # apply configured marker offset from origin to the detected poses for marker_id in poses.keys(): try: cpose = MARKERS[marker_id] except KeyError: logger.debug(f"Detected un-configured marker id {marker_id}.") continue mpose = poses[marker_id] weight = 1.0 / math.sqrt(mpose.position.x**2 + mpose.position.y**2 + mpose.position.z**2) known_markers.append((tr.make_pose_abs(cpose, mpose), weight)) logger.debug(f"Known marker : {marker_id}") logger.debug(f"...original pose : {poses[marker_id]}") logger.debug(f"...transformed pose: {poses[marker_id]}") logger.debug(f"...weight : {weight}") if not known_markers: return jsonify("No known marker detected."), 404 weights = [marker[1] for marker in known_markers] # combine all detections # TODO this is just initial (naive) solution with weight equal to distance to the origin pose = Pose() for mpose, weight in known_markers: pose.position += mpose.position * weight pose.position *= 1.0 / sum(weights) quaternions = np.array([ quaternion.as_float_array(km[0].orientation.as_quaternion()) for km in known_markers ]) pose.orientation.set_from_quaternion( quaternion.from_float_array( weighted_average_quaternions(quaternions, np.array(weights)))) inverse = request.args.get("inverse", default="false") == "true" if inverse: logger.debug("Inverting the output pose.") pose = pose.inversed() return jsonify(pose), 200