Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Point Cloud Density Calculation
"""
Provides methods for calculating point cloud densities.
All methods can handle instances of PLYObject or list or ndarray instances of lists of vertices.
"""
import numpy as np
import scipy.spatial
def getRealDensityFromPlane(ply, planeParams):
"""
Calculates the point density from a given PLY plane object (or array of vertices).
For calculating the correct point density, the given plane object will be z-aligned.
For this process, the fitted plane parameters need to be specified. If the passed
object is a PLYObject instance and has a fitPlane method, you can set planeParams to
None. The plane fitting will then be done automatically.
:param ply: Instance of PLYObject or list of vertices. If passed as ndarray (or list), a (N,3) array is expected.
:param planeParams: Tuple of plane params fitted to the vertices. Set to None if the first parameter is a PLYObject instance and let the plane fitting run automatically.
:returns: Point density for the given plane. The unit is depending on the unit of the passed vertices.
"""
if planeParams is None:
fitPlaneMethod = getattr(ply, 'fitPlane', None)
if callable(fitPlaneMethod):
planeParams = fitPlaneMethod()
else:
raise ValueError('No plane parameters given and unable to fit plane to vertices')
elif len(planeParams) != 4:
raise ValueError('Invalid number of values for plane parameters')
vertices = _getVertices(ply)
zAxis = np.array([0.0, 0.0, 1.0])
planeNormal = np.array(planeParams[:3])
planeNormalLength = np.linalg.norm(planeNormal)
unitPlaneNormal = planeNormal / planeNormalLength
vec = planeNormal / planeNormalLength**2
# A = unitPlaneNormal
# B = zAxis
# Shift plane to origin
vertices += vec
# Find roation aligning the plane normal vector to Z axis
vecDot = unitPlaneNormal.dot(zAxis)
vecCross = np.cross(zAxis, unitPlaneNormal)
vecCrossNorm = np.linalg.norm(vecCross)
G = np.array([[vecDot, -1 * vecCrossNorm, 0], [vecCrossNorm, vecDot, 0], [0, 0, 1]])
u = unitPlaneNormal
v = (zAxis - vecDot * unitPlaneNormal) / np.linalg.norm(zAxis - vecDot * unitPlaneNormal)
w = vecCross
Finv = np.column_stack([u, v, w])
U = Finv.dot(G.dot(np.linalg.inv(Finv)))
# Apply rotation to vertices to align them with axis
vertices = U.dot(vertices.T).T
# Use naiv method to get density
hull = scipy.spatial.ConvexHull(vertices[:, :2])
# Note: As the hull was calculated in 2D space,
# the hull 'volume' is actually the surface area.
return vertices.shape[0] / hull.volume
def getNaivDensityFromPlane(ply, alignedAxis=2):
"""
Calculates the point density a given PLY plane object (or array of vertices) assuming the plane is nearly aligned to a given axis.
The vertices are simply projected on the coordinate plane they are nearly aligned to.
This should give a good estimation of the point density.
:param ply: Instance of PLYObject or list of vertices. If passed as ndarray (or list), a (N,3) array is expected.
:param alignedAxis: The axis the plane object is nearly perpendicular to. Coordinates for this axis will be set to 0.
:returns: Point density for the given plane. The unit is depending on the unit of the passed vertices.
"""
idx = np.array([0, 1, 2])
idx = idx[idx != alignedAxis]
v = _getVertices(ply)
v = v[:, idx]
hull = scipy.spatial.ConvexHull(v)
# Note: As the hull was calculated in 2D space,
# the hull 'volume' is actually the surface area.
return v.shape[0] / hull.volume
def getRealDensityFromObject(ply):
"""
Calculates the point density a given PLY object (or array of vertices).
:param ply: Instance of PLYObject or list of vertices. If passed as ndarray (or list), a (N,d) array is expected. N is the number of points, d is the dimensionality.
:returns: Point density for the given object. The unit is depending on the unit of the passed vertices.
"""
vertices = _getVertices(ply)
hull = scipy.spatial.ConvexHull(vertices)
# Note: As the hull was calculated in 2D space,
# the hull 'volume' is actually the surface area.
return vertices.shape[0] / hull.volume
def _getVertices(ply):
getVerticesMethod = getattr(ply, 'getVertices', None)
if callable(getVerticesMethod):
vertices = getVerticesMethod().T
elif isinstance(ply, np.ndarry):
vertices = ply
else:
vertices = np.array(ply)
if len(vertices.shape) != 2 or vertices.shape[1] != 3:
raise ValueError('Invalid number of dimensions (transpose necesary?)')
return vertices
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment