Point Cloud Density Calculation

PointCloudDensity.py

"""
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