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
