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.")