Camera Transform

class pyrobot.algorithms.camera_transform.CameraTransform(configs, world, ros_launch_manager=None, robots={}, sensors={}, algorithms={})

Bases: pyrobot.algorithms.algorithm.Algorithm

Base class of Camera Transform algorithms.

Specifically, this algorithm handles getting transformation from image to 3D points. It also give access to camera intrinsics and extrinsics.

__init__(configs, world, ros_launch_manager=None, robots={}, sensors={}, algorithms={})

Initialize self. See help(type(self)) for accurate signature.

check_cfg()

This function checks required configs shared by all camera transform instances in constructor.

Specifically, it checks for:

1. The camera transform algorithm handles one camera, and one camera only

2. The camera is either a component of a robot or a sensor

3. The camera has a base frame and an camera frame

4. The camera has intrinsic params: fx, fy, cx, cy

5. The camera has image size specified

For any algorithm specific config checks, please extend the check_cfg function

with customized algorithm config checks after calling this function using super().check_cfg()

get_intrinsics()

Returns:

intrinsics_matrix: a 3x3 matrix structured as: K = [

[fx, 0, cx], [0, fy, cy], [0, 0, 1 ],

] where f is focal length and c is center pixel.

get_transform_matrix(source_frame, target_frame)

This function takes in a source frame and a target frame,

get a transform from source frame to target frame.

Args:

source_frame: string of source frame name target_frame: string of target frame name

Returns:

transformation_matrix: a 4x4 matrix structured as: M = [

[Rxx, Rxy, Rxz, tx], [Ryx, Ryy, Ryz, ty], [Rzx, Rzy, Rzz, tz], [0, 0, 0, 1 ]

] such that for any point p = [x, y, z] in source frame, construct p’ = [x, y, z, 1], Mp’ gives you rotated and translated point in target frame; construct p’’ = [x, y, z, 0], Mp’’ gives you rotated point (no translation) in target frame.

pcd_from_img(depth_img, rgb_img=None, in_cam=True)

This function takes in depth and rgb image,

and map to 3d point in camera frame or in specified base frame.

Args:

depth_img: array of depth image with shape (Camera.height, Camera.width) rgb_img: array of rgb image with shape (Camera.height, Camera.width, 3). Default: None.

If None, the function would only return 3D points but not color.

in_cam: bool, specify if the returned 3d point cloud are in camera frame. Default: True.

If True, the function would return 3D point cloud w.r.t camera frame; else it would return points w.r.t base frame specified in camera configs.

Returns:

pts: array of queried pixel positions in 3D with shape (n, 3),

where n is the number of valid points in depth image. Each of the n entries is an [x, y, z] vector

colors: array of queried pixel colors in 3D with shape (n, 3).

where n is the number of valid points in depth image. Each of the n entries is an [r, g, b] vector If rgb_img is None, colors would be None.

pix_to_pt(rows, columns, depth_img, rgb_img=None, in_cam=True)

This function takes in row and column indices of depth and rgb image,

and map to 3d point in camera frame or in specified base frame.

Args:

rows: list-like vector of indices of the rows of queried pixels, length is number of queried pixels. columns: list-like vector of indices of the columns of queried pixels, length should be the same as rows. depth_img: array of depth image with shape (Camera.height, Camera.width) rgb_img: array of rgb image with shape (Camera.height, Camera.width, 3). Default: None.

If None, the function would only return 3D points but not color.

in_cam: bool, specify if the returned 3d points are in camera frame. Default: True.

If True, the function would return 3D points w.r.t camera frame; else it would return points w.r.t base frame specified in camera configs.

Returns:

pts: array of queried pixel positions in 3D with shape (n, 3).

Each of the n entries is an [x, y, z] vector

colors: array of queried pixel colors in 3D with shape (n, 3).

Each of the n entries is an [r, g, b] vector If rgb_img is None, colors would be None.