pydrake.perception

Python bindings for //perception

class pydrake.perception.BaseField

Indicates the data the point cloud stores.

Members:

kNone :

kXYZs : XYZ point in Cartesian space.

kNormals : Normals.

kRGBs : RGB colors.

__init__(self: pydrake.perception.BaseField, value: int) None
kNone = <BaseField.kNone: 0>
kNormals = <BaseField.kNormals: 4>
kRGBs = <BaseField.kRGBs: 8>
kXYZs = <BaseField.kXYZs: 2>
property name
property value
pydrake.perception.Concatenate(clouds: list[pydrake.perception.PointCloud]) pydrake.perception.PointCloud

Returns a new point cloud that includes all of the points from the point clouds in clouds. All of the clouds must have the same fields.

Precondition:

clouds contains at least one point cloud.

Raises

RuntimeError if the clouds have different fields defined.

class pydrake.perception.DepthImageToPointCloud

Bases: pydrake.systems.framework.LeafSystem

Converts a depth image to a point cloud.

depth_image→
color_image (optional)→
camera_pose (optional)→
DepthImageToPointCloud
→ point_cloud

The system has an input port that takes a depth image, an optional input port that takes a color image, and an additional optional input port that takes the camera_pose, X_PC. If the camera_pose input is connected, then the point cloud is represented in the parent frame (e.g., if camera_pose is the pose of the camera in the world frame, then the point_cloud output will be a PointCloud in the world frame). If the camera_pose input is not connected, the PointCloud will be represented in the camera frame. Note that if a color image is provided, it must be in the same frame as the depth image.

If a pixel is NaN, the converted point will be (NaN, NaN, NaN). If a pixel is kTooClose or kTooFar (as defined by ImageTraits), the converted point will be (+Inf, +Inf, +Inf). Note that this matches the convention used by the Point Cloud Library (PCL).

__init__(self: pydrake.perception.DepthImageToPointCloud, camera_info: pydrake.systems.sensors.CameraInfo, pixel_type: pydrake.systems.sensors.PixelType = <PixelType.kDepth32F: 6>, scale: float = 1.0, fields: int = <BaseField.kXYZs: 2>) None

Constructs the converter.

Parameter camera_info:

The camera info.

Parameter depth_pixel_type:

The pixel type of the depth image input. Only 16U and 32F are supported.

Parameter scale:

The depth image input is multiplied by this scale factor before projecting to a point cloud. (This is useful for converting mm to meters, etc.)

Parameter fields:

The fields the point cloud contains.

camera_pose_input_port(self: pydrake.perception.DepthImageToPointCloud) pydrake.systems.framework.InputPort

Returns the abstract valued input port that expects X_PC as a RigidTransformd. (This input port does not necessarily need to be connected; refer to the class overview for details.)

color_image_input_port(self: pydrake.perception.DepthImageToPointCloud) pydrake.systems.framework.InputPort

Returns the abstract valued input port that expects an ImageRgba8U.

depth_image_input_port(self: pydrake.perception.DepthImageToPointCloud) pydrake.systems.framework.InputPort

Returns the abstract valued input port that expects either an ImageDepth16U or ImageDepth32F (depending on the constructor argument).

point_cloud_output_port(self: pydrake.perception.DepthImageToPointCloud) pydrake.systems.framework.OutputPort

Returns the abstract valued output port that provides a PointCloud. Only the channels passed into the constructor argument “fields” are present.

class pydrake.perception.Fields

Allows combination of BaseField and DescriptorType for a PointCloud. You may combine multiple BaseField`s, but you may have only zero or one `DescriptorType.

This provides the mechanism to use basic bit-mask operators (| &) to combine / intersect fields for convenience.

__init__(self: pydrake.perception.Fields, base_fields: int) None
Raises
  • RuntimeError if base_fields is not composed of valid

  • BaseField

base_fields(self: pydrake.perception.Fields) int

Returns the contained base fields.

has_base_fields(self: pydrake.perception.Fields) bool

Returns whether there are any base fields contained by this set of fields.

class pydrake.perception.PointCloud

Implements a point cloud (with contiguous storage), whose main goal is to offer a convenient, synchronized interface to commonly used fields and data types applicable for basic 3D perception.

This is a mix between the philosophy of PCL (templated interface to provide a compile-time open set, run-time closed set) and VTK (non-templated interface to provide a very free form run-time open set).

Definitions:

  • point - An entry in a point cloud (not exclusively an XYZ point).

  • feature - Abstract representation of local properties (geometric and non-geometric)

  • descriptor - Concrete representation of a feature.

  • field - A feature or descriptor described by the point cloud.

This point cloud class provides the following fields:

  • xyz - Cartesian XYZ coordinates (float[3]).

  • descriptor - A descriptor that is run-time defined (float[X]).

Note

“contiguous” here means contiguous in memory. This was chosen to avoid ambiguity between PCL and Eigen, where in PCL “dense” implies that the point cloud corresponds to a cloud with only valid values, and in Eigen “dense” implies contiguous storage.

Note

The accessors / mutators for the point fields of this class returns references to the original Eigen matrices. This implies that they are invalidated whenever memory is reallocated for the values. Given this, minimize the lifetime of these references to be as short as possible. Additionally, algorithms wanting fast access to values should avoid the single point accessors / mutatotrs (e.g. xyz(i), mutable_descriptor(i)) to avoid overhead when accessing a single element (either copying or creating a reference).

Note

The definitions presented here for “feature” and “descriptor” are loosely based on their definitions within PCL and Radu Rusu’s dissertation: Rusu, Radu Bogdan. “Semantic 3d object maps for everyday manipulation in human living environments.” KI-Künstliche Intelligenz 24.4 (2010): 345-348. This differs from other definitions, such as having “feature” describe geometric quantities and “descriptor” describe non-geometric quantities which is presented in the following survey paper: Pomerleau, François, Francis Colas, and Roland Siegwart. “A review of point cloud registration algorithms for mobile robotics.” Foundations and Trends® in Robotics 4.1 (2015): 1-104.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.perception.PointCloud, new_size: int = 0, fields: pydrake.perception.Fields = Fields(base_fields=2)) -> None

Constructs a point cloud of a given new_size, with the prescribed fields. If kDescriptors is one of the fields, then descriptor should be included and should not be kNone.

Parameter new_size:

Size of the point cloud after construction.

Parameter fields:

Fields that the point cloud contains.

Parameter skip_initialize:

Do not default-initialize new values.

  1. __init__(self: pydrake.perception.PointCloud, other: pydrake.perception.PointCloud) -> None

Copies another point cloud’s fields and data.

C

alias of numpy.uint8

Crop(self: pydrake.perception.PointCloud, lower_xyz: numpy.ndarray[numpy.float32[3, 1]], upper_xyz: numpy.ndarray[numpy.float32[3, 1]]) pydrake.perception.PointCloud

Returns a new point cloud containing only the points in this with xyz values within the axis-aligned bounding box defined by lower_xyz and upper_xyz. Requires that xyz values are defined.

Precondition:

lower_xyz <= upper_xyz (elementwise).

Raises

RuntimeError if has_xyzs() != true.

D

alias of numpy.float32

EstimateNormals(self: pydrake.perception.PointCloud, radius: float, num_closest: int, parallelize: pydrake.common.Parallelism = False) bool

Estimates the normal vectors in this by fitting a plane at each point in the cloud using up to num_closest points within Euclidean distance radius from the point. If has_normals() is false, then new normals will be allocated (and has_normals() will become true). Points for which the normals cannot be estimated (because the this has less than two closest points within the radius), will receive normal [NaN, NaN, NaN]. Normals estimated from two closest points will be orthogonal to the vector between those points, but can be arbitrary in the last dimension. parallelize enables OpenMP parallelization.

Returns

true iff all points were assigned normals by having at least three closest points within radius.

Precondition:

radius > 0 and num_closest >= 3.

Raises

RuntimeError if has_xyzs() is false.

fields(self: pydrake.perception.PointCloud) pydrake.perception.Fields

Returns the fields provided by this point cloud.

FlipNormalsTowardPoint(self: pydrake.perception.PointCloud, p_CP: numpy.ndarray[numpy.float32[3, 1]]) None

Changes the sign of the normals in this, if necessary, so that each normal points toward the point P in the frame C in which the xyzs of this cloud are represented. This can be useful, for instance, when P is the position of the camera used to generate the cloud.

Raises

RuntimeError if has_xyzs() != true or has_normals() != true.

has_normals(self: pydrake.perception.PointCloud) bool

Returns if this cloud provides normals.

has_rgbs(self: pydrake.perception.PointCloud) bool

Returns if this cloud provides RGB colors.

has_xyzs(self: pydrake.perception.PointCloud) bool

Returns if this cloud provides XYZ values.

static IsDefaultValue(value: float) bool
static IsInvalidValue(value: float) bool
kDefaultValue = nan
mutable_normal(self: pydrake.perception.PointCloud, i: int) numpy.ndarray[numpy.float32[3, 1], flags.writeable]

Returns mutable access to a normal.

Precondition:

has_normals() must be true.

mutable_normals(self: pydrake.perception.PointCloud) numpy.ndarray[numpy.float32[3, n], flags.writeable, flags.f_contiguous]

Returns mutable access to normals.

Precondition:

has_normals() must be true.

mutable_rgb(self: pydrake.perception.PointCloud, i: int) numpy.ndarray[numpy.uint8[3, 1], flags.writeable]

Returns mutable access to an RGB color.

Precondition:

has_rgbs() must be true.

mutable_rgbs(self: pydrake.perception.PointCloud) numpy.ndarray[numpy.uint8[3, n], flags.writeable, flags.f_contiguous]

Returns mutable access to RGB colors.

Precondition:

has_rgbs() must be true.

mutable_xyz(self: pydrake.perception.PointCloud, i: int) numpy.ndarray[numpy.float32[3, 1], flags.writeable]

Returns mutable access to an XYZ value.

Precondition:

has_xyzs() must be true.

mutable_xyzs(self: pydrake.perception.PointCloud) numpy.ndarray[numpy.float32[3, n], flags.writeable, flags.f_contiguous]

Returns mutable access to XYZ values.

Precondition:

has_xyzs() must be true.

normal(self: pydrake.perception.PointCloud, i: int) numpy.ndarray[numpy.float32[3, 1]]

Returns access to a normal.

Precondition:

has_normals() must be true.

normals(self: pydrake.perception.PointCloud) numpy.ndarray[numpy.float32[3, n], flags.f_contiguous]

Returns access to normals.

Precondition:

has_normals() must be true.

resize(self: pydrake.perception.PointCloud, new_size: int) None

Conservative resize; will maintain existing data, and initialize new data to their invalid values.

Parameter new_size:

The new size of the value. If less than the present size(), then the values will be truncated. If greater than the present size(), then the new values will be uninitialized if skip_initialize is not true.

Parameter skip_initialize:

Do not default-initialize new values.

rgb(self: pydrake.perception.PointCloud, i: int) numpy.ndarray[numpy.uint8[3, 1]]

Returns access to an RGB color.

Precondition:

has_rgbs() must be true.

rgbs(self: pydrake.perception.PointCloud) numpy.ndarray[numpy.uint8[3, n], flags.f_contiguous]

Returns access to RGB colors.

Precondition:

has_rgbs() must be true.

SetFields(self: pydrake.perception.PointCloud, new_fields: pydrake.perception.Fields, skip_initialize: bool = False) None

Updates the point cloud to a given set of fields. In the case of introducing a new field, its container will be allocated with the current size and default initialized. The data for all retained fields will remain unchanged.

Parameter new_fields:

New fields to set to.

Parameter skip_initialize:

Do not default-initialize new values.

SetFrom(self: pydrake.perception.PointCloud, other: pydrake.perception.PointCloud) None

Copies all points from another point cloud.

Parameter other:

Other point cloud.

Parameter fields_in:

Fields to copy. If this is kInherit, then `other`s fields will be copied. Otherwise, both clouds must support the fields indicated this parameter.

Parameter allow_resize:

Permit resizing to the other cloud’s size.

size(self: pydrake.perception.PointCloud) int

Returns the number of points in this point cloud.

T

alias of numpy.float32

VoxelizedDownSample(self: pydrake.perception.PointCloud, voxel_size: float, parallelize: pydrake.common.Parallelism = False) pydrake.perception.PointCloud

Returns a down-sampled point cloud by grouping all xyzs in this cloud into a 3D grid with cells of dimension voxel_size. Each occupied voxel will result in one point in the downsampled cloud, with a location corresponding to the centroid of the points in that voxel. Points with non-finite xyz values are ignored. All other fields (e.g. rgbs, normals, and descriptors) with finite values will also be averaged across the points in a voxel. parallelize enables OpenMP parallelization. Equivalent to Open3d’s voxel_down_sample or PCL’s VoxelGrid filter.

Raises
  • RuntimeError if has_xyzs() is false.

  • RuntimeError if voxel_size <= 0.

xyz(self: pydrake.perception.PointCloud, i: int) numpy.ndarray[numpy.float32[3, 1]]

Returns access to an XYZ value.

Precondition:

has_xyzs() must be true.

xyzs(self: pydrake.perception.PointCloud) numpy.ndarray[numpy.float32[3, n], flags.f_contiguous]

Returns access to XYZ values.

Precondition:

has_xyzs() must be true.

class pydrake.perception.PointCloudToLcm

Bases: pydrake.systems.framework.LeafSystem

Converts PointCloud inputs to lcmt_point_cloud output messages. The message can be transmitted to other processes using LcmPublisherSystem.

point_cloud→
PointCloudToLcm
→ lcmt_point_cloud

Any descriptor channels of the PointCloud will currently be ignored, though may be added in a future revision.

Only the finite points from the cloud are copied into the message (too-close or too-far points from a depth sensor are omitted).

__init__(self: pydrake.perception.PointCloudToLcm, frame_name: str = '') None

Constructs a system that outputs messages using the given frame_name.