Feature/robust ops

change.txt

@@ -4,8 +4,13 @@ YEAR-MONTH-DAY
 Date    : 2023-??
 Version : 0.27.0
 
+- Added RobustFittingOps to make robust fitting of shapes easier.
+  * Supports plane and cylinders for now.
 - Added method for estimating surface normals on a point cloud
-- Added functions for estimating a cylinder from points and surface normals
+- Shape fitting using points with surface normals
+  * Plane and Cylinder
+- Added ShapeFittingRobustOps
+  * Easy interface for using standard robust algorithms to fit points to shapes
 
 ---------------------------------------------
 Date    : 2023-Nov-05

main/src/georegression/fitting/ShapeFittingRobustOps.java

@@ -0,0 +1,191 @@
+/*
+ * 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;
+
+import georegression.fitting.cylinder.GenerateCylinderFromPointNormals_F64;
+import georegression.fitting.cylinder.ModelManagerCylinder3D_F64;
+import georegression.fitting.cylinder.PointNormalDistanceFromCylinder_F64;
+import georegression.fitting.plane.*;
+import georegression.struct.plane.PlaneGeneral3D_F64;
+import georegression.struct.plane.PlaneNormal3D_F64;
+import georegression.struct.point.Point3D_F64;
+import georegression.struct.shapes.Cylinder3D_F64;
+import org.ddogleg.fitting.modelset.DistanceFromModel;
+import org.ddogleg.fitting.modelset.ModelGenerator;
+import org.ddogleg.fitting.modelset.ModelManager;
+import org.ddogleg.fitting.modelset.lmeds.LeastMedianOfSquares;
+import org.ddogleg.fitting.modelset.lmeds.LeastMedianOfSquares_MT;
+import org.ddogleg.struct.Factory;
+
+import java.util.List;
+
+/**
+ * High level API for fitting points and related to shapes. Different robust fitting algorithms are applied
+ */
+public class ShapeFittingRobustOps {
+
+	/** If it's fewer than this number of points it will use single threaded versions */
+	public int thresholdConcurrent = 100;
+
+	/** If true it will always use a single threaded implementation */
+	public boolean forceSingleThread = false;
+
+	/** Random number generator used by robust fitter. Same seed is used each time it's called */
+	public long randomSeed = 0xDEADBEEFL;
+
+	/** Either the maximum number of iterations or the number of iterations, depending on implementation */
+	public int maxIterations = 0;
+
+	/** Inlier threshold used by RANSAC */
+	public double ransacThreshold = 0.0;
+
+	/** LMedS will return else if the inlier error is more than this value */
+	public double lsmedMaxAllowedError = Double.MAX_VALUE;
+
+	/** Inlier fraction for LMedS */
+	public double lsmedInlierFraction = 0.5;
+
+	/**
+	 * Configures RANSAC parameters
+	 */
+	public void configRansac( int iterations, double threshold ) {
+		if (iterations <= 0)
+			throw new IllegalArgumentException("Iterations must be more than zero");
+		if (threshold <= 0)
+			throw new IllegalArgumentException("threshold must be a positive number");
+		this.maxIterations = iterations;
+		this.ransacThreshold = threshold;
+	}
+
+	/**
+	 * Estimates a plane from points
+	 */
+	public PlaneGeneral3D_F64 ransacPlaneFromPoints( List<Point3D_F64> points ) {
+		return fitWithRansac(points, new ModelManagerPlaneGeneral3D_F64(),
+				GeneratorPlaneGeneral3D_F64::new, PointDistanceFromPlaneGeneral_F64::new);
+	}
+
+	/**
+	 * Estimates a plane from points with surface normals
+	 */
+	public PlaneGeneral3D_F64 ransacPlaneFromPointNormals( List<PlaneNormal3D_F64> points ) {
+		return fitWithRansac(points, new ModelManagerPlaneGeneral3D_F64(),
+				GeneratorPlaneFromPlane_F64::new, PointNormalDistanceFromPlaneGeneral_F64::new);
+	}
+
+	/**
+	 * Estimates a plane from points
+	 */
+	public PlaneGeneral3D_F64 lmedsPlaneFromPoints( List<Point3D_F64> points ) {
+		return fitWithLMedS(points, new ModelManagerPlaneGeneral3D_F64(),
+				GeneratorPlaneGeneral3D_F64::new, PointDistanceFromPlaneGeneral_F64::new);
+	}
+
+	/**
+	 * Estimates a plane from points with surface normals
+	 */
+	public PlaneGeneral3D_F64 lmedsPlaneFromPointNormals( List<PlaneNormal3D_F64> points ) {
+		return fitWithLMedS(points, new ModelManagerPlaneGeneral3D_F64(),
+				GeneratorPlaneFromPlane_F64::new, PointNormalDistanceFromPlaneGeneral_F64::new);
+	}
+
+	/**
+	 * Estimates a plane from points with surface normals
+	 */
+	public Cylinder3D_F64 ransacCylinderFromPointNormals( List<PlaneNormal3D_F64> points ) {
+		return fitWithRansac(points, new ModelManagerCylinder3D_F64(),
+				GenerateCylinderFromPointNormals_F64::new, PointNormalDistanceFromCylinder_F64::new);
+	}
+
+	/**
+	 * Estimates a plane from points with surface normals
+	 */
+	public Cylinder3D_F64 lmedsCylinderFromPointNormals( List<PlaneNormal3D_F64> points ) {
+		return fitWithLMedS(points, new ModelManagerCylinder3D_F64(),
+				GenerateCylinderFromPointNormals_F64::new, PointNormalDistanceFromCylinder_F64::new);
+	}
+
+	protected <Model, Point>
+	Model fitWithLMedS( List<Point> points,
+						ModelManager<Model> modelManager,
+						Factory<ModelGenerator<Model, Point>> factoryGenerator,
+						Factory<DistanceFromModel<Model, Point>> factoryDistance ) {
+		if (points.isEmpty())
+			throw new IllegalArgumentException("No points");
+
+		LeastMedianOfSquares<Model, Point> robust;
+		if (useConcurrent(points)) {
+			robust = new LeastMedianOfSquares_MT<>(randomSeed, maxIterations,
+					lsmedMaxAllowedError, lsmedInlierFraction,
+					modelManager,
+					(Class<Point>)points.get(0).getClass());
+		} else {
+			robust = new LeastMedianOfSquares<>(randomSeed, maxIterations,
+					lsmedMaxAllowedError, lsmedInlierFraction,
+					modelManager,
+					(Class<Point>)points.get(0).getClass());
+		}
+		robust.setModel(factoryGenerator, factoryDistance);
+
+		if (!robust.process(points)) {
+			throw new RuntimeException("LMedS failed");
+		}
+
+		return robust.getModelParameters();
+	}
+
+	protected <Model, Point>
+	Model fitWithRansac( List<Point> points,
+						 ModelManager<Model> modelManager,
+						 Factory<ModelGenerator<Model, Point>> factoryGenerator,
+						 Factory<DistanceFromModel<Model, Point>> factoryDistance ) {
+		if (maxIterations == 0 || ransacThreshold <= 0.0)
+			throw new IllegalArgumentException("Must configure RANSAC first");
+
+		if (points.isEmpty())
+			throw new IllegalArgumentException("No points");
+
+		LeastMedianOfSquares<Model, Point> robust;
+		if (useConcurrent(points)) {
+			robust = new LeastMedianOfSquares_MT<>(randomSeed, maxIterations,
+					lsmedMaxAllowedError, lsmedInlierFraction,
+					modelManager,
+					(Class<Point>)points.get(0).getClass());
+		} else {
+			robust = new LeastMedianOfSquares<>(randomSeed, maxIterations,
+					lsmedMaxAllowedError, lsmedInlierFraction,
+					modelManager,
+					(Class<Point>)points.get(0).getClass());
+		}
+		robust.setModel(factoryGenerator, factoryDistance);
+
+		if (!robust.process(points)) {
+			throw new RuntimeException("LMedS failed");
+		}
+
+		return robust.getModelParameters();
+	}
+
+	/**
+	 * Checks to see if it should use a concurrent or single thread algorithm
+	 */
+	protected boolean useConcurrent( List<?> inputs ) {
+		return !forceSingleThread && inputs.size() >= thresholdConcurrent;
+	}
+}

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

@@ -22,7 +22,7 @@
 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.plane.PlaneNormal3D_F64;
 import georegression.struct.shapes.Cylinder3D_F64;
 import org.ddogleg.fitting.modelset.ModelGenerator;
 
@@ -31,12 +31,12 @@
 /**
  * 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> {
+public class GenerateCylinderFromPointNormals_F64 implements ModelGenerator<Cylinder3D_F64, PlaneNormal3D_F64> {
 
 	LineParametric3D_F64 lineA = new LineParametric3D_F64();
 	LineParametric3D_F64 lineB = new LineParametric3D_F64();
 
-	@Override public boolean generate( List<PointNormal3D_F64> dataSet, Cylinder3D_F64 output ) {
+	@Override public boolean generate( List<PlaneNormal3D_F64> dataSet, Cylinder3D_F64 output ) {
 		if (dataSet.size() == 2) {
 			return twoPointFormula(dataSet.get(0), dataSet.get(1), output);
 		}
@@ -50,7 +50,7 @@ public class GenerateCylinderFromPointNormals_F64 implements ModelGenerator<Cyli
 	 *
 	 * @return true if no error detected
 	 */
-	public boolean twoPointFormula( PointNormal3D_F64 a, PointNormal3D_F64 b, Cylinder3D_F64 output ) {
+	public boolean twoPointFormula( PlaneNormal3D_F64 a, PlaneNormal3D_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

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

@@ -19,19 +19,19 @@
 package georegression.fitting.cylinder;
 
 import georegression.metric.Distance3D_F64;
-import georegression.struct.point.PointNormal3D_F64;
+import georegression.struct.plane.PlaneNormal3D_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
+ * Implementation of {@link DistanceFromModel} for {@link Cylinder3D_F64} and {@link PlaneNormal3D_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> {
+public class PointNormalDistanceFromCylinder_F64 implements DistanceFromModel<Cylinder3D_F64, PlaneNormal3D_F64> {
 	Cylinder3D_F64 cylinder = new Cylinder3D_F64();
 
 	@Override
@@ -40,20 +40,20 @@ public void setModel( Cylinder3D_F64 plane ) {
 	}
 
 	@Override
-	public /**/double distance( PointNormal3D_F64 point ) {
+	public /**/double distance( PlaneNormal3D_F64 point ) {
 		return Math.abs(Distance3D_F64.distance(cylinder, point.p));
 	}
 
 	@Override
-	public void distances( List<PointNormal3D_F64> list, /**/double[] errors ) {
+	public void distances( List<PlaneNormal3D_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;
+	public Class<PlaneNormal3D_F64> getPointType() {
+		return PlaneNormal3D_F64.class;
 	}
 
 	@Override

main/src/georegression/fitting/plane/GeneratorPlaneFromPlane_F64.java

@@ -0,0 +1,44 @@
+/*
+ * 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.plane;
+
+import georegression.geometry.UtilPlane3D_F64;
+import georegression.struct.plane.PlaneGeneral3D_F64;
+import georegression.struct.plane.PlaneNormal3D_F64;
+import org.ddogleg.fitting.modelset.ModelGenerator;
+
+import java.util.List;
+
+/**
+ * Estimates a {@link PlaneGeneral3D_F64} from {@link PlaneNormal3D_F64}. A bit silly, but needed when the
+ * surface is described using a point and their normal.
+ */
+public class GeneratorPlaneFromPlane_F64 implements ModelGenerator<PlaneGeneral3D_F64, PlaneNormal3D_F64> {
+	@Override public boolean generate( List<PlaneNormal3D_F64> dataSet, PlaneGeneral3D_F64 output ) {
+		if (dataSet.size() != 1)
+			throw new IllegalArgumentException("Must have one and only one point");
+
+		UtilPlane3D_F64.convert(dataSet.get(0), output);
+		return true;
+	}
+
+	@Override public int getMinimumPoints() {
+		return 1;
+	}
+}

main/src/georegression/fitting/plane/PointNormalDistanceFromPlaneGeneral_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.plane;
+
+import georegression.metric.Distance3D_F64;
+import georegression.struct.plane.PlaneGeneral3D_F64;
+import georegression.struct.plane.PlaneNormal3D_F64;
+import org.ddogleg.fitting.modelset.DistanceFromModel;
+
+import java.util.List;
+
+/**
+ * Implementation of {@link DistanceFromModel} for {@link PlaneGeneral3D_F64} and {@link PlaneNormal3D_F64}.
+ * The {@link PlaneNormal3D_F64} is assumed to be a point on the surface with its normal.
+ *
+ * @author Peter Abeles
+ */
+public class PointNormalDistanceFromPlaneGeneral_F64 implements DistanceFromModel<PlaneGeneral3D_F64, PlaneNormal3D_F64> {
+	PlaneGeneral3D_F64 plane = new PlaneGeneral3D_F64();
+
+	@Override
+	public void setModel( PlaneGeneral3D_F64 plane ) {
+		this.plane.setTo(plane);
+	}
+
+	@Override
+	public /**/double distance( PlaneNormal3D_F64 point ) {
+		return Math.abs(Distance3D_F64.distanceSigned(plane, point.p));
+	}
+
+	@Override
+	public void distances( List<PlaneNormal3D_F64> list, /**/double[] errors ) {
+		for (int i = 0; i < list.size(); i++) {
+			errors[i] = Math.abs(Distance3D_F64.distanceSigned(plane, list.get(i).p));
+		}
+	}
+
+	@Override
+	public Class<PlaneNormal3D_F64> getPointType() {
+		return PlaneNormal3D_F64.class;
+	}
+
+	@Override
+	public Class<PlaneGeneral3D_F64> getModelType() {
+		return PlaneGeneral3D_F64.class;
+	}
+}