===== Point Cloud Tools: Algorithms and Math =====

**Note:** This tutorial is based on the plugin "PointCloud" that you can get either through the Plugin manager (within Groimp) or by downloading the jar [[https://gitlab.com/grogra/groimp-plugins/pointcloud|here]].

Because formulas can not be displayed here efficiently, we would recommend to read the belonging practical reports for more information about the math behind the point cloud features. In case of interest, please contact the GroIMP developer team.

This overview contains the most important algorithms that have been used for the point cloud-to-object fitting features. Here, the algorithms are written in pseudocode. The java implementation of each algorithm can be found in the belonging java code (project IMP-3D, class ''de.grogra.imp3d.pointcloud.PointCloudFittingTools.java'').

In the pseudocode examples, some further functions are used:

The function ''GET_CENTER_POINT'' requires a list of 3D points and returns a new point with the average position of all given points.

The function ''GET_POINT_POINT_DISTANCE'' requires two 3D points and returns the pythagorean distance between these two points.

The function ''GET_LINE_POINT_DISTANCE'' requires a position vector of a 3D line, a direction vector of a 3D line, and a 3D point. It returns the minimum distance between the point and the line.

The function ''GET_PLANE_POINT_DISTANCE'' requires a position vector of a 3D plane, a normal vector of a 3D plane, and a 3D point. It returns the minimum distance between the point and the plane.

The function ''GET_SURFACE_POINT_DISTANCE'' requires a 3D object (a sphere, a cylinder, a frustum or a cone), and a 3D point. It returns the minimum distance between the point and the objects surface.

The function ''SET_VECTOR_LENGTH'' requires a vector and a numeric value. It multiplies all components of the vector, so that the pythagorean length of the vector is as long as the given numeric value.

The function ''LENGTH'' requires a vector and returns the pythagorean length of the vector.

The function ''CREATE_POINT'' requires three values (x, y, and z value) and returns a point object with the given coordinates.

The function ''CREATE_SPHERE'' requires a position vector and a radius. It returns a sphere object.

The function ''CREATE_CYLINDER'' requires a position vector, a direction vector, a radius, and a length. It returns a cylinder object.

The function ''CREATE_FRUSTUM'' requires a position vector, a direction vector, a base radius, a top radius, and a length. It returns a frustum object.

The function ''CREATE_CONE'' requires a position vector, a direction vector, a base radius, and a length. It returns a cone object.

The function ''SIZE'' requires an array and returns its number of elements.

The function ''SUM'' requires an array and returns the sum of all values in that array.

The function ''VOLUME'' requires a sphere, a cylinder, a frustum, or a cone and returns its volume.

The function ''AREA'' requires a sphere, a cylinder, a frustum, or a cone and returns its surface area.

The functions ''SQRT'', ''ABS'', ''SIN'' and ''COS'' require a numeric value and return the square root, the absolute value, the sinus value, and the cosinus value of the parameter.


==== Generating a Fibonacci sphere ====

A Fibonacci sphere is a list of points, distributed around a sphere surface as evenly as possible. This function is used to create such a sphere. The center position, the radius and the number of points must be provided as parameter. With more points, the sphere gets more precise and the result of the object fitting gets more accurate. On the other hand, a higher precision requires a longer calculation time.

<code java>
FUNCTION CREATE_FIBONACCI_SPHERE(center, radius, number)
	points := [number]
	phi := PI * (3 - SQRT(5))
	index := 0
	WHILE (index < number)
		y := radius - (index / (number - 1)) * 2 * radius
		temporaryRadius := SQRT(radius^2 - y^2)
		theta := phi * index
		x := COS(theta) * temporaryRadius
		z := SIN(theta) * temporaryRadius
		points[index] := CREATE_POINT(center_x + x, center_y + y, center_z + z)
		index := index + 1
	END
	RETURN points
END
</code>

==== Calculating a score to compare objects ====

The score function is one of the most important parts of the fitting algorithm. The object fitting works with creating lots of potential objects and comparing them with the original point cloud. Due to this, the quality of the result of the fitting algorithm is mostly based on this score function. To calculate a score for an object and a point cloud, the average distance of all points to the objects surface is calculated first. The distance is always handled as absolute value. Points with a large distance to the object lead to a worse score and points in the center of the object do also lead to a worse score. In practice, large point clouds can result in a nearly perfect score, if all points are near to the theoretical object surface, but most parts of the surface are not used. This phenomena can happen if a small point cloud has only points on a surface of a much larger object. To prevent this wrong result from being good, the volume and the surface area of the object are added to the score. This has the effect that objects with a small volume and a small surface area are preferred in contrast to huge ones with only some point cloud points in the middle of the surface.

<code java>
FUNCTION GET_SCORE(points[], object)
	score := 0
	FOR (point : points)
		distance := GET_SURFACE_POINT_DISTANCE(object, point)
		score := score + ABS(distance)
	END
	RETURN score / SIZE(points) + VOLUME(object) + AREA(object)
END
</code>

==== Fitting a cylinder with principal component analysis ====

The principal component analysis was the first attempt (in this practical course) to fit a cylinder to a point cloud. It expects a point cloud (or a list of points) and calculates the center position of the point cloud as well as the distance between the two most distant points.

The longest distance is used as direction vector (''firstDirection'') and principal component of the analyzed point cloud. With this direction vector, the second direction (''secondDirection'') is calculated so that it is orthogonal to the first direction vector and targets the point with the most far distance to the first direction vector. With the cross product of the first direction vector and the second direction vector, the third direction vector (''thirdDirection'') is calculated.

For all three direction vectors, a cylinder is calculated so that it directs into the respective direction. For each of the cylinders, a score is calculated. The cylinder with the least score is returned.

<code java>
FUNCTION FIT_CYLINDER_WITH_PRINCIPAL_COMPONENT_ANALYSIS(points[])
	center := GET_CENTER_POINT(points)
	mostFarPoint := NULL
	mostFarDistance := 0
	FOR (point : points)
		distance := GET_POINT_POINT_DISTANCE(point, center)
		IF (distance > mostFarDistance)
			mostFarPoint := point
			mostFarDistance := distance
		END
	END
	firstDirection := mostFarPoint - center
	orthogonalMostFarPoint := NULL
	orthogonalMostFarDistance := 0
	FOR (point : points)
		distance := GET_LINE_POINT_DISTANCE(center, firstDirection, point)
		IF (distance > orthogonalMostFarDistance)
			orthogonalMostFarPoint := point
			orthogonalMostFarDistance := distance
		END
	END
	diagonalVector := orthogonalMostFarPoint - center
	diagonalDistance := LENGTH(diagonalVector)
	distanceOnLine := SQRT(diagonalDistance^2 - orthogonalMostFarDistance^2)
	negative := firstDirection * diagonalVector
	SET_VECTOR_LENGTH(newVector, (negative < 0 ? -1 : 1) * distanceOnLine)
	footPoint := center + newVector
	secondDirection := orthogonalMostFarPoint - footPoint
	thirdDirection := firstDirection X secondDirection // X is the cross product
	thirdMostFarDistance := 0
	FOR (point : points)
		distance := GET_PLANE_POINT_DISTANCE(center, thirdDirection, point)
		IF (distance > thirdMostFarDistance)
			thirdMostFarDistance := distance
		END
	END
	SET_VECTOR_LENGTH(thirdDirection, thirdMostFarDistance)
	xLength := LENGTH(firstDirection)
	yLength := LENGTH(secondDirection)
	zLength := LENGTH(thirdDirection)
	xPosition := center - firstDirection
	yPosition := center - secondDirection
	zPosition := center - thirdDirection
	xAverage := (yLength + zLength) / 2
	yAverage := (xLength + zLength) / 2
	zAverage := (xLength + yLength) / 2
	xCylinder := CREATE_CYLINDER(xPosition, firstDirection, xAverage, 2 * xLength)
	yCylinder := CREATE_CYLINDER(yPosition, secondDirection, yAverage, 2 * yLength)
	zCylinder := CREATE_CYLINDER(zPosition, thirdDirection, zAverage, 2 * zLength)
	xScore := GET_SCORE(points, xCylinder)
	yScore := GET_SCORE(points, yCylinder)
	zScore := GET_SCORE(points, zCylinder)
	IF (xScore < yScore AND xScore < zScore)
		RETURN xCylinder
	ELSE IF (yScore < zScore)
		RETURN yCylinder
	ELSE
		RETURN zCylinder
	END
END
<code>

==== Fitting a cylinder with Fibonacci sphere analysis ====

The cylinder fitting algorithm with Fibonacci sphere analysis is the final solution for the object fitting feature in GroIMP. It is split into two algorithms. The main algorithm creates a Fibonacci sphere around the point cloud. The Fibonacci sphere has a specific number of points (= precision). The algorithm creates a cylinder for each point of the Fibonacci sphere and calculates the score for each cylinder. Finally, the scores are compared and the cylinder with the best score is returned.

The cylinder fitting function is later used for the frustum fitting and the cone fitting. This function provides the most basic information about the point cloud. It returns the position vector and the direction vector. Later, for the frustums, two different radii can be calculated, based on the cylinder returned by this function. The cone fitting is based on frustum fitting, but with a custom base radius and an adapted length. By reusing this algorithm for the other kinds of objects, lots of redundant implementation can be avoided.

<code java>
FUNCTION FIT_CYLINDER_WITH_FIBONACCI_SPHERE_ANALYSIS(points[], precision)
	center := GET_CENTER_POINT(points)
	mostFarPoint := NULL
	mostFarDistance := 0
	FOR (point : points)
		distance := GET_POINT_POINT_DISTANCE(point, center)
		IF (distance > mostFarDistance)
			mostFarPoint := point
			mostFarDistance := distance
		END
	END
	sphere := CREATE_FIBONACCI_SPHERE(center, mostFarDistance, precision
	score := INFINITY
	bestCylinder := NULL
	FOR (point : sphere)
		direction := point - center
		cylinder := FIT_CYLINDER_TO_POINT_WITH_GIVEN_DIRECTION(points, direction)
		cylinderScore := GET_SCORE(cylinder)
		IF (cylinderScore < score)
			score := cylinderScore
			bestCylinder := cylinder
		END
	END
	RETURN bestCylinder
END
</code>

The main function of the algorithm to fit a cylinder to a point cloud, using the Fibonacci sphere, is also using this function to create a cylinder for a given point cloud and a given direction vector. First, this function requests the center point of the point cloud and uses it for the calculation of the position vector of the returned cylinder. After that, the radius and the length for the cylinder can be calculated with the given direction vector. The returned cylinder is not the finally returned cylinder of the fitting algorithm. It is only used for the test case that tests how good the cylinder is.

<code java>
FUNCTION FIT_CYLINDER_TO_POINT_WITH_GIVEN_DIRECTION(points[], direction)
	center := GET_CENTER_POINT(points)
	length := 0
	radius := 0
	FOR (point : points)
		distanceToPlane := GET_LINE_POINT_DISTANCE(center, direction, point)
		distanceToNormal := GET_PLANE_POINT_DISTANCE(center, direction, point)
		IF (distanceToPlane < length)
			length := distanceToPlane
		END
		IF (distanceToNormal < radius)
			radius := distanceToNormal
		END
	END
	lengthVector := direction
	SET_VECTOR_LENGTH(lengthVector, length)
	position := center - lengthVector
	RETURN CREATE_CYLINDER(position, direction, radius, 2 * length)
END
</code>

==== Calculating a linear regression for frustums and cones ====

The linear regression function is used to calculate the angle of the side surface of a frustum, using a given cylinder. The idea of this algorithm is to use the positions of the points in the point cloud as data set and the already fitted cylinder as coordinate system. The direction vector of the cylinder represents the x-axis, while the base surface (in all directions around the direction vector) represents the y-axis. This has the advantage that the distance of each point in the point cloud to the direction vector of the cylinder can be calculated and used as y-value. It is then independent from the angle around the direction vector of the cylinder. The distance to the base surface is then used as x-value. This function returns an array with two values. The first value is the slope of the linear regression, the second one is the intercept.

<code java>
FUNCTION LINEAR_REGRESSION(cylinder, points[])
	position := cylinder.position
	direction := cylinder.direction
	positions := [SIZE(points)]
	values := [SIZE(points)]
	index := 0
	FOR (point : points)
		positions[index] := GET_PLANE_POINT_DISTANCE(position, direction, point)
		values[index] := GET_LINE_POINT_DISTANCE(position, direction, point)
		index := index + 1
	END
	RETURN LINEAR_REGRESSION(positions, values)
END
</code>

Internally, the linear regression is done with raw x- and y-values. They are extracted from the given cylinder and the given point cloud and provided to this function. For the linear regression, the center x-y-center position of the data set is calculated. Then, the average slope is calculated by analyzing the local slopes between each data set. Finally, the intercept can be calculated and the regression parameters can be returned. The returned value is an array that contains the slope in the first field and the intercept in the second field.

<code java>
FUNCTION LINEAR_REGRESSION(positions[], values[])
	sumX := SUM(positions)
	sumY := SUM(values)
	averageX := sumX / SIZE(positions)
	averageY := sumY / SIZE(values)
	sumX := 0
	sumY := 0
	index := 0
	number := SIZE(positions)
	WHILE (index < number)
		difference := positions[index] - averageX
		sumX := sumX + difference * (positions[index] - averageX)
		sumY := sumY + difference * (values[index] - averageY)
		index := index + 1
	END
	slope := sumY / sumX
	intercept := averageY - slope * averageX
	RETURN {slope, intercept}
END
<code>

==== Fitting spheres to point clouds ====

A sphere with a maximum radius is fitted to a point cloud in two steps. The first step is to calculate the center position of the point cloud. In the second step, the point with the maximum distance to the center position is choosed and stored. The returned sphere is then located at the center position and has a radius, specified by the maximum distance.

<code java>
FUNCTION FIT_SPHERE_MAXIMUM(points[])
	center := GET_CENTER_POINT(points)
	radius := 0
	FOR (point : points)
		distance := GET_POINT_POINT_DISTANCE(center, point)
		IF (distance > radius)
			radius := distance
		END
	END
	RETURN CREATE_SPHERE(center, radius)
END
</code>

A sphere with an average radius is also fitted to a point cloud in two steps. The first step is to calculate the center position of the point cloud. In the second step, the average distance of all points to the center position is calculated and stored. The returned sphere is then located at the center position and has a radius, specified by the average distance.

<code java>
FUNCTION FIT_SPHERE_AVERAGE(points[])
	center := GET_CENTER_POINT(points)
	sum := 0
	FOR (point : points)
		sum := sum + GET_POINT_POINT_DISTANCE(center, point)
	END
	radius := sum / SIZE(points)
	RETURN CREATE_SPHERE(center, radius)
END
</code>

==== Fitting cylinders to point clouds ====

The cylinder fitting with maximum radius is already done by the algorithm that fits a cylinder by using the Fibonacci sphere analysis. This function is only a mapping function (for completeness in this overview).

<code java>
FUNCTION FIT_CYLINDER_MAXIMUM(points[], precision)
	RETURN FIT_CYLINDER_WITH_FIBONACCI_SPHERE_ANALYSIS(points, precision)
END
</code>

The cylinder fitting with average radius is a bit more interesting. It first generates a cylinder with maximum radius. After that, it uses the direction vector of the cylinder as x-axis and the distance of each point of the point cloud to the direction vector as y-value and calculates the average radius. Finally, the radius of the found cylinder is changed to the average radius and the cylinder is returned.

<code java>
FUNCTION FIT_CYLINDER_AVERAGE(points[], precision)
	cylinder := FIT_CYLINDER_MAXIMUM(points, precision)
	position := cylinder.position
	direction := cylinder.direction
	radiusSum := 0
	FOR (point : points)
		radiusSum = radiusSum + GET_LINE_POINT_DISTANCE(position, direction, point)
	END
	radius := radiusSum / SIZE(points)
	RETURN CREATE_CYLINDER(position, direction, radius, cylinder.length)
END
</code>

==== Fitting frustums to point clouds ====

To fit a frustum with average radius is a bit more complex. The first step is to generate a cylinder. The position vector, the direction vector, and the length are then extracted from the cylinder and reused for the returned frustum. To get the base radius and the top radius, a linear regression is executed with the points of the point cloud, oriented on the direction vector of the cylinder. To get the radii, the y-values of the linear regressions are calculated for the x-values 0 (base surface) and length (top surface). The resulting y-values are then used as base radius and top radius of the returned frustum. In a second step, the frustum is flipped if the base radius is smaller than the top radius. This ensures that frustums do always have a logically deterministic direction.

<code java>
FUNCTION FIT_FRUSTUM_AVERAGE(points[], precision)
	cylinder := FIT_CYLINDER_AVERAGE(points, precision)
	regression := LINEAR_REGRESSION(cylinder, points)
	slope := regression[0]
	intercept := regression[1]
	position := cylinder.position
	direction := cylinder.direction
	length := cylinder.length
	baseRadius := slope * 0 + intercept
	topRadius := slope * length + intercept
	IF (baseRadius < topRadius)
		vector := direction
		SET_VECTOR_LENGTH(vector, length)
		position := position + vector
		direction := -1 * direction
		temporary := topRadius
		topRadius := baseRadius
		baseRadius := temporary
	END
	RETURN CREATE_FRUSTUM(position, direction, baseRadius, topRadius, length)
END
</code>

The frustum fitting with maximum radius is based on the respective fitting algorithm with average radius. This algorithm searches for the point in the point cloud with the maximum distance to the local position that would have been calculated with linear regression. By doing this, an additional value for the radius can be calculated and added to the existing radius. It is then added to the base radius and to the top radius to get a frustum with the same angle as before, but with the surface on the most-outside point.

<code java>
FUNCTION FIT_FRUSTUM_MAXIMUM(points[], precision)
	frustum := FIT_FRUSTUM_AVERAGE(points, precision)
	position := cylinder.position
	direction := cylinder.direction
	maximumDistance := 0
	FOR (point : points)
		distanceToPrincipalAxis := GET_LINE_POINT_DISTANCE(position, direction, point)
		distanceToBasePlane := GET_PLANE_POINT_DISTANCE(position, direction, point)
		distance := distanceToPrincipalAxis - (slope * distanceToBasePlane + intercept)
		IF (distance > maximumDistance)
			maximumDistance := distance
		END
	END
	frustum.baseRadius := frustum.baseRadius + maximumDistance
	frustum.topRadius := frustum.topRadius + maximumDistance
	RETURN frustum
END
</code>

==== Fitting cones to point clouds ====

The calculation of a cone with average radius is based on the calculation of a frustum with average radius. This function generates a frustum and scales the length so that the angle of the frustum side surface is kept.

<code java>
FUNCTION FIT_CONE_AVERAGE(points[], precision)
	frustum := FIT_FRUSTUM_AVERAGE(points, precision)
	ratio := frustum.baseRadius / frustum.topRadius
	additionalLength := frustum.length / (ratio - 1)
	coneLength := additionalLength + frustum.length
	radius := frustum.baseRadius
	RETURN CREATE_CONE(frustum.position, frustum.direction, radius, coneLength)
END
</code>

The calculation of a cone with maximum radius is also based on the calculation of a frustum with maximum radius. This function also generates a frustum and scales the length so that the angle of the frustum side surface is kept.

<code java>
FUNCTION FIT_CONE_MAXIMUM(points[], precision)
	frustum := FIT_FRUSTUM_MAXIMUM(points, precision)
	ratio := frustum.baseRadius / frustum.topRadius
	additionalLength := frustum.length / (ratio - 1)
	coneLength := additionalLength + frustum.length
	radius := frustum.baseRadius
	RETURN CREATE_CONE(frustum.position, frustum.direction, radius, coneLength)
END
</code>

==== Fitting automatic objects to point clouds ====

The automatic fitting function is the most powerful function in the set of point cloud fitting algorithms. It generates one object of each type (a sphere, a cylinder, a frustum, and a cone) and compares them by their score. The object with the best score is returned. This function also considers the average/maximum mode.

Two special cases have to be considered for frustums. In some cylindric or conical test point clouds, only frustums were returned during the debugging phase. This effect appears because no objects are perfect in nature.

Cylinders are not perfect. Their radius on the top end and on the bottom end are slightly different. This brings the algorithm to always return a frustum for natural cylinders. To avoid this, a frustum is only returned, if the radii differ by at least 5%. For smaller differences, a cylinder is returned.

Cones are also not perfect. The algorithm will only return a cone, if the tip is represented by a point. Due to floating point unprecision, this is nearly impossible for natural cones. To avoid this effect, a cone is returned if the top radius of the frustum has 5% of the base radius or less. If the top radius is larger, a frustum is returned.

<code java>
FUNCTION FIT_AUTOMATIC_OBJECT(points[], precision, average)
	IF (average)
		sphereObject := FIT_SPHERE_AVERAGE(points)
		cylinderObject := FIT_CYLINDER_AVERAGE(points, precision)
		frustumObject := FIT_FRUSTUM_AVERAGE(points, precision)
		coneObject := FIT_CONE_AVERAGE(points, precision)
	ELSE
		sphereObject := FIT_SPHERE_MAXIMUM(points)
		cylinderObject := FIT_CYLINDER_MAXIMUM(points, precision)
		frustumObject := FIT_FRUSTUM_MAXIMUM(points, precision)
		coneObject := FIT_CONE_MAXIMUM(points, precision)
	END
	sphere := GET_SCORE(sphereObject, points)
	cylinder := GET_SCORE(cylinderObject, points)
	frustum := GET_SCORE(frustumObject, points)
	cone := GET_SCORE(coneObject, points)
	IF (sphere < cylinder && sphere < frustum && sphere < cone)
		RETURN sphereObject
	ELSE IF (cylinder < frustum && cylinder < cone)
		RETURN cylinderObject
	ELSE IF (frustum < cone)
		IF (frustum.topRadius < frustum.baseRadius * 0.1)
			RETURN coneObject
		ELSE IF (frustum.topRadius > frustum.baseRadius * 0.9)
			RETURN cylinderObject
		ELSE
			RETURN frustumObject
		END
	ELSE
		RETURN coneObject
	END
END
</code>