feature/robust-ops

- Added ShapeFittingRobustOps
- Easier way to fit points to shapes robustly

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/plane/GeneratorPlaneFromPlane_F64.java

@@ -18,21 +18,23 @@
 
 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 PlaneNormal3D_F64} from {@link PlaneNormal3D_F64}. A bit silly, but needed when the
+ * 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<PlaneNormal3D_F64, PlaneNormal3D_F64> {
-	@Override public boolean generate( List<PlaneNormal3D_F64> dataSet, PlaneNormal3D_F64 output ) {
+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");
 
-		output.setTo(dataSet.get(0));
+		UtilPlane3D_F64.convert(dataSet.get(0), output);
 		return true;
 	}
 

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;
+	}
+}