feature/cylinder

- Added method for estimating surface normals on a point cloud
- Added functions for estimating a cylinder from points and surface normals

change.txt

@@ -1,5 +1,12 @@
 YEAR-MONTH-DAY
 
+---------------------------------------------
+Date    : 2023-??
+Version : 0.27.0
+
+- Added method for estimating surface normals on a point cloud
+- Added functions for estimating a cylinder from points and surface normals
+
 ---------------------------------------------
 Date    : 2023-Nov-05
 Version : 0.26.3

main/src/georegression/fitting/cylinder/GenerateCylinderFromPointNormals_F64.java

@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) 2022, Peter Abeles. All Rights Reserved.
+ *
+ * This file is part of Geometric Regression Library (GeoRegression).
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package georegression.fitting.cylinder;
+
+import georegression.geometry.GeometryMath_F64;
+import georegression.metric.ClosestPoint3D_F64;
+import georegression.metric.Distance3D_F64;
+import georegression.struct.line.LineParametric3D_F64;
+import georegression.struct.point.PointNormal3D_F64;
+import georegression.struct.shapes.Cylinder3D_F64;
+import org.ddogleg.fitting.modelset.ModelGenerator;
+
+import java.util.List;
+
+/**
+ * Given a list of two point and surface normal pairs, first a cylinder using an analytic equation.
+ */
+public class GenerateCylinderFromPointNormals_F64 implements ModelGenerator<Cylinder3D_F64, PointNormal3D_F64> {
+
+	LineParametric3D_F64 lineA = new LineParametric3D_F64();
+	LineParametric3D_F64 lineB = new LineParametric3D_F64();
+
+	@Override public boolean generate( List<PointNormal3D_F64> dataSet, Cylinder3D_F64 output ) {
+		if (dataSet.size() == 2) {
+			return twoPointFormula(dataSet.get(0), dataSet.get(1), output);
+		}
+
+		return false;
+	}
+
+	/**
+	 * If there is no noise and the two points don't lie at the same location or have the same surface normal, then
+	 * the following equation is valid.
+	 *
+	 * @return true if no error detected
+	 */
+	public boolean twoPointFormula( PointNormal3D_F64 a, PointNormal3D_F64 b, Cylinder3D_F64 output ) {
+
+		// The closest point between the two lines defined by the surface normals and each point lies on
+		// the axis of the cylinder
+		lineA.p = a.p;
+		lineA.slope = a.n;
+		lineB.p = b.p;
+		lineB.slope = b.n;
+
+		if (null == ClosestPoint3D_F64.closestPoint(lineA, lineB, output.line.p))
+			return false;
+
+		// The cross product will point along the axis
+		GeometryMath_F64.cross(a.n, b.n, output.line.slope);
+
+		// Find the radius by averaging the two distances
+		// Typically using the average of multiple solutions is more stable and accurate.
+		double ra = Distance3D_F64.distance(output.line, a.p);
+		double rb = Distance3D_F64.distance(output.line, b.p);
+		output.radius = (ra + rb)/2.0;
+
+		// If the two points are at the same location then an infinite number of cylinders will match
+		// If the two points have the same surface normal then the radius can't be determined
+
+		return true;
+	}
+
+	@Override public int getMinimumPoints() {
+		return 2;
+	}
+}

main/src/georegression/fitting/cylinder/PointNormalDistanceFromCylinder_F64.java

@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2022, Peter Abeles. All Rights Reserved.
+ *
+ * This file is part of Geometric Regression Library (GeoRegression).
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package georegression.fitting.cylinder;
+
+import georegression.metric.Distance3D_F64;
+import georegression.struct.point.PointNormal3D_F64;
+import georegression.struct.shapes.Cylinder3D_F64;
+import org.ddogleg.fitting.modelset.DistanceFromModel;
+
+import java.util.List;
+
+/**
+ * Implementation of {@link DistanceFromModel} for {@link Cylinder3D_F64} and {@link PointNormal3D_F64}. It
+ * returns the distance the point is from the cylinder's surface. The normal vector is ignored.
+ *
+ * @author Peter Abeles
+ */
+public class PointNormalDistanceFromCylinder_F64 implements DistanceFromModel<Cylinder3D_F64, PointNormal3D_F64> {
+	Cylinder3D_F64 cylinder = new Cylinder3D_F64();
+
+	@Override
+	public void setModel( Cylinder3D_F64 plane ) {
+		this.cylinder.setTo(plane);
+	}
+
+	@Override
+	public /**/double distance( PointNormal3D_F64 point ) {
+		return Math.abs(Distance3D_F64.distance(cylinder, point.p));
+	}
+
+	@Override
+	public void distances( List<PointNormal3D_F64> list, /**/double[] errors ) {
+		for (int i = 0; i < list.size(); i++) {
+			errors[i] = Math.abs(Distance3D_F64.distance(cylinder, list.get(i).p));
+		}
+	}
+
+	@Override
+	public Class<PointNormal3D_F64> getPointType() {
+		return PointNormal3D_F64.class;
+	}
+
+	@Override
+	public Class<Cylinder3D_F64> getModelType() {
+		return Cylinder3D_F64.class;
+	}
+}

main/src/georegression/fitting/points/PointCloudToNormals_F64.java

@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2022, Peter Abeles. All Rights Reserved.
+ *
+ * This file is part of Geometric Regression Library (GeoRegression).
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package georegression.fitting.points;
+
+import georegression.fitting.plane.FitPlane3D_F64;
+import georegression.helper.KdTreePoint3D_F64;
+import georegression.struct.point.Point3D_F64;
+import georegression.struct.point.Vector3D_F64;
+import org.ddogleg.nn.FactoryNearestNeighbor;
+import org.ddogleg.nn.NearestNeighbor;
+import org.ddogleg.nn.NnData;
+import org.ddogleg.struct.DogArray;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Takes in a point cloud and returns the same point cloud with surface norms. In this implementation, we use a KDTree
+ * to find all the N local neighbors of each point. A plane is fit to those neighbors and the normal extracted from that.
+ * The sign of the normal is arbitrary as additional information is needed.
+ */
+public class PointCloudToNormals_F64 {
+	NearestNeighbor<Point3D_F64> nn = FactoryNearestNeighbor.kdtree(new KdTreePoint3D_F64());
+
+	Helper helper = new Helper();
+
+	public int numNeighbors = 3;
+
+	public void convert( List<Point3D_F64> input, DogArray<Vector3D_F64> output ) {
+		output.resize(input.size());
+		nn.setPoints(input, false);
+
+		convert(0, input.size(), input, output, helper);
+
+		removeInvalidPoints(output);
+	}
+
+	/**
+	 * Convert all the points within the specified range.
+	 */
+	protected void convert( int idx0, int idx1,
+							List<Point3D_F64> input, DogArray<Vector3D_F64> output, Helper helper) {
+
+		final NearestNeighbor.Search<Point3D_F64> search = helper.search;
+		final DogArray<NnData<Point3D_F64>> found = helper.found;
+		final FitPlane3D_F64 planeFitter = helper.planeFitter;
+		final List<Point3D_F64> inputForFitter = helper.inputForFitter;
+
+		for (int pointIdx = idx0; pointIdx < idx1; pointIdx++) {
+			// Find close by points
+			Point3D_F64 target = input.get(pointIdx);
+			search.findNearest(target, -1, numNeighbors - 1, found);
+
+			// Put into a format that the plane fitting algorithm understands
+			inputForFitter.clear();
+			for (int foundIdx = 0; foundIdx < found.size; foundIdx++) {
+				inputForFitter.add(found.get(foundIdx).point);
+			}
+
+			// Find the normal for this plane, assume the target point is on the plane
+			Vector3D_F64 normal = output.get(pointIdx);
+			if (!planeFitter.solvePoint(inputForFitter, target, normal)) {
+				// Mark it as invalid so that it's filtered later on
+				normal.x = Double.NaN;
+			}
+		}
+	}
+
+	/**
+	 * Points which could not have a normal computed are removed
+	 */
+	static void removeInvalidPoints( DogArray<Vector3D_F64> output ) {
+		for (int i = 0; i < output.size; ) {
+			Vector3D_F64 normal = output.get(i);
+			if (Double.isNaN(normal.x)) {
+				output.removeSwap(i);
+			} else {
+				i++;
+			}
+		}
+	}
+
+	/**
+	 * Contains everything that a single thread needs to search for points
+	 */
+	protected class Helper {
+		NearestNeighbor.Search<Point3D_F64> search = nn.createSearch();
+		DogArray<NnData<Point3D_F64>> found = new DogArray<>(NnData::new);
+		FitPlane3D_F64 planeFitter = new FitPlane3D_F64();
+		List<Point3D_F64> inputForFitter = new ArrayList<>();
+	}
+}

main/src/georegression/fitting/points/PointCloudToNormals_MT_F64.java

@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2022, Peter Abeles. All Rights Reserved.
+ *
+ * This file is part of Geometric Regression Library (GeoRegression).
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package georegression.fitting.points;
+
+import georegression.struct.point.Point3D_F64;
+import georegression.struct.point.Vector3D_F64;
+import org.ddogleg.DDoglegConcurrency;
+import org.ddogleg.struct.DogArray;
+import pabeles.concurrency.GrowArray;
+
+import java.util.List;
+
+/**
+ * A concurrent implementation of {@link PointCloudToNormals_F64}.
+ */
+public class PointCloudToNormals_MT_F64 extends PointCloudToNormals_F64 {
+
+	/** There needs to be at least this many points for it to use the concurrent implementation */
+	public int minimumPointsConcurrent = 200;
+
+	GrowArray<Helper> concurrentHelper = new GrowArray<>(Helper::new);
+
+	@Override
+	public void convert( List<Point3D_F64> input, DogArray<Vector3D_F64> output ) {
+		output.resize(input.size());
+		nn.setPoints(input, false);
+
+		if (input.size() < minimumPointsConcurrent) {
+			convert(0, input.size(), input, output, helper);
+		} else {
+			// Take advantage that every normal is computed independently of each other
+			DDoglegConcurrency.loopBlocks(0, input.size(), concurrentHelper, ( helper, idx0, idx1 ) -> {
+				convert(idx0, idx1, input, output, helper);
+			});
+		}
+
+		removeInvalidPoints(output);
+	}
+}

main/src/georegression/struct/point/PointNormal3D_F64.java

@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2022, Peter Abeles. All Rights Reserved.
+ *
+ * This file is part of Geometric Regression Library (GeoRegression).
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package georegression.struct.point;
+
+import org.ejml.UtilEjml;
+import org.ejml.ops.MatrixIO;
+
+import java.text.DecimalFormat;
+
+/**
+ * Point and a normal vector. Typically used to define a region on an object's surface.
+ */
+public class PointNormal3D_F64 {
+	/** Point in 3D space */
+	public Point3D_F64 p = new Point3D_F64();
+
+	/** Norm of the surface at this point */
+	public Vector3D_F64 n = new Vector3D_F64();
+
+	public PointNormal3D_F64 setTo( PointNormal3D_F64 src ) {
+		this.p.setTo(src.p);
+		this.n.setTo(src.n);
+		return this;
+	}
+
+	public PointNormal3D_F64 setTo( Point3D_F64 point, Vector3D_F64 norm ) {
+		this.p.setTo(point);
+		this.n.setTo(norm);
+		return this;
+	}
+
+	public PointNormal3D_F64 setTo( double px, double py, double pz, double nx, double ny, double nz ) {
+		this.p.setTo(px, py, pz);
+		this.n.setTo(nx, ny, nz);
+		return this;
+	}
+
+	public void zero() {
+		p.zero();
+		n.zero();
+	}
+
+	@Override
+	public String toString() {
+		DecimalFormat format = new DecimalFormat("#");
+		String sx = UtilEjml.fancyString(p.x, format, MatrixIO.DEFAULT_LENGTH, 4);
+		String sy = UtilEjml.fancyString(p.y, format, MatrixIO.DEFAULT_LENGTH, 4);
+		String sz = UtilEjml.fancyString(p.z, format, MatrixIO.DEFAULT_LENGTH, 4);
+
+		String nx = UtilEjml.fancyString(n.x, format, MatrixIO.DEFAULT_LENGTH, 4);
+		String ny = UtilEjml.fancyString(n.y, format, MatrixIO.DEFAULT_LENGTH, 4);
+		String nz = UtilEjml.fancyString(n.z, format, MatrixIO.DEFAULT_LENGTH, 4);
+
+		return getClass().getSimpleName() + "{ P(" + sx + " , " + sy + " , " + sz + " ), V( " + nx + " , " + ny + " , " + nz + ") }";
+	}
+}