klampt.model.sensing module

A collection of utility functions for dealing with sensors and sensor data.

More sophisticated operations call for the use of a full-fledged sensor package, such as Open3D or PCL.

Sensor transforms

The get_sensor_xform()/set_sensor_xform() functions are used to interface cleanly with the klampt klampt.math.se3 transform representation.

Getting images and point clouds

The camera_to_images(), camera_to_points(), and camera_to_points_world() functions convert raw CameraSensor outputs to Python objects that are more easily operated upon, e.g., images and point clouds. Use these to retrieve images as Numpy arrays.

The image_to_points() function converts a depth / color image to a point cloud, given camera intrinsic information.

Working with cameras

The ProjectiveCameraModel, camera_to_viewport(), and camera_from_viewport() help with converting to and from the klampt.vis.glprogram.GLViewport class used in klampt.vis.

camera_ray(), and camera_project() convert to/from image points. visible() determines whether a point or object is visible from a camera.

Functions:

camera_project(camera, robot, pt[, clip])

Given a point in world space, returns the (x,y,z) coordinates of the projected pixel.

camera_ray(camera, robot, x, y)

Returns the (source,direction) of a ray emanating from the SimRobotSensor at pixel coordinates (x,y).

camera_to_images(camera[, image_format, …])

Given a SimRobotSensor that is a CameraSensor, returns either the RGB image, the depth image, or both.

camera_to_points(camera[, points_format, …])

Given a SimRobotSensor that is a CameraSensor, returns a point cloud associated with the current measurements.

camera_to_points_world(camera, robot[, …])

Same as camera_to_points(), but converts to the world coordinate system given the robot to which the camera is attached.

camera_to_viewport(camera, robot)

Returns a GLViewport instance corresponding to the camera’s view.

get_sensor_xform(sensor[, robot])

Extracts the transform of a SimRobotSensor.

image_to_points(depth, color, xfov[, yfov, …])

Given a depth and optionally color image, returns a point cloud representing the depth or RGB-D scene.

set_sensor_xform(sensor, T[, link])

Given a link-mounted sensor (e.g., CameraSensor or ForceSensor), sets its link-local transform to T.

viewport_to_camera(viewport, camera, robot)

Fills in a simulated camera’s settings to match a GLViewport specifying the camera’s view.

visible(camera, object[, full, robot])

Tests whether the given object is visible in a SimRobotSensor or a GLViewport.
klampt.model.sensing.camera_project(camera, robot, pt, clip=True)[source]

Given a point in world space, returns the (x,y,z) coordinates of the projected pixel. z is given in absolute coordinates, while x,y are given in pixel values.

If clip=True and the point is out of the viewing volume, then None is returned. Otherwise, if the point is exactly at the focal plane then the middle of the viewport is returned.

If you are doing this multiple times, it’s faster to convert the camera to GLViewport and use GLViewport.project.

Parameters

  • camera (SimRobotSensor) – the camera

  • robot (RobotModel) – the robot on which the camera is mounted.

  • pt (3-vector) – world coordinates of point

  • clip (bool, optional) – if true, None will be returned if the point is outside of the viewing volume.

Returns

(x,y,z), where x,y are pixel value of image, z is depth.

Return type

tuple

klampt.model.sensing.camera_ray(camera, robot, x, y)[source]

Returns the (source,direction) of a ray emanating from the SimRobotSensor at pixel coordinates (x,y).

If you are doing this multiple times, it’s faster to convert the camera to GLViewport and use GLViewport.click_ray.

Parameters

  • camera (SimRobotSensor) – the camera

  • robot (RobotModel) – the robot on which the camera is mounted.

  • x (int/float) – x pixel coordinates

  • y (int/float) – y pixel coordinates

Returns

world-space ray source/direction.

Return type

(source,direction)

klampt.model.sensing.camera_to_images(camera, image_format='numpy', color_format='channels')[source]

Given a SimRobotSensor that is a CameraSensor, returns either the RGB image, the depth image, or both.

Parameters

  • camera (SimRobotSensor) – a sensor that is of ‘CameraSensor’ type

  • image_format (str) –

    governs the return type. Can be:

    • ’numpy’ (default): returns numpy arrays. Depending on the value of color_format, the RGB image either has shape (h,w,3) and dtype uint8 or (h,w) and dtype uint32. Depth images as numpy arrays with shape (h,w). Will fall back to ‘native’ if numpy is not available.

    • ’native’: returns list-of-lists arrays in the same format as above

  • color_format (str) – governs how pixels in the RGB result are packed.

  • be (Can) –

    • ‘channels’ (default): returns a 3D array with 3 channels

      corresponding to R, G, B values in the range [0,255].

    • ’rgb’ returns a 2D array with a 32-bit integer channel, with R,G,B channels packed in hex format 0xrrggbb.

    • ’bgr’: similar to ‘rgb’ but with hex order 0xbbggrr.

(Note that image_format=’native’ takes up a lot of extra memory, especially with color_format=’channels’)

Returns

(rgb, depth), which are either numpy arrays or list-of-lists format, as specified by image_format.

  • rgb: the RGB result (packed as specified by color_format)

  • depth: the depth result (floats)

Return type

tuple

klampt.model.sensing.camera_to_points(camera, points_format='numpy', all_points=False, color_format='channels')[source]

Given a SimRobotSensor that is a CameraSensor, returns a point cloud associated with the current measurements.

Points are triangulated with respect to the camera’s intrinsic coordinates, and are returned in the camera local frame (+z backward, +x toward the right, +y toward up).

The arguments

Parameters

  • points_format (str, optional) –

    configures the format of the return value. Can be:

    • ’numpy’ (default): either an Nx3, Nx4, or Nx6 numpy array, depending on whether color is requested (and its format). Will fall back to ‘native’ if numpy is not available.

    • ’native’: same as numpy, but in list-of-lists format rather than numpy arrays.

    • ’PointCloud’: a Klampt PointCloud object

    • ’Geometry3D’: a Klampt Geometry3D point cloud object

  • all_points (bool, optional) – configures whether bad points should be stripped out. If False (default), this strips out all pixels that don’t have a good depth reading (i.e., the camera sensor’s maximum reading.) If True, these pixels are all set to (0,0,0).

  • color_format (str) –

    If the sensor has an RGB component, then color channels may be produced. This value configures the output format, and can take on the values:

    • ’channels’: produces individual R,G,B channels in the range [0,1]. (note this is different from the interpretation of camera_to_images)

    • ’rgb’: produces a single 32-bit integer channel packing the 8-bit color channels together in the format 0xrrggbb.

    • None: no color is produced.

Returns

the point cloud in the requested format.

Return type

object

klampt.model.sensing.camera_to_points_world(camera, robot, points_format='numpy', color_format='channels')[source]

Same as camera_to_points(), but converts to the world coordinate system given the robot to which the camera is attached.

Points that have no reading are stripped out.

klampt.model.sensing.camera_to_viewport(camera, robot)[source]

Returns a GLViewport instance corresponding to the camera’s view.

See klampt.vis.glprogram and klampt.vis.visualization for information about how to use the object with the visualization, e.g. vis.setViewport(vp).

Parameters

  • camera (SimRobotSensor) – the camera instance.

  • robot (RobotModel) – the robot on which the camera is located, which should be set to the robot’s current configuration. This could be set to None, in which case the camera’s transform is in its link’s local coordinates.

Returns

matches the camera’s viewport.

Return type

GLViewport

klampt.model.sensing.get_sensor_xform(sensor, robot=None)[source]

Extracts the transform of a SimRobotSensor. The sensor must be of a link-mounted type, e.g., a CameraSensor or ForceSensor.

Parameters

  • sensor (SimRobotSensor) –

  • robot (RobotModel, optional) – if provided, returns the world coordinates of the sensor. Otherwise, returns the local coordinates on the link to which it is mounted.

Returns

the sensor transform

Return type

klampt.se3 object

klampt.model.sensing.image_to_points(depth, color, xfov, yfov=None, depth_scale=None, depth_range=None, color_format='auto', points_format='numpy', all_points=False)[source]

Given a depth and optionally color image, returns a point cloud representing the depth or RGB-D scene.

Parameters

  • depth (list of lists or numpy array) – the w x h depth image (rectified).

  • color (list of lists or numpy array, optional) – the w x h color image. Assumed that color maps directly onto depth pixels. If None, an uncolored point cloud will be produced.

  • xfov (float) – horizontal field of view, in radians.

  • yfov (float, optional) – vertical field of view, in radians. If not given, square pixels are assumed.

  • depth_scale (float, optional) – a scaling from depth image values to absolute depth values.

  • depth_range (pair of floats, optional) – if given, only points within this depth range (non-inclusive) will be extracted. If all_points=False, points that fail the range test will be stripped from the output. E.g., (0.5,8.0) only extracts points with z > 0.5 and z < 8 units.

  • color_format (str) –

    governs how pixels in the RGB result are packed. Can be:

    • ’auto’ (default): if it’s a 3D array, it assumes elements are in ‘channels’ format, otherwise it assumes ‘rgb’.

    • ’channels’: a 3D array with 3 channels corresponding to R, G, B values in the range [0,255] if uint8 type, otherwise in the range [0,1].

    • ’rgb’ a 2D array with a 32-bit integer channel, with R,G,B channels packed in hex format 0xrrggbb.

  • points_format (str, optional) –

    configures the format of the return value. Can be:

    • ’numpy’ (default): either an Nx3, Nx4, or Nx6 numpy array, depending on whether color is requested (and its format). Will fall back to ‘native’ if numpy is not available.

    • ’native’: same as numpy, but in list-of-lists format rather than numpy arrays.

    • ’PointCloud’: a Klampt PointCloud object

    • ’Geometry3D’: a Klampt Geometry3D point cloud object

  • all_points (bool, optional) – configures whether bad points should be stripped out. If False (default), this strips out all pixels that don’t have a good depth reading (i.e., the camera sensor’s maximum reading.) If True, these pixels are all set to (0,0,0).

Returns

the point cloud. Represented as being local to the standard camera frame with +x to the right, +y down, +z forward.

Return type

numpy ndarray or Geometry3D

klampt.model.sensing.set_sensor_xform(sensor, T, link=None)[source]

Given a link-mounted sensor (e.g., CameraSensor or ForceSensor), sets its link-local transform to T.

Parameters

Another way to set a sensor is to give a coordinates.Frame object. This frame must either be associated with a RobotModelLink or its parent should be associated with one.

(the reason why you should use this is that the Tsensor attribute has a particular format using the loader.writeSe3 function.)

klampt.model.sensing.viewport_to_camera(viewport, camera, robot)[source]

Fills in a simulated camera’s settings to match a GLViewport specifying the camera’s view.

Parameters

  • viewport (GLViewport) – the viewport to match

  • camera (SimRobotSensor) – the viewport will be output to this sensor

  • robot (RobotModel) – the robot on which the camera is located, which should be set to the robot’s current configuration. This could be set to None, in which case the camera’s transform is in its link’s local coordinates.

klampt.model.sensing.visible(camera, object, full=True, robot=None)[source]

Tests whether the given object is visible in a SimRobotSensor or a GLViewport.

If you are doing this multiple times, first convert to GLViewport.

Parameters

  • camera (SimRobotSensor or GLViewport) – the camera.

  • object – a 3-vector, a (center,radius) pair indicating a sphere, an axis-aligned bounding box (bmin,bmax), a Geometry3D, or an object that has a geometry() method, e.g., RigidObjectModel, RobotModelLink.

  • full (bool, optional) – if True, the entire object must be in the viewing frustum for it to be considered visible. If False, any part of the object can be in the viewing frustum.

  • robot (RobotModel) – if camera is a SimRobotSensor, this will be used to derive the transform.