klampt.model.geometry module

Utility functions for operating on geometry. See the Geometry3D documentation for the core geometry class.

Added in version 0.8.6.

[functions moved here from klampt.model.sensing]

Working with geometric primitives

box() and sphere() are aliases for the functions in klampt.model.create.primitives.

Working with point clouds

point_cloud_normals() estimates normals from a normal-free PointCloud.

The fit_plane(), fit_plane3(), and PlaneFitter class help with plane estimation.

The align_points() and align_points_rotation() functions solve for point cloud alignment. .. versionadded:: 0.9.2

point_cloud_simplify() simplifies a PointCloud.

point_cloud_colors() and point_cloud_set_colors() sets / gets colors from a PointCloud.

Other utilities

merge() can put together TriangleMesh and PointCloud geometries into unified geometries.

triangle_normals() computes triangle normals for a TriangleMesh. Note: should be replaced with TriangleMesh.triangle_normals() for future code.

vertex_normals() computes vertex normals for a TriangleMesh. Note: should be replaced with TriangleMesh.triangle_normals() for future code. .. versionadded:: 0.9.2

sample_surface() samples points on the surface of a Geometry3D. .. versionadded:: 0.9.2

klampt.model.geometry.box(width, depth, height, center=None, R=None, t=None, world=None, name=None, mass=inf, type='TriangleMesh')[source]

Alias for klampt.model.create.primitives.box()

klampt.model.geometry.sphere(radius, center=None, R=None, t=None, world=None, name=None, mass=inf, resolution=0.0, type='TriangleMesh')[source]

Alias for klampt.model.create.primitives.sphere()

class klampt.model.geometry.PlaneFitter(points=None)[source]

Bases: object

Online fitting of planes to 3D point clouds. Updating via add_point() and retrieving via plane_equation() is O(1).

normal

best-fit normal

Type:

3-vector

centroid

centroid of points

Type:

3-vector

count

# of points

Type:

int

sse

fitting sum of squared errors

Type:

float

cov

covariance of points

Type:

3x3 array

plane_equation()[source]

Returns (a,b,c,d) with ax+by+cz+d=0 the plane equation

goodness_of_fit()[source]

Returns corrected RMSE

add_point(pt)[source]

Online estimation of best fit plane

merge(fitter, inplace=False)[source]

Online merging of two plane fitters.

If inplace = False, returns a new PlaneFitter. If inplace = True, self is updated with the result.

distance(pt)[source]

Returns the signed distance to this plane

klampt.model.geometry.point_cloud_simplify(pc, radius)[source]

Simplifies a point cloud by averaging points within neighborhoods. Uses a fast hash grid data structure.

Return type:

Union[Geometry3D, PointCloud]

Parameters:
klampt.model.geometry.point_cloud_normals(pc, estimation_radius=None, estimation_knn=None, estimation_viewpoint=None, add=True)[source]

Returns the normals of the point cloud. If pc has the standard normal_x, normal_y, normal_z properties, these will be returned. Otherwise, they will be estimated using plane fitting.

The plane fitting method uses scipy nearest neighbor detection if scipy is available. Otherwise it uses a spatial grid. The process is as follows: :rtype: ndarray

  • If estimation_radius is provided, then it will use neighbors within this range. For a spatial grid, this is the grid size.

  • If estimation_knn is provided, then planes will be fit to these number of neighbors.

  • If neither is provided, then estimation_radius is set to 3 * max dimension of the point cloud / sqrt(N).

  • If not enough points are within a neighborhood (either 4 or estimation_knn, whichever is larger), then the normal is set to 0.

  • If estimation_viewpoint is provided, this must be a 3-list. The normals are oriented such that they point toward the viewpoint.

If add=True, estimated normals will be added to the point cloud under the normal_x, normal_y, normal_z properties.

Returns:

A N x 3 numpy array of normals in the local frame of the point cloud.

klampt.model.geometry.fit_plane3(point1, point2, point3)[source]

Returns a 3D plane equation fitting the 3 points.

The result is (a,b,c,d) with the plane equation ax+by+cz+d=0

Return type:

Tuple[float, float, float, float]

klampt.model.geometry.fit_plane(points)[source]

Returns a 3D plane equation that is a least squares fit through the points (len(points) >= 3).

Return type:

Tuple[float, float, float, float]

klampt.model.geometry.fit_plane_centroid(points)[source]

Similar to fit_plane(), but returns a (centroid,normal) pair.

Return type:

Tuple[Sequence[float], Sequence[float]]

klampt.model.geometry.align_points_rotation(apts, bpts)[source]

Computes a 3x3 rotation matrix that rotates the points apts to minimize the distance to bpts.

apts and bpts can either be a list of 3-vectors, nx3 numpy array, or a PointCloud.

Return type:

Sequence[float]

Returns:

the klampt.so3 element that minimizes the sum of squared errors ||R*ai-bi||^2.

Return type:

Rotation

klampt.model.geometry.align_points(apts, bpts)[source]

Finds a 3D rigid transform that maps the list of points apts to the list of points bpts.

apts and bpts can either be a list of 3-vectors, nx3 numpy array, or a PointCloud.

Return type:

Tuple[Sequence[float], Sequence[float]]

Returns:

the klampt.se3 element that minimizes the sum of squared errors ||T*ai-bi||^2.

Return type:

RigidTransform

klampt.model.geometry.point_cloud_colors(pc, format='rgb')[source]

Returns the colors of the point cloud in the given format. If the point cloud has no colors, this returns None. If the point cloud has no colors but has opacity, this returns white colors.

Return type:

ndarray

Parameters:

  • pc (PointCloud) – the point cloud

  • format –

    describes the output color format, either:

    • ’rgb’: packed 32bit int, with the hex format 0xrrggbb (only 24

      bits used),

    • ’bgr’: packed 32bit int, with the hex format 0xbbggrr (only 24

      bits used),

    • ’rgba’: packed 32bit int, with the hex format 0xrrggbbaa,

    • ’bgra’: packed 32bit int, with the hex format 0xbbggrraa,

    • ’argb’: packed 32bit int, with the hex format 0xaarrggbb,

    • ’abgr’: packed 32bit int, with the hex format 0xaabbggrr,

    • (‘r’,’g’,’b’): triple with each channel in range [0,1]

    • (‘r’,’g’,’b’,’a’): tuple with each channel in range [0,1]

    • ’channels’: returns a list of channels, in the form (r,g,b) or

      (r,g,b,a), where each value in the channel has range [0,1].

    • ’opacity’: returns opacity only, in the range [0,1].

Returns:

A an array of len(pc.points) colors corresponding to the points in the point cloud. If format=’channels’, the return value is a tuple (r,g,b) or (r,g,b,a).

klampt.model.geometry.point_cloud_set_colors(pc, colors, color_format='rgb', pc_property='auto')[source]

Sets the colors of a point cloud.

Return type:

None

Parameters:

  • pc (PointCloud) – the point cloud

  • colors (list or numpy.ndarray) – the array of colors, and each color can be either ints, tuples, or channels, depending on color_format.

  • color_format –

    describes the format of each element of colors, and can be:

    • ’rgb’: packed 32bit int, with the hex format 0xrrggbb (only 24

      bits used),

    • ’bgr’: packed 32bit int, with the hex format 0xbbggrr (only 24

      bits used),

    • ’rgba’: packed 32bit int, with the hex format 0xrrggbbaa,

    • ’bgra’: packed 32bit int, with the hex format 0xbbggrraa,

    • ’argb’: packed 32bit int, with the hex format 0xaarrggbb,

    • ’abgr’: packed 32bit int, with the hex format 0xaabbggrr,

    • (‘r’,’g’,’b’): triple with each channel in range [0,1]. Also use this if colors is an n x 3 numpy array.

    • (‘r’,’g’,’b’,’a’): tuple with each channel in range [0,1]. Also use this if colors is an n x 4 numpy array.

    • ’channels’: colors is a list of 3 or 4 channels, in the form

      (r,g,b) or (r,g,b,a), where each element in a channel has range [0,1].

    • ’opacity’: opacity only, in the range [0,1].

  • pc_property (str) – describes to which property the colors should be set. ‘auto’ determines chooses the property from the point cloud if it’s already colored, or color_format if not. ‘channels’ sets the ‘r’, ‘g’, ‘b’, and optionally ‘a’ properties.

klampt.model.geometry.triangle_normals(trimesh)[source]

Returns a list or numpy array of (outward) triangle normals for a triangle mesh.

Return type:

ndarray

Parameters:

trimesh (TriangleMesh or Geometry3D)

Returns:

An N x 3 matrix of triangle normals with N the number of triangles.

klampt.model.geometry.vertex_normals(trimesh, area_weighted=True)[source]

Returns a list or numpy array of (outward) vertex normals for a triangle mesh.

Return type:

ndarray

Parameters:

  • trimesh (TriangleMesh or Geometry3D)

  • area_weighted (bool) – whether to compute area-weighted average or simple average.

Returns:

An N x 3 matrix of triangle normals with N the number of vertices.

klampt.model.geometry.merge(*items)[source]

Merges one or more geometries into a single geometry.

Return type:

Geometry3D

Parameters:

items – Each arg must be a Geometry3D, TriangleMesh, PointCloud, RobotLinkModel, RigidObjectModel, or TerrainModel.

Returns:

A merged geometry. If all underlying geometries have PointCloud type, then the result will have PointCloud type. Otherwise, they must all be convertable to TriangleMesh, and the result will be of TriangleMesh type.

klampt.model.geometry.sample_surface(geom, num_samples=None, local=True, want_elements=False, want_normals=False)[source]

Samples the surface of a geometry uniformly at random.

If local=True, points are returned in the local coordinate frame. Otherwise, the object’s current transform is applied.

If want_elements=True, then the indices of the elements (i.e., triangles or point indices) are returned the result column 3. This only makes sense for TriangleMesh and PointCloud types.

If want_normals=True, then the normals at each point are returned in the result columns [3:6] (if want_elements=False) or [4:7] if (want_elements=True).

If num_samples = None, then all vertices of the representation will be returned. This cannot be None for spheres.

Return type:

ndarray