def from_xml(element):
"""
Build a StereoRig object out of the given xml node
@type element: lxml.etree.SubElement
@param element: the element to construct an StereoRig object from
@return a new StereoRig object constructed from XML node with matrices in
OpenCV format
"""
use_old_format = element.find("CameraExtrinsics") is not None
if use_old_format:
extrinsics0 = Camera.Extrinsics()
extrinsics1 = Camera.Extrinsics.from_xml(
element.find("CameraExtrinsics"))
cameras_elem = element.find(Rig.CAMERAS_ELEMENT_TAG)
cameras = (Camera.from_xml(cameras_elem[0]),
Camera.from_xml(cameras_elem[1]))
cameras[0].extrinsics = extrinsics0
cameras[1].extrinsics = extrinsics1
extrinsics = None
else:
cameras_element = element.find(Rig.CAMERAS_ELEMENT_TAG)
cameras = tuple([
Camera.from_xml(camera_element)
for camera_element in cameras_element
])
extrinsics_element = element.find("Extrinsics")
if extrinsics_element is not None:
extrinsics = Camera.Extrinsics.from_xml(extrinsics_element)
else:
extrinsics = None
return Rig(cameras, extrinsics)
def __init__(self, stereo_rigs=None, rig=None):
"""
Construct a multi-stereo rig out of either a collection of stereo rigs or a single rig
@type stereo_rigs: tuple[calib.rig.Rig]
@param stereo_rigs: rigs with two cameras each and extrinsics with reference to the global coordinate frame,
i.e. the first camera of the first rig
@type rig: calib.rig.Rig
@param rig: a regular rig to convert to a multistereo rig. Assumes stereo camera pairs are in order in the input
rig.
"""
if stereo_rigs is not None and rig is None:
ix_rig = 0
for rig in stereo_rigs:
if len(rig.cameras) != 2:
raise ValueError(
"Each of the stereo rigs should only have two cameras")
if rig.extrinsics is None and ix_rig != 0:
raise ValueError(
"Expecting valid extrinsics for rig at position {:d} relative to the first rig."
.format(ix_rig))
ix_rig += 1
self.rigs = stereo_rigs
elif rig is not None and stereo_rigs is None:
# convert an arbitrary rig to a multistereo rig.
if len(rig.cameras) % 2 != 0 and len(rig.cameras) != 0:
raise ValueError(
"Expecting a non-zero even number of cameras in the input rig."
)
rigs = [
Rig(cameras=(rig.cameras[0], rig.cameras[1],
Camera.Extrinsics()))
]
# find local extrinsics
for ix_cam in range(2, len(rig.cameras), 2):
left_cam = rig.cameras[ix_cam]
right_cam = rig.cameras[ix_cam + 1]
left_extrinsics = left_cam.extrinsics
right_extrinsics = right_cam.extrinsics
left_pose = Pose(
rotation=left_extrinsics.rotation,
translation_vector=left_extrinsics.translation)
right_pose = Pose(
rotation=right_extrinsics.rotation,
translation_vector=right_extrinsics.translation)
local_pose = Pose(
transform=np.linalg.inv(left_pose.T).dot(right_pose.T))
local_right_extrinsics = Camera.Extrinsics(
rotation=local_pose.rmat, translation=local_pose.rvec)
stereo_rig_extrinsics = left_cam.extrinsics
left_cam.extrinsics = Camera.Extrinsics()
right_cam.extrinsics = local_right_extrinsics
rigs.append(
Rig(cameras=(left_cam, right_cam),
extrinsics=stereo_rig_extrinsics))
self.rigs = tuple(rigs)
else:
raise ValueError(
"Either the ['stereo_rigs' and 'stereo_rig_extrinsics'] OR the 'rig' argument should be "
"provided and not None.")
