Skip to content

utils

get_camera_extrinsic_params_filename(robot_id, name, codec, width, height) #

Get parameters filename based on current package path and camera parameters

Source code in cogip/tools/camera/utils.py 
116
117
118
119
120
121
122
123
124
125
126
def get_camera_extrinsic_params_filename(
    robot_id: int,
    name: CameraName,
    codec: VideoCodec,
    width: int,
    height: int,
) -> Path:
    """Get parameters filename based on current package path and camera parameters"""
    params_filename = Path(__file__).parent
    params_filename /= f"cameras/{robot_id}/{name.name}_{codec.name}_{width}x{height}/extrinsic_params.json"
    return params_filename

get_camera_intrinsic_params_filename(robot_id, name, codec, width, height) #

Get parameters filename based on current package path and camera parameters

Source code in cogip/tools/camera/utils.py 
86
87
88
89
90
91
92
93
94
95
96
def get_camera_intrinsic_params_filename(
    robot_id: int,
    name: CameraName,
    codec: VideoCodec,
    width: int,
    height: int,
) -> Path:
    """Get parameters filename based on current package path and camera parameters"""
    params_filename = Path(__file__).parent
    params_filename /= f"cameras/{robot_id}/{name.name}_{codec.name}_{width}x{height}/intrinsic_params.yaml"
    return params_filename

is_rotation_matrix(R) #

Checks if a matrix is a valid rotation matrix

Source: https://www.learnopencv.com/rotation-matrix-to-euler-angles/

Source code in cogip/tools/camera/utils.py 
21
22
23
24
25
26
27
28
29
30
31
def is_rotation_matrix(R):
    """
    Checks if a matrix is a valid rotation matrix

    Source: https://www.learnopencv.com/rotation-matrix-to-euler-angles/
    """
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    ident = np.identity(3, dtype=R.dtype)
    n = np.linalg.norm(ident - shouldBeIdentity)
    return n < 1e-6

load_camera_extrinsic_params(path) #

Loads camera position relative to robot center.

Source code in cogip/tools/camera/utils.py 
134
135
136
def load_camera_extrinsic_params(path: Path) -> CameraExtrinsicParameters:
    """Loads camera position relative to robot center."""
    return CameraExtrinsicParameters.model_validate_json(path.read_text())

load_camera_intrinsic_params(path) #

Loads camera matrix and distortion coefficients.

Source code in cogip/tools/camera/utils.py 
107
108
109
110
111
112
113
def load_camera_intrinsic_params(path: Path) -> tuple[cv2.typing.MatLike, cv2.typing.MatLike]:
    """Loads camera matrix and distortion coefficients."""
    cv_file = cv2.FileStorage(str(path), cv2.FILE_STORAGE_READ)
    camera_matrix = cv_file.getNode("K").mat()
    dist_coefs = cv_file.getNode("D").mat()
    cv_file.release()
    return [camera_matrix, dist_coefs]

rotate_2d(vector, angle) #

Rotate a 2D point with specify angle

Source code in cogip/tools/camera/utils.py 
60
61
62
63
64
65
66
def rotate_2d(vector: cv2.typing.MatLike, angle: float) -> cv2.typing.MatLike:
    """Rotate a 2D point with specify angle"""
    rotation_matrix = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]])
    vector = vector.reshape(1, 2)
    vector = vector.T
    rotated: cv2.typing.MatLike = (rotation_matrix @ vector).T
    return rotated.squeeze()

rotation_matrix_to_euler_angles(R) #

Calculates rotation matrix to euler angles. The result is the same as MATLAB except the order of the euler angles (x and z are swapped).

Source: https://www.learnopencv.com/rotation-matrix-to-euler-angles/

Source code in cogip/tools/camera/utils.py 
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
def rotation_matrix_to_euler_angles(R):
    """
    Calculates rotation matrix to euler angles.
    The result is the same as MATLAB except the order
    of the euler angles (x and z are swapped).

    Source: https://www.learnopencv.com/rotation-matrix-to-euler-angles/
    """
    assert is_rotation_matrix(R)

    sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    singular = sy < 1e-6

    if not singular:
        x = np.arctan2(R[2, 1], R[2, 2])
        y = np.arctan2(-R[2, 0], sy)
        z = np.arctan2(R[1, 0], R[0, 0])
    else:
        x = np.arctan2(-R[1, 2], R[1, 1])
        y = np.arctan2(-R[2, 0], sy)
        z = 0

    return np.array([x, y, z])

save_camera_extrinsic_params(params, path) #

Save the camera position relative to robot center to given path/file.

Source code in cogip/tools/camera/utils.py 
129
130
131
def save_camera_extrinsic_params(params: CameraExtrinsicParameters, path: Path):
    """Save the camera position relative to robot center to given path/file."""
    path.write_text(CameraExtrinsicParameters.model_dump_json(indent=2))

save_camera_intrinsic_params(camera_matrix, dist_coefs, path) #

Save the camera matrix and the distortion coefficients to given path/file.

Source code in cogip/tools/camera/utils.py 
 99
100
101
102
103
104
def save_camera_intrinsic_params(camera_matrix: cv2.typing.MatLike, dist_coefs: cv2.typing.MatLike, path: Path):
    """Save the camera matrix and the distortion coefficients to given path/file."""
    cv_file = cv2.FileStorage(str(path), cv2.FILE_STORAGE_WRITE)
    cv_file.write("K", camera_matrix)
    cv_file.write("D", dist_coefs)
    cv_file.release()

wrap_to_pi(angle) #

Wrap angle to PI, return a angle value between -PI and PI

Parameters:

Name Type Description Default
angle float

Angle in radians

 required

Returns:

Type Description
float

Wrapped angle, in radians
Source code in cogip/tools/camera/utils.py 
69
70
71
72
73
74
75
76
77
78
def wrap_to_pi(angle: float) -> float:
    """Wrap angle to PI, return a angle value between `-PI` and `PI`

    Arguments:
        angle: Angle in radians

    Returns:
        Wrapped angle, in radians
    """
    return np.arctan2(np.sin(angle), np.cos(angle))