def init(self, xform):
        """Initialize with the given xform, calculating python-only variables"""
        self.xform = xform

        corners = (Vec3(-0.5, -0.5),
                   Vec3(-0.5,  0.5),
                   Vec3( 0.5,  0.5),
                   Vec3( 0.5, -0.5))
        self.world_vertices = [xform.transform(c) for c in corners]
        self.world_normal = xform.transform_vector(-Vec3.z_axis()).normalize()
        self.world_center = xform.transform(Vec3.origin())

        # Point upwards by rotating up vector by -90, -180, or -270 degrees.
        # Direction is the rotation angle (in units of 90-degrees).
        #     0=up, 1=left, 2=down, 3=right
        # In image space, up is negative.
        up = xform.transform_vector(-Vec3.y_axis()).normalized()
        direc = 0
        if abs(up.x) > abs(up.y):
            rot = Mat4.new_rotate_axis(self.world_normal, math.pi*0.5)
            up = rot.transform(up)
            direc = 1
        if  up.y < 0:
            rot = Mat4.new_rotate_axis(self.world_normal, math.pi)
            up = rot.transform(up)
            direc += 2
        self.world_up = up
        self.world_direction = direc
    def init(self, xform):
        """Initialize with the given xform, calculating python-only variables"""
        self.xform = xform

        corners = (Vec3(-0.5, -0.5), Vec3(-0.5,
                                          0.5), Vec3(0.5,
                                                     0.5), Vec3(0.5, -0.5))
        self.world_vertices = [xform.transform(c) for c in corners]
        self.world_normal = xform.transform_vector(-Vec3.z_axis()).normalize()
        self.world_center = xform.transform(Vec3.origin())

        # Point upwards by rotating up vector by -90, -180, or -270 degrees.
        # Direction is the rotation angle (in units of 90-degrees).
        #     0=up, 1=left, 2=down, 3=right
        # In image space, up is negative.
        up = xform.transform_vector(-Vec3.y_axis()).normalized()
        direc = 0
        if abs(up.x) > abs(up.y):
            rot = Mat4.new_rotate_axis(self.world_normal, math.pi * 0.5)
            up = rot.transform(up)
            direc = 1
        if up.y < 0:
            rot = Mat4.new_rotate_axis(self.world_normal, math.pi)
            up = rot.transform(up)
            direc += 2
        self.world_up = up
        self.world_direction = direc
def _debug_image(draw, coords, xform, plane_xform, cam_xform):
    """Draw debugging info on the calibration image"""
    minv, maxv = coords
    ixform = xform.inverse()
    iplane_xform = plane_xform.inverse()
    icam_xform = cam_xform.inverse()
    vertices = (Vec3(maxv.x, minv.y, minv.z),
                Vec3(maxv.x, maxv.y, minv.z), 
                Vec3(minv.x, maxv.y, minv.z), 
                Vec3(minv.x, minv.y, minv.z))
    _draw_vertices(draw, vertices, (255, 0, 0, 255), width=7)

    vertices = [icam_xform.transform(vertex) for vertex in vertices]
    vertices = [ixform.transform(vertex) for vertex in vertices]
    vertices1 = (cam_xform.transform(vertex) for vertex in vertices)
    _draw_vertices(draw, vertices1, (0, 255, 0, 200), width=5)

    vertices = [iplane_xform.transform(vertex) for vertex in vertices]
    vertices1 = (cam_xform.transform(vertex) for vertex in vertices)
    _draw_vertices(draw, vertices1, (0, 0, 255, 200), width=3)
def _find_display_coords(marker, marker_vertices, display):
    """Calculate the display corners from the marker corners. For now,
    assume everything is orthogonal."""

    # Get bounding box
    minv = Vec3.min(*marker_vertices)
    maxv = Vec3.max(*marker_vertices)
    size = maxv - minv

    # Stretch to account for white border
    minv -= size / 8.0
    maxv += size / 8.0
    size = maxv - minv

    # Stretch to display aspect ratio
    direction = marker.world_direction
    dar = display.aspectRatio
    if direction == 1 or direction == 3 :
        dar = 1.0 / dar

    if dar > 1:
        # The display is bigger horizontally than vertically.
        width = size.y * dar
        stretch = (width - size.x) / 2.0
        # The image is centered, so stretch in both directions.
        minv.x -= stretch
        maxv.x += stretch

    else:
        # The display is bigger vertically than horizontally.
        height = size.x / dar
        stretch = (height - size.y) / 2.0
        # The image is centered, so stretch in both directions.
        minv.y -= stretch
        maxv.y += stretch

    # Done
    return [minv, maxv]
def _calibrate(jumbotron, displays, image, debug=False, debug_image=False):
    """Calibrate a jumbotron with a calibration images."""
    if debug_image:
        draw = ImageDraw.Draw(image, "RGBA")

    # Initialize display viewports
    for display in displays.values():
        display.viewport = None

    # Find markers
    found_markers = artoolkit.detect(image, confidence_threshold=0.5,
                                     debug=debug, debug_image=debug_image)

    # Throw out unknown markers
    markers = [marker for marker in found_markers if marker.id in displays]
    if not markers:
        return 0
    if len(markers) < len(found_markers):
        # TODO: get this info to node.js
        #logging.warn("Found unknown markers in jumbotron %s", jumbotron.name)
        pass

    # The method used here has problems when combining very small
    # displays with very large displays. The small displays will seem
    # to be very far away, so any vertical or horizontal separation
    # between them and the larger display will be amplified. An
    # alternative might be to use the screen coordinates of each
    # marker. Needs more thought if this becomes an issue.

    # Get camera xform
    cam_xform = _get_camera_xform(image)

    # Align markers as best we can 
    _align_markers(markers)

    # Find best fitting plane
    plane_xform = _find_best_change_basis_xform(markers)

    # Project each marker to x-y plane and stretch to display size
    coords = []
    for marker in markers:
        idx = marker.id

        # Rotate from the best-fitting plane to the x-y plane.
        rot_vertices = [plane_xform.transform(vertex)
                        for vertex in marker.world_vertices]

        # Then rotate about the marker's center to the x-y plane
        center = plane_xform.transform(marker.world_center)
        normal = plane_xform.transform_vector(marker.world_normal).normalize()
        up     = plane_xform.transform_vector(marker.world_up).normalize()
        xform = _get_change_basis_xform(center, up, normal)
        xy_vertices = [xform.transform(vertex) for vertex in rot_vertices]

        # Use the camera transform to project to the screen
        screen_vertices = [cam_xform.transform(vertex)
                           for vertex in xy_vertices]

        # Finally, stretch to the display corners
        coord = _find_display_coords(marker, screen_vertices, displays[idx])
        coords.append(coord)

        if debug:
            _debug(Display=marker.id,
                   world=marker.world_vertices, plane=rot_vertices,
                   xyplane=xy_vertices, cam=screen_vertices, bbox=coord)
        if debug_image:
            _debug_image(draw, coords[idx], xform, plane_xform, cam_xform)

    # Find the bounding box of all the markers
    allminv = Vec3.min(*(coord[0] for coord in coords))
    allmaxv = Vec3.max(*(coord[1] for coord in coords))
    allsize = allmaxv - allminv
    if debug_image:
        _draw_rectangle(draw, allminv, allmaxv, "white", width=1)
    jumbotron.aspectRatio = allsize.x / allsize.y

    # Normalize viewports and set in displays
    for marker, coord in zip(markers, coords):
        idx = marker.id
        minv = (coord[0] - allminv)
        maxv = (coord[1] - allminv)
        size = maxv - minv

        displays[idx].viewport = dict(x=minv.x / allsize.x,
                                      y=minv.y / allsize.y,
                                      width=size.x / allsize.x,
                                      height=size.y / allsize.y,
                                      rotation=marker.world_direction)

    if debug:
        logging.debug("Final displays:")
        for marker, coord in zip(markers, coords):
            logging.debug("display {0} {1}".format(marker.id, coord))

    if debug_image:
        image.save("calibrate_out.jpg")

    return len(markers)
def _draw_rectangle(draw, minv, maxv, color="white", width=5):
    """Draw a rectangle"""
    # Convert to 2d tuples
    vertices = [minv, Vec3(minv.x, maxv.y), maxv, Vec3(maxv.x, minv.y)]
    _draw_vertices(draw, vertices, color, width)