"""
Thin wrapper around Open3D point-cloud objects with MVTB look and feel.
"""
from pathlib import Path
from warnings import warn
from typing import Any, Callable
import numpy as np
import spatialmath.base as smb
from spatialmath import SE3
try:
import open3d as o3d
_open3d = True
except ImportError:
_open3d = False
[docs]
class PointCloud:
[docs]
def __init__(self, arg: Any, depth_scale: float = 1.0, **kwargs) -> None:
"""
Create new point cloud object
:param arg: point cloud data as an Open3D or MVTB PointCloud object, 3xN array of points.
:type arg: :obj:`o3d.geometry.PointCloud`, :obj:`~machinevisiontoolbox.PointCloud`, ndarray(3,N)
:param depth_scale: depth scale factor, defaults to 1.0
:type depth_scale: float, optional
:raises RuntimeError: PointCloud class requires Open3D to be installed: pip install open3d or pip install machinevision-toolbox-python[open3d]
:raises ValueError: bad arguments
This object wraps an Open3D :obj:`open3d.geometry.PointCloud` object. It can be
created from:
- an Open3D point cloud object
- a 3xN array of point coordinates
- a depth image as a 1-plane :class:`~machinevisiontoolbox.Image` instance
- an RGBD image as a 4-plane :class:`~machinevisiontoolbox.Image` instance
- a tuple of two images (RGB, D) as 3-plane and 1-plane :class:`~machinevisiontoolbox.Image` instances
.. warning:: Open3D must be installed.
The wrapped Open3D :obj:`open3d.geometry.PointCloud` object has many methods and properties
but rather than writing individual wrappers the :meth:`__getattr__` picks up these
references and invokes them on the underlying Open3D object. Any returned Open3D
point cloud objects are converted to a :class:`PointCloud` instance.
:seealso: :meth:`__getattr__` :obj:`open3d.geometry.PointCloud` :class:`~machinevisiontoolbox.Image` :class:`CentralCamera`
"""
if not _open3d:
raise RuntimeError(
"PointCloud class requires Open3D to be installed: "
"pip install open3d or "
"pip install machinevision-toolbox-python[open3d]\n"
" ** Note that Open3d typically does not run on the latest Python version(s)"
)
from machinevisiontoolbox import Image
if not _open3d:
raise RuntimeError(
"PointCloud class requires Open3D to be installed: "
"pip install open3d or "
"pip install machinevision-toolbox-python[open3d]"
)
if isinstance(arg, o3d.geometry.PointCloud):
# Open3D point cloud
pcd = arg
elif isinstance(arg, np.ndarray):
arg = smb.getmatrix(arg, (3, None)).astype("float32")
# simple point cloud:
# passed a 3xN array of point coordinates
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(arg.T)
else:
raise ValueError(
"arg must be PointCloud or ndarray, for a depth image use DepthImage() constructor"
)
self._pcd = pcd
@staticmethod
def _intrinsics(camera: Any) -> Any:
"""Return an Open3D style camera intrinsic object
:param camera: camera model
:type camera: :class:`~machinevisiontoolbox.Camera.CentralCamera`
:return: Open3D camera intrinsic object
:rtype: :class:`open3d.camera.PinholeCameraIntrinsic`
"""
return o3d.camera.PinholeCameraIntrinsic(
camera.width, camera.height, *camera.fpix, *camera.pp
)
@staticmethod
def _image(image: Any) -> Any:
"""Return an Open3D style image object
:param image: MVTB image
:type image: :class:`~machinevisiontoolbox.Image` instance
:return: Open3D image
:rtype: :class:`open3d.geometry.Image` instance
"""
img = image._A
if not img.data.c_contiguous:
img = img.copy() # make contiguous
return o3d.geometry.Image(img)
[docs]
def copy(self) -> "PointCloud":
"""
Copy point cloud
:return: copy of point cloud
:rtype: :class:`PointCloud`
Return a new point cloud instance that contains a copy of
the point data.
"""
return self.__class__(o3d.geometry.PointCloud(self._pcd))
[docs]
def __len__(self) -> int:
"""
Number of points
:return: number of points
:rtype: int
"""
return np.asarray(self._pcd.points).shape[0]
# allow methods of o3d.geometry.PointCloud to be invoked indirectly
[docs]
def __getattr__(self, name: str) -> Any:
"""
Access Open3D PointCloud methods and attributes
:param name: method or attribute name
:type name: str
If ``P`` is a :class:`PointCloud` then ``P.name`` invokes
method ``name`` of the underlying :obj:`open3d.geometry.PointCloud`
object. For example:
.. code-block:: python
pcfiltered, ind = pc.remove_radius_outlier(radius=0.2, nb_points=100)
pc.is_empty() # invoke is_empty predicate method
pc.colors # get colors attribute
.. note::
- This is a convenience method to avoid having to explicitly all
the properties and methods of the Open3D point cloud object.
- If the method returns a new point cloud object, it is converted to a
Toolbox :class:`PointCloud` instance.
"""
def wrapper(*args: Any, **kwargs: Any) -> Any:
meth = getattr(self._pcd, name)
retval = meth(*args, **kwargs)
if isinstance(retval, tuple):
return tuple(
PointCloud(r) if isinstance(r, o3d.geometry.PointCloud) else r
for r in retval
)
else:
return retval
if hasattr(self._pcd, name):
if callable(getattr(self._pcd, name)):
return wrapper
else:
return getattr(self._pcd, name)
else:
raise ValueError(f"object has no attribute '{name}'")
[docs]
def __str__(self) -> str:
"""
Concise string representation of point cloud parameters
:return: concise string representation of the wrapped point cloud
:rtype: str
"""
return str(self._pcd)
[docs]
def __repr__(self) -> str:
return f"PointCloud(npoints={len(self)}, colored={self.iscolor})"
@property
def pcd(self) -> Any:
return self._pcd
@property
def points(self) -> np.ndarray:
"""
Get points as array
:return: points as array
:rtype: ndarray(3,N)
Points are returned as columns of the array. The columns correspond
to the same columns in the :attr:`colors` and :attr:`normal` properties.
:seealso: :attr:`colors` :attr:`normals`
"""
return np.asarray(self._pcd.points).T
@property
def colors(self) -> np.ndarray:
"""
Get point color data as array
:return: point color
:rtype: ndarray(3,N)
Point colors are returned as columns of the array. The columns correspond
to the same columns in the :attr:`points` and :attr:`normal` properties.
:seealso: :attr:`points` :attr:`normals`
"""
return np.asarray(self._pcd.colors).T
@property
def iscolor(self) -> bool:
return self._pcd.has_colors()
[docs]
@classmethod
def Read(cls, filename: str, *args: Any, **kwargs: Any) -> "PointCloud":
"""
Create point cloud from file
:param filename: name of file
:type filename: str
:return: new point cloud
:rtype: :class:`PointCloud` instance
Read point cloud data from a file using :func:`open3d.io.read_point_cloud`.
Supported filetypes include PLY.
If the path is not absolute it is first searched for relative
to the current directory, and if not found, it is searched for in
the ``data`` folder of the
`mvtb_data package <https://github.com/petercorke/machinevision-toolbox-python/tree/master/packages/mvtb-data>`_.
.. note:: ``filename`` may be given as a bare name (``'bunny.ply'``)
or with a redundant leading ``data/`` (``'data/bunny.ply'``, the
older convention some book examples still use) -- both resolve
to the same file.
"""
from machinevisiontoolbox import mvtb_path_to_datafile
# accept a caller-supplied redundant "data/" prefix (older
# convention) without double-joining it against the "data" this
# method already prepends
parts = Path(filename).parts
if parts and parts[0] == "data":
filename = str(Path(*parts[1:])) if len(parts) > 1 else ""
filename = mvtb_path_to_datafile("data", filename, string=True)
pcd = o3d.io.read_point_cloud(filename, *args, **kwargs)
return cls(pcd)
@classmethod
def DepthImage(
cls,
depth: Any,
camera: Any,
rgb: Any = None,
depth_scale: float = 1.0,
**kwargs: Any,
) -> "PointCloud":
"""Create point cloud from depth image
Creates a colored or uncolored point cloud.
:param depth: depth image or an RGBD image.
:type depth: :class:`~machinevisiontoolbox.Image`
:param camera: perspective camera model
:type camera: :class:`~machinevisiontoolbox.Camera.CentralCamera`
:param rgb: RGB image for colorizing point cloud, defaults to None
:type rgb: :class:`~machinevisiontoolbox.Image`, optional
:param depth_scale: depth scale factor, defaults to 1.0
:type depth_scale: float, optional
:raises RuntimeError: PointCloud class requires Open3D to be installed: pip install open3d or pip install machinevision-toolbox-python[open3d]
:raises ValueError: input images has wrong number of planes
The depth image is converted to a point cloud using the Open3D, and the pixel
values are scaled by ``depth_scale``. If the depth image is a multi-plane image
then the ``D`` plane is used.
There are two sources of color for point colorization:
- RGB planes of the depth image, if present.
- ``rgb`` is an optional RGB image.
Camera intrinsics are required and provided by a :class:`~machinevisiontoolbox.CentralCamera` instance.
"""
from machinevisiontoolbox import Image
if camera is None:
raise ValueError("camera model must be provided")
if not isinstance(depth, Image):
raise ValueError("depth image must be an Image instance")
if depth.nplanes == 4:
rgb = depth["RGB"]
depth = depth["D"]
if rgb is None:
# no RGB, uncolored point cloud
pcd = o3d.geometry.PointCloud.create_from_depth_image(
PointCloud._image(depth),
PointCloud._intrinsics(camera),
depth_scale=depth_scale,
**kwargs,
)
else:
# colored point cloud
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
color=PointCloud._image(rgb),
depth=PointCloud._image(depth),
depth_scale=depth_scale,
**kwargs,
)
pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd_image, PointCloud._intrinsics(camera)
)
return cls(pcd)
[docs]
def write(self, filename: str) -> bool:
"""
Write point cloud to file
:param filename: name of file
:type filename: str
:return: return status
:rtype: bool
The file format is determined by the file extension.
"""
return o3d.io.write_point_cloud(filename, self._pcd)
[docs]
def disp(self, block: bool = False, file: str | None = None, **kwargs: Any) -> None:
"""
Display point cloud using Open3D
:param block: block until window dismissed, defaults to False
:type block: bool, optional
:param file: save display as an image, defaults to None
:type file: str, optional
:param front: set front vector
:type front: array_like(3)
:param lookat: set lookat vector
:type lookat: array_like(3)
:param up: set up vector
:type up: array_like(3)
:param zoom: set zoom value
:type zoom: float
Various viewing options are passed to the Open3D :class:`open3d.visualization.ViewControl`.
Typing "h" in the display window will print help information to the console.
:seealso: :class:`open3d.visualization.ViewControl`
"""
if block:
o3d.visualization.draw_geometries([self._pcd], **kwargs)
else:
# nonblocking display
vis = o3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(self._pcd)
# get object to control viewpoint
view_control = vis.get_view_control()
# handle the possible options
if "front" in kwargs:
view_control.set_front(kwargs["front"])
if "lookat" in kwargs:
view_control.set_lookat(kwargs["lookat"])
if "up" in kwargs:
view_control.set_up(kwargs["up"])
if "zoom" in kwargs:
view_control.set_zoom(kwargs["zoom"])
# update the display
vis.poll_events()
vis.update_renderer()
# save to file if requested
if file is not None:
vis.capture_screen_image(str(file), do_render=False)
[docs]
def __rmul__(self, T: SE3) -> "PointCloud":
"""
Overloaded * operator to transform points
:param T: rigid-body transform to apply to the point cloud
:type T: :class:`~spatialmath.pose3d.SE3`
:return: point cloud
:rtype: :class:`PointCloud`
Transform a :class:`PointCloud` by premultiplying by an
:class:`~spatialmath.pose3d.SE3` instance.
:seealso: :meth:`transform` :meth:`__imul__`
"""
if isinstance(T, SE3):
return self.copy().transform(T)
else:
return NotImplemented
[docs]
def __imul__(self, T: SE3) -> "PointCloud":
"""
Overloaded ``*=`` operator to transform points
:param T: rigid-body transform to apply to the point cloud
:type T: :class:`~spatialmath.pose3d.SE3`
:return: point cloud
:rtype: :class:`PointCloud`
Transform a :class:`PointCloud` by inplace multiplication by an
:class:`~spatialmath.pose3d.SE3` instance.
:seealso: :meth:`__rmul__` :meth:`transform`
"""
if isinstance(T, SE3):
self.transform(T)
return self
else:
return NotImplemented
[docs]
def __add__(self, other: "PointCloud") -> "PointCloud":
"""
Overloaded the + operator to concatenate point clouds
:param other: second point cloud
:type other: :class:`PointCloud`
:return: concatenated point clouds
:rtype: :class:`PointCloud`
"""
pcd = o3d.geometry.PointCloud()
pcd.points.extend(self.points.T)
pcd.points.extend(other.points.T)
pcd.colors.extend(self.colors.T)
pcd.colors.extend(other.colors.T)
return self.__class__(pcd)
[docs]
def downsample_voxel(self, voxel_size: float) -> "PointCloud":
"""
Downsample point cloud by voxelization
:param voxel_size: voxel dimension
:type voxel_size: float
:return: downsampled point cloud
:rtype: :class:`PointCloud`
Point cloud resolution is reduced by keeping only one
point per voxel.
:seealso: :meth:`downsample_random`
"""
return self.__class__(self._pcd.voxel_down_sample(voxel_size))
# random downsample
[docs]
def downsample_random(
self, fraction: float, seed: int | None = None
) -> "PointCloud":
"""
Downsample point cloud by random selection
:param fraction: fraction of points to retain
:type fraction: float
:param seed: random number seed, defaults to None
:type seed: int, optional
:return: downsampled point cloud
:rtype: :class:`PointCloud`
Point cloud resolution is reduced by randomly selecting
a subset of points.
:seealso: :meth:`downsample_random`
"""
if seed is None:
return self.__class__(self._pcd.random_down_sample(fraction))
if seed >= 0:
np.random.seed(seed)
n = len(self)
ind = np.random.choice(n, int(fraction * n), replace=False)
return self.__class__(self._pcd.select_by_index(ind))
def estimate_normals(self, **kwargs: Any) -> None:
"""
Estimate point normals
Normals are computed and stored within the Open3D point cloud
object. They can be:
- accessed using the ``normals`` property
- toggled on for display when the point cloud is displayed.
:seealso: :meth:`disp` :meth:`open3d.geometry.PointCloud.estimate_normals`
"""
# radius=0.1, max_nn=30
self._pcd.estimate_normals(
search_param=o3d.geometry.KDTreeSearchParamHybrid(**kwargs)
)
@property
def normals(self) -> np.ndarray:
"""
Get point normal data as array
:return: point normal
:rtype: ndarray(3,N)
Point normals are returned as columns of the array. The columns correspond
to the same columns in the :attr:`points` and :attr:`colors` properties.
:seealso: :meth:`estimate_normals` :attr:`points` :attr:`colors`
"""
return np.asarray(self._pcd.normals).T
[docs]
def remove_outlier(self, **kwargs: Any) -> "PointCloud":
"""
Remove point cloud outliers
:return: cleaned up point cloud
:rtype: :class:`PointCloud`
Remove outlying points. Any point with fewer than ``nb_points`` neighbors
within a radius of ``radius`` is removed.
.. deprecated:: 1.0.3
Use :meth:`open3d.geometry.PointCloud.remove_radius_outlier` directly.
:seealso: :meth:`open3d.geometry.PointCloud.remove_radius_outlier`
"""
warn(
"Deprecated in 1.0.3: use pc.remove_radius_outlier(radius=, nb_points=)[0] instead of remove_outlier().",
DeprecationWarning,
stacklevel=2,
)
# nb_points=16, radius=0.2
pcd, ind = self._pcd.remove_radius_outlier(**kwargs)
return self.__class__(pcd)
[docs]
def segment_plane(self, **kwargs: Any) -> tuple[Any, "PointCloud", "PointCloud"]:
# distance_threshold=0.05,
# ransac_n=3,
# num_iterations=1000
default_dict = dict(ransac_n=3, num_iterations=100)
kwargs = {**default_dict, **kwargs}
plane_model, inliers = self._pcd.segment_plane(**kwargs)
inlier_cloud = self._pcd.select_by_index(inliers)
outlier_cloud = self._pcd.select_by_index(inliers, invert=True)
return plane_model, self.__class__(inlier_cloud), self.__class__(outlier_cloud)
[docs]
def select(
self, ind: np.ndarray | Callable[..., bool], invert: bool = False
) -> "PointCloud":
"""
Select points by index
:param ind: indices or lambda function
:type ind: ndarray() or callable
:param invert: exclude points, defaults to False
:type invert: bool, optional
:return: subset of point cloud
:rtype: :class:`PointCloud`
Create point cloud from points with indices given by integer
array ``ind``. If ``invert`` is True then select those points
not given by ``ind``.
If ``ind`` is a callable function then it is applied to each column of the
3xN array of points and the point is included if the function returns True.
For example:
.. code-block:: python
pcd.select(lambda p: p[2] < 2)
will select all points with a z-coordinate less than 2.
"""
if callable(ind):
ind = np.where(np.apply_along_axis(ind, axis=0, arr=self.points))[0]
else:
ind = np.array(ind)
return self.__class__(self._pcd.select_by_index(ind, invert=invert))
[docs]
def paint(self, color: Any) -> "PointCloud":
"""
Colorize point cloud
:param color: color for points
:type color: array_like(3), str
:return: colorized point cloud
:rtype: :class:`PointCloud`
Paint all points in the point cloud this color.
"""
self._pcd.paint_uniform_color(color)
return self
[docs]
def ICP(
self,
data: "PointCloud",
T0: SE3 | None = None,
max_correspondence_distance: float = 1,
**kwargs: Any,
) -> tuple[SE3, Any]:
"""
Register point cloud using ICP
:param data: point cloud to register
:type data: :class:`PointCloud`
:param T0: initial transform, defaults to None
:type T0: :class:`~spatialmath.pose3d.SE3`, optional
:param max_correspondence_distance: distance beyond which correspondence is broken, defaults to 1
:type max_correspondence_distance: float, optional
:return: pose of ``data`` with respect to instance points
:rtype: :class:`~spatialmath.pose3d.SE3`
:seealso: :obj:`open3d.pipelines.registration.TransformationEstimationPointToPoint`
"""
estimation = o3d.pipelines.registration.TransformationEstimationPointToPoint()
# Convergence-Criteria for Vanilla ICP
criteria = o3d.pipelines.registration.ICPConvergenceCriteria(**kwargs)
if T0 is None:
T0 = np.eye(4)
else:
T0 = T0.A
status = o3d.pipelines.registration.registration_icp(
self._pcd, data._pcd, max_correspondence_distance, T0, estimation, criteria
)
# voxel_size, save_loss_log)
T = SE3(smb.trnorm(status.transformation))
return T, status
[docs]
def voxel_grid(self, voxel_size: float) -> "VoxelGrid":
"""
Voxelize point cloud
:param voxel_size: voxel dimension
:type voxel_size: float
:return: voxel grid
:rtype: :class:`VoxelGrid`
"""
return VoxelGrid(self, voxel_size)
class VoxelGrid:
def __init__(self, pcd: PointCloud, voxel_size: float) -> None:
self._voxels = o3d.geometry.VoxelGrid.create_from_point_cloud(
pcd._pcd, voxel_size=voxel_size
)
def write(self, filename: str) -> None:
"""
Write voxel grid to file
:param filename: filename
:type filename: str
:seealso: :obj:`open3d.io.write_voxel_grid`
"""
o3d.io.write_voxel_grid(filename, self._voxels)
def disp(self, block: bool = True, file: str | None = None, **kwargs: Any) -> None:
"""
Display voxel grid
:param block: block until window is dismissed, defaults to True
:type block: bool, optional
:param file: save display to this filename, defaults to None
:type file: str, optional
"""
if block:
o3d.visualization.draw_geometries([self._voxels], **kwargs)
else:
# nonblocking display
vis = o3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(self._voxels)
# get object to control viewpoint
view_control = vis.get_view_control()
# handle the possible options
if "front" in kwargs:
view_control.set_front(kwargs["front"])
if "lookat" in kwargs:
view_control.set_lookat(kwargs["lookat"])
if "up" in kwargs:
view_control.set_up(kwargs["up"])
if "zoom" in kwargs:
view_control.set_zoom(kwargs["zoom"])
render = vis.get_render_option()
render.mesh_show_wireframe = True
# update the display
vis.poll_events()
vis.update_renderer()
# save to file if requested
if file is not None:
vis.capture_screen_image(str(file), do_render=False)
if __name__ == "__main__":
from pathlib import Path
import pytest
pytest.main(
[
str(Path(__file__).parent.parent.parent / "tests" / "test_pointcloud.py"),
"-v",
]
)
