|
| 1 | +#!/usr/bin/env python3 |
| 2 | + |
| 3 | +import numpy as np |
| 4 | + |
| 5 | + |
| 6 | +def set_default_params(params): |
| 7 | + """ Just set the default parameter values in case there aren't any |
| 8 | + @params params - dictionary with values |
| 9 | + """ |
| 10 | + if not "z_near" in params: |
| 11 | + params["z_near"] = 1.0 |
| 12 | + if not "z_far" in params: |
| 13 | + params["z_far"] = 100.0 |
| 14 | + if not "camera_width_angle" in params: |
| 15 | + params["camera_width_angle"] = 90.0 |
| 16 | + |
| 17 | + |
| 18 | +def from_image_to_box(params, pt_uv): |
| 19 | + """ Convert a point in width/height to -1, 1 x -1, 1 |
| 20 | + @params - has image size |
| 21 | + @params - pt_uv - list/point with u, v |
| 22 | + @return - pt_xy_in_box """ |
| 23 | + im_size = params["image_size"] |
| 24 | + pt_xy = [2.0 * (pt_uv[i] - im_size[i] / 2.0) / im_size[i] for i in range(0, 2)] |
| 25 | + |
| 26 | + # Images are indexed from upper left corner, so y needs to be inverted |
| 27 | + pt_xy[1] *= -1 |
| 28 | + return pt_xy |
| 29 | + |
| 30 | +def frame_at_z_near(params): |
| 31 | + """ return left, right, bottom, top for the frame at the near plane |
| 32 | + params has information about the camera |
| 33 | + @param params - z_near, z_far, image_size as 2x1 array, camera_width_angle in degrees |
| 34 | + @return 4 numbers in a list""" |
| 35 | + |
| 36 | + set_default_params(params) |
| 37 | + ang_width_half = 0.5 * np.pi * params["camera_width_angle"] / 180.0 |
| 38 | + |
| 39 | + width_window = params["image_size"][0] |
| 40 | + height_window = params["image_size"][1] |
| 41 | + aspect_ratio = width_window / height_window |
| 42 | + |
| 43 | + frame_width = params["z_near"] * np.tan(ang_width_half) |
| 44 | + frame_height = frame_width / aspect_ratio |
| 45 | + |
| 46 | + return [-frame_width, frame_width, -frame_height, frame_height] |
| 47 | + |
| 48 | + |
| 49 | +def frustrum_matrix(params): |
| 50 | + """ params has information about the camera |
| 51 | + @param params - z_near, z_far, image_size as 2x1 array, camera_width_angle in degrees |
| 52 | + @return 4x4 projection matrix""" |
| 53 | + mat = np.identity(4) |
| 54 | + frame = frame_at_z_near(params) |
| 55 | + |
| 56 | + left = frame[0] |
| 57 | + right = frame[1] |
| 58 | + bottom = frame[2] |
| 59 | + top = frame[3] |
| 60 | + |
| 61 | + print(f"Frame {frame}") |
| 62 | + print(f"params {params}") |
| 63 | + mat[0, 0] = 2.0 * params["z_near"] / (right - left) |
| 64 | + mat[1, 1] = 2.0 * params["z_near"] / (top - bottom) |
| 65 | + # Shifts due to center of projection not being 0, 0 |
| 66 | + mat[0, 2] = (right + left) / (right - left) |
| 67 | + mat[1, 2] = (top + bottom) / (top - bottom) |
| 68 | + # Also known as k - the scaling factor |
| 69 | + mat[2, 2] = (params["z_far"] + params["z_near"]) / (params["z_far"] - params["z_near"]) |
| 70 | + mat[2, 3] = -(2.0 * params["z_far"] * params["z_near"]) / (params["z_far"] - params["z_near"]) |
| 71 | + mat[3, 3] = 0.0 |
| 72 | + mat[3, 2] = -1.0 |
| 73 | + |
| 74 | + return mat |
| 75 | + |
| 76 | + |
| 77 | +if __name__ == '__main__': |
| 78 | + params = {"image_size":[640, 480]} |
| 79 | + # Check image to box |
| 80 | + pt_ul_im = [640, 0] |
| 81 | + pt_ul_xy = from_image_to_box(params, pt_ul_im) |
| 82 | + assert(np.isclose(pt_ul_xy[0], 1.0)) |
| 83 | + assert(np.isclose(pt_ul_xy[1], 1.0)) |
| 84 | + |
| 85 | + pt_lr_im = [0, 480] |
| 86 | + pt_lr_xy = from_image_to_box(params, pt_lr_im) |
| 87 | + assert(np.isclose(pt_lr_xy[0], -1.0)) |
| 88 | + assert(np.isclose(pt_lr_xy[1], -1.0)) |
0 commit comments