From 8b71e79a88a3b6c4e1c39da29ca3b99e618601d7 Mon Sep 17 00:00:00 2001 From: Peter Abeles Date: Fri, 17 Nov 2023 09:26:43 -0800 Subject: [PATCH] feature/robust-ops - Added ShapeFittingRobustOps - Easier way to fit points to shapes robustly --- change.txt | 7 +- .../fitting/ShapeFittingRobustOps.java | 191 ++++++++++++++ .../plane/GeneratorPlaneFromPlane_F64.java | 10 +- ...intNormalDistanceFromPlaneGeneral_F64.java | 63 +++++ .../geometry/UtilPoint3D_F64.java | 247 +++++++++++++----- .../fitting/TestShapeFittingRobustOps.java | 175 +++++++++++++ .../TestGeneratorPlaneFromPlane_F64.java | 19 +- .../TestPointNormalFromPlaneGeneral_F64.java | 90 +++++++ 8 files changed, 723 insertions(+), 79 deletions(-) create mode 100644 main/src/georegression/fitting/ShapeFittingRobustOps.java create mode 100644 main/src/georegression/fitting/plane/PointNormalDistanceFromPlaneGeneral_F64.java create mode 100644 main/test/georegression/fitting/TestShapeFittingRobustOps.java create mode 100644 main/test/georegression/fitting/plane/TestPointNormalFromPlaneGeneral_F64.java diff --git a/change.txt b/change.txt index 10cc84e9..214dbdcf 100644 --- a/change.txt +++ b/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 diff --git a/main/src/georegression/fitting/ShapeFittingRobustOps.java b/main/src/georegression/fitting/ShapeFittingRobustOps.java new file mode 100644 index 00000000..dd8939e9 --- /dev/null +++ b/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 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 points ) { + return fitWithRansac(points, new ModelManagerPlaneGeneral3D_F64(), + GeneratorPlaneFromPlane_F64::new, PointNormalDistanceFromPlaneGeneral_F64::new); + } + + /** + * Estimates a plane from points + */ + public PlaneGeneral3D_F64 lmedsPlaneFromPoints( List 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 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 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 points ) { + return fitWithLMedS(points, new ModelManagerCylinder3D_F64(), + GenerateCylinderFromPointNormals_F64::new, PointNormalDistanceFromCylinder_F64::new); + } + + protected + Model fitWithLMedS( List points, + ModelManager modelManager, + Factory> factoryGenerator, + Factory> factoryDistance ) { + if (points.isEmpty()) + throw new IllegalArgumentException("No points"); + + LeastMedianOfSquares robust; + if (useConcurrent(points)) { + robust = new LeastMedianOfSquares_MT<>(randomSeed, maxIterations, + lsmedMaxAllowedError, lsmedInlierFraction, + modelManager, + (Class)points.get(0).getClass()); + } else { + robust = new LeastMedianOfSquares<>(randomSeed, maxIterations, + lsmedMaxAllowedError, lsmedInlierFraction, + modelManager, + (Class)points.get(0).getClass()); + } + robust.setModel(factoryGenerator, factoryDistance); + + if (!robust.process(points)) { + throw new RuntimeException("LMedS failed"); + } + + return robust.getModelParameters(); + } + + protected + Model fitWithRansac( List points, + ModelManager modelManager, + Factory> factoryGenerator, + Factory> factoryDistance ) { + if (maxIterations == 0 || ransacThreshold <= 0.0) + throw new IllegalArgumentException("Must configure RANSAC first"); + + if (points.isEmpty()) + throw new IllegalArgumentException("No points"); + + LeastMedianOfSquares robust; + if (useConcurrent(points)) { + robust = new LeastMedianOfSquares_MT<>(randomSeed, maxIterations, + lsmedMaxAllowedError, lsmedInlierFraction, + modelManager, + (Class)points.get(0).getClass()); + } else { + robust = new LeastMedianOfSquares<>(randomSeed, maxIterations, + lsmedMaxAllowedError, lsmedInlierFraction, + modelManager, + (Class)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; + } +} diff --git a/main/src/georegression/fitting/plane/GeneratorPlaneFromPlane_F64.java b/main/src/georegression/fitting/plane/GeneratorPlaneFromPlane_F64.java index a9ef79fc..8ff17191 100644 --- a/main/src/georegression/fitting/plane/GeneratorPlaneFromPlane_F64.java +++ b/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 { - @Override public boolean generate( List dataSet, PlaneNormal3D_F64 output ) { +public class GeneratorPlaneFromPlane_F64 implements ModelGenerator { + @Override public boolean generate( List 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; } diff --git a/main/src/georegression/fitting/plane/PointNormalDistanceFromPlaneGeneral_F64.java b/main/src/georegression/fitting/plane/PointNormalDistanceFromPlaneGeneral_F64.java new file mode 100644 index 00000000..2ff1b9da --- /dev/null +++ b/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 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 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 getPointType() { + return PlaneNormal3D_F64.class; + } + + @Override + public Class getModelType() { + return PlaneGeneral3D_F64.class; + } +} diff --git a/main/src/georegression/geometry/UtilPoint3D_F64.java b/main/src/georegression/geometry/UtilPoint3D_F64.java index d76858af..a783117e 100644 --- a/main/src/georegression/geometry/UtilPoint3D_F64.java +++ b/main/src/georegression/geometry/UtilPoint3D_F64.java @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021, Peter Abeles. All Rights Reserved. + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. * * This file is part of Geometric Regression Library (GeoRegression). * @@ -23,6 +23,7 @@ import georegression.struct.point.Point3D_F64; import georegression.struct.point.Vector3D_F64; import georegression.struct.shapes.Box3D_F64; +import georegression.struct.shapes.Cylinder3D_F64; import org.jetbrains.annotations.Nullable; import java.util.ArrayList; @@ -40,7 +41,7 @@ public class UtilPoint3D_F64 { * * @param tol Tolerance in Euclidean distance. */ - public static int findClosestIdx(double x, double y, double z, List pts, double tol) { + public static int findClosestIdx( double x, double y, double z, List pts, double tol ) { double bestDist = Double.MAX_VALUE; int best = -1; @@ -51,7 +52,7 @@ public static int findClosestIdx(double x, double y, double z, List best = i; } } - if (bestDist <= tol * tol) + if (bestDist <= tol*tol) return best; return -1; } @@ -67,8 +68,8 @@ public static int findClosestIdx(double x, double y, double z, List * @param z1 z-axis on Point 1 * @return Euclidean distance */ - public static double distance(double x0, double y0, double z0, - double x1, double y1, double z1) { + public static double distance( double x0, double y0, double z0, + double x1, double y1, double z1 ) { return norm(x1 - x0, y1 - y0, z1 - z0); } @@ -83,20 +84,20 @@ public static double distance(double x0, double y0, double z0, * @param z1 z-axis on Point 1 * @return Euclidean distance squared */ - public static double distanceSq(double x0, double y0, double z0, - double x1, double y1, double z1) { + public static double distanceSq( double x0, double y0, double z0, + double x1, double y1, double z1 ) { double dx = x1 - x0; double dy = y1 - y0; double dz = z1 - z0; - return dx * dx + dy * dy + dz * dz; + return dx*dx + dy*dy + dz*dz; } - public static double norm(double x, double y, double z) { - return Math.sqrt(x * x + y * y + z * z); + public static double norm( double x, double y, double z ) { + return Math.sqrt(x*x + y*y + z*z); } - public static List copy(List pts) { + public static List copy( List pts ) { List ret = new ArrayList(); for (Point3D_F64 p : pts) { @@ -106,25 +107,25 @@ public static List copy(List pts) { return ret; } - public static Point3D_F64 noiseNormal(Point3D_F64 mean, - double sigmaX, double sigmaY, double sigmaZ, - Random rand, - @Nullable Point3D_F64 output) { + public static Point3D_F64 noiseNormal( Point3D_F64 mean, + double sigmaX, double sigmaY, double sigmaZ, + Random rand, + @Nullable Point3D_F64 output ) { if (output == null) output = new Point3D_F64(); - output.x = mean.x + rand.nextGaussian() * sigmaX; - output.y = mean.y + rand.nextGaussian() * sigmaY; - output.z = mean.z + rand.nextGaussian() * sigmaZ; + output.x = mean.x + rand.nextGaussian()*sigmaX; + output.y = mean.y + rand.nextGaussian()*sigmaY; + output.z = mean.z + rand.nextGaussian()*sigmaZ; return output; } - public static void noiseNormal(List pts, double sigma, Random rand) { + public static void noiseNormal( List pts, double sigma, Random rand ) { for (Point3D_F64 p : pts) { - p.x += rand.nextGaussian() * sigma; - p.y += rand.nextGaussian() * sigma; - p.z += rand.nextGaussian() * sigma; + p.x += rand.nextGaussian()*sigma; + p.y += rand.nextGaussian()*sigma; + p.z += rand.nextGaussian()*sigma; } } @@ -133,11 +134,11 @@ public static void noiseNormal(List pts, double sigma, Random rand) * using a uniform distribution. * * @param plane Plane - * @param max Maximum distance from center - * @param rand random number generator + * @param max Maximum distance from center + * @param rand random number generator * @return set of points on the plane */ - public static List random(PlaneNormal3D_F64 plane, double max, int num, Random rand) { + public static List random( PlaneNormal3D_F64 plane, double max, int num, Random rand ) { List ret = new ArrayList<>(); @@ -147,13 +148,13 @@ public static List random(PlaneNormal3D_F64 plane, double max, int UtilPlane3D_F64.selectAxis2D(plane.n, axisX, axisY); for (int i = 0; i < num; i++) { - double x = 2 * max * (rand.nextDouble() - 0.5); - double y = 2 * max * (rand.nextDouble() - 0.5); + double x = 2*max*(rand.nextDouble() - 0.5); + double y = 2*max*(rand.nextDouble() - 0.5); Point3D_F64 p = new Point3D_F64(); - p.x = plane.p.x + axisX.x * x + axisY.x * y; - p.y = plane.p.y + axisX.y * x + axisY.y * y; - p.z = plane.p.z + axisX.z * x + axisY.z * y; + p.x = plane.p.x + axisX.x*x + axisY.x*y; + p.y = plane.p.y + axisX.y*x + axisY.y*y; + p.z = plane.p.z + axisX.z*x + axisY.z*y; ret.add(p); } @@ -161,16 +162,130 @@ public static List random(PlaneNormal3D_F64 plane, double max, int return ret; } - public static List random(double min, double max, int num, Random rand) { + public static List randomNormals( PlaneNormal3D_F64 plane, double max, int num, Random rand ) { + + var points = random(plane, max, num, rand); + var output = new ArrayList(); + for (int i = 0; i < points.size(); i++) { + var pn = new PlaneNormal3D_F64(); + pn.p.setTo(points.get(i)); + pn.n.setTo(plane.n); + output.add(pn); + } + + return output; + } + + public static List random( Cylinder3D_F64 shape, int radialRadius, int count, Random rand ) { + var output = new ArrayList(); + + // Give the axis vector unit length to make math easier + var axisNorm = new Vector3D_F64(); + axisNorm.setTo(shape.line.slope); + axisNorm.normalize(); + + var axisZ = new Vector3D_F64(0, 0, 1); + + for (int i = 0; i < count; i++) { + var point = new Point3D_F64(); + + // Where along the axis this point will appear + double axis = radialRadius*2*(rand.nextDouble() - 0.5); + double angle = rand.nextDouble()*2*3.14; + + // Put it on a point that's on the axis + point.x = shape.line.p.x + axisNorm.x*axis; + point.y = shape.line.p.y + axisNorm.y*axis; + point.z = shape.line.p.z + axisNorm.z*axis; + + // Define the axis of the new coordinate system + var axisX = axisZ.crossWith(shape.line.slope); + axisX.normalize(); + var axisY = axisX.crossWith(shape.line.slope); + axisY.normalize(); + + // rotate point around the cylinder + double xx = Math.cos(angle); + double yy = Math.sin(angle); + + // find the normal vector + double nx = axisX.x*xx + axisY.x*yy; + double ny = axisX.y*xx + axisY.y*yy; + double nz = axisX.z*xx + axisY.z*yy; + + // Move it out to the cylinder's surface + point.x += nx*shape.radius; + point.y += ny*shape.radius; + point.z += nz*shape.radius; + + output.add(point); + } + + return output; + } + + /** + * Points randomly along a cylinder with surface normals + */ + public static List randomNormals( Cylinder3D_F64 shape, + int radialRadius, int count, Random rand ) { + var output = new ArrayList(); + + // Give the axis vector unit length to make math easier + var axisNorm = new Vector3D_F64(); + axisNorm.setTo(shape.line.slope); + axisNorm.normalize(); + + var axisZ = new Vector3D_F64(0, 0, 1); + + for (int i = 0; i < count; i++) { + var point = new PlaneNormal3D_F64(); + + // Where along the axis this point will appear + double axis = radialRadius*2*(rand.nextDouble() - 0.5); + double angle = rand.nextDouble()*2.0*3.14; + + // Put it on a point that's on the axis + point.p.x = shape.line.p.x + axisNorm.x*axis; + point.p.y = shape.line.p.y + axisNorm.y*axis; + point.p.z = shape.line.p.z + axisNorm.z*axis; + + // Define the axis of the new coordinate system + var axisX = axisZ.crossWith(shape.line.slope); + axisX.normalize(); + var axisY = axisX.crossWith(shape.line.slope); + axisY.normalize(); + + // rotate point around the cylinder + double xx = Math.cos(angle); + double yy = Math.sin(angle); + + // find the normal vector + point.n.x = axisX.x*xx + axisY.x*yy; + point.n.y = axisX.y*xx + axisY.y*yy; + point.n.z = axisX.z*xx + axisY.z*yy; + + // Move it out to the cylinder's surface + point.p.x += point.n.x*shape.radius; + point.p.y += point.n.y*shape.radius; + point.p.z += point.n.z*shape.radius; + + output.add(point); + } + + return output; + } + + public static List random( double min, double max, int num, Random rand ) { List ret = new ArrayList(); double d = max - min; for (int i = 0; i < num; i++) { Point3D_F64 p = new Point3D_F64(); - p.x = rand.nextDouble() * d + min; - p.y = rand.nextDouble() * d + min; - p.z = rand.nextDouble() * d + min; + p.x = rand.nextDouble()*d + min; + p.y = rand.nextDouble()*d + min; + p.z = rand.nextDouble()*d + min; ret.add(p); } @@ -181,18 +296,18 @@ public static List random(double min, double max, int num, Random r /** * Creates a list of random points from a uniform distribution along each axis */ - public static List random(Point3D_F64 mean, - double minX, double maxX, - double minY, double maxY, - double minZ, double maxZ, - int num, Random rand) { + public static List random( Point3D_F64 mean, + double minX, double maxX, + double minY, double maxY, + double minZ, double maxZ, + int num, Random rand ) { List ret = new ArrayList<>(); for (int i = 0; i < num; i++) { Point3D_F64 p = new Point3D_F64(); - p.x = mean.x + rand.nextDouble() * (maxX - minX) + minX; - p.y = mean.y + rand.nextDouble() * (maxY - minY) + minY; - p.z = mean.z + rand.nextDouble() * (maxZ - minZ) + minZ; + p.x = mean.x + rand.nextDouble()*(maxX - minX) + minX; + p.y = mean.y + rand.nextDouble()*(maxY - minY) + minY; + p.z = mean.z + rand.nextDouble()*(maxZ - minZ) + minZ; ret.add(p); } @@ -200,25 +315,25 @@ public static List random(Point3D_F64 mean, return ret; } - public static List random(Point3D_F64 mean, - double min, double max, - int num, Random rand) { + public static List random( Point3D_F64 mean, + double min, double max, + int num, Random rand ) { return random(mean, min, max, min, max, min, max, num, rand); } /** * Creates a list of random points from a normal distribution along each axis */ - public static List randomN(Point3D_F64 mean, - double stdX, double stdY, double stdZ, - int num, Random rand) { + public static List randomN( Point3D_F64 mean, + double stdX, double stdY, double stdZ, + int num, Random rand ) { List ret = new ArrayList<>(); for (int i = 0; i < num; i++) { Point3D_F64 p = new Point3D_F64(); - p.x = mean.x + rand.nextGaussian() * stdX; - p.y = mean.y + rand.nextGaussian() * stdY; - p.z = mean.z + rand.nextGaussian() * stdZ; + p.x = mean.x + rand.nextGaussian()*stdX; + p.y = mean.y + rand.nextGaussian()*stdY; + p.z = mean.z + rand.nextGaussian()*stdZ; ret.add(p); } @@ -226,7 +341,7 @@ public static List randomN(Point3D_F64 mean, return ret; } - public static List randomN(Point3D_F64 mean, double stdev, int num, Random rand) { + public static List randomN( Point3D_F64 mean, double stdev, int num, Random rand ) { return randomN(mean, stdev, stdev, stdev, num, rand); } @@ -234,10 +349,10 @@ public static List randomN(Point3D_F64 mean, double stdev, int num, * Computes the mean of the list of points. * * @param points List of points - * @param mean (Optional) storage for the mean. Can be null + * @param mean (Optional) storage for the mean. Can be null * @return Mean */ - public static Point3D_F64 mean(List points, @Nullable Point3D_F64 mean) { + public static Point3D_F64 mean( List points, @Nullable Point3D_F64 mean ) { if (mean == null) mean = new Point3D_F64(); @@ -249,9 +364,9 @@ public static Point3D_F64 mean(List points, @Nullable Point3D_F64 m z += p.z; } - mean.x = x / points.size(); - mean.y = y / points.size(); - mean.z = z / points.size(); + mean.x = x/points.size(); + mean.y = y/points.size(); + mean.z = z/points.size(); return mean; } @@ -260,11 +375,11 @@ public static Point3D_F64 mean(List points, @Nullable Point3D_F64 m * Computes the mean of the list of points up to element num. * * @param points List of points - * @param num use points up to num, exclusive. - * @param mean (Optional) storage for the mean. Can be null + * @param num use points up to num, exclusive. + * @param mean (Optional) storage for the mean. Can be null * @return Mean */ - public static Point3D_F64 mean(List points, int num, @Nullable Point3D_F64 mean) { + public static Point3D_F64 mean( List points, int num, @Nullable Point3D_F64 mean ) { if (mean == null) mean = new Point3D_F64(); @@ -277,9 +392,9 @@ public static Point3D_F64 mean(List points, int num, @Nullable Poin z += p.z; } - mean.x = x / num; - mean.y = y / num; - mean.z = z / num; + mean.x = x/num; + mean.y = y/num; + mean.z = z/num; return mean; } @@ -287,10 +402,10 @@ public static Point3D_F64 mean(List points, int num, @Nullable Poin /** * Finds the minimal volume {@link Box3D_F64} which contains all the points. * - * @param points Input: List of points. + * @param points Input: List of points. * @param bounding Output: Bounding box */ - public static void boundingBox(List points, Box3D_F64 bounding) { + public static void boundingBox( List points, Box3D_F64 bounding ) { double minX = Double.MAX_VALUE, maxX = -Double.MAX_VALUE; double minY = Double.MAX_VALUE, maxY = -Double.MAX_VALUE; double minZ = Double.MAX_VALUE, maxZ = -Double.MAX_VALUE; @@ -318,7 +433,7 @@ public static void boundingBox(List points, Box3D_F64 bounding) { /** * Returns the axis with the largest absolute value */ - public static int axisLargestAbs(GeoTuple3D_F64 p) { + public static int axisLargestAbs( GeoTuple3D_F64 p ) { double x = Math.abs(p.x); double y = Math.abs(p.y); double z = Math.abs(p.z); diff --git a/main/test/georegression/fitting/TestShapeFittingRobustOps.java b/main/test/georegression/fitting/TestShapeFittingRobustOps.java new file mode 100644 index 00000000..c3219cd6 --- /dev/null +++ b/main/test/georegression/fitting/TestShapeFittingRobustOps.java @@ -0,0 +1,175 @@ +/* + * 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.geometry.UtilPlane3D_F64; +import georegression.geometry.UtilPoint3D_F64; +import georegression.metric.Distance3D_F64; +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.junit.jupiter.api.Test; + +import java.util.List; +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +class TestShapeFittingRobustOps { + Random rand = new Random(0xDEADBEEFL); + + @Test void ransac_plane_points() { + var plane = new PlaneNormal3D_F64(1, 1, 2, 0, 0, 1); + + List points = UtilPoint3D_F64.random(plane, 2, 100, rand); + + // add one outlier + points.get(12).setTo(200, -2394, 93213); + + var alg = new ShapeFittingRobustOps(); + alg.configRansac(100, 0.1); + PlaneGeneral3D_F64 found = alg.ransacPlaneFromPoints(points); + + // See if the points lie on the found plane, except for the outlier + int count = 0; + for (int i = 0; i < points.size(); i++) { + if (Math.abs(UtilPlane3D_F64.evaluate(found, points.get(i))) < 1) { + count++; + } + } + + assertEquals(points.size() - 1, count); + } + + @Test void ransac_plane_normals() { + var plane = new PlaneNormal3D_F64(1, 1, 2, 0, 0, 1); + + List points = UtilPoint3D_F64.randomNormals(plane, 2, 100, rand); + + // add one outlier + points.get(12).p.setTo(200, -2394, 93213); + + var alg = new ShapeFittingRobustOps(); + alg.configRansac(100, 0.1); + PlaneGeneral3D_F64 found = alg.ransacPlaneFromPointNormals(points); + + // See if the points lie on the found plane, except for the outlier + int count = 0; + for (int i = 0; i < points.size(); i++) { + if (Math.abs(UtilPlane3D_F64.evaluate(found, points.get(i).p)) < 1) { + count++; + } + } + + assertEquals(points.size() - 1, count); + } + + @Test void lmeds_plane_points() { + var plane = new PlaneNormal3D_F64(1, 1, 2, 0, 0, 1); + + List points = UtilPoint3D_F64.random(plane, 2, 100, rand); + + // add one outlier + points.get(12).setTo(200, -2394, 93213); + + var alg = new ShapeFittingRobustOps(); + alg.maxIterations = 100; + PlaneGeneral3D_F64 found = alg.lmedsPlaneFromPoints(points); + + // See if the points lie on the found plane, except for the outlier + int count = 0; + for (int i = 0; i < points.size(); i++) { + if (Math.abs(UtilPlane3D_F64.evaluate(found, points.get(i))) < 1) { + count++; + } + } + + assertEquals(points.size() - 1, count); + } + + @Test void lmeds_plane_normals() { + var plane = new PlaneNormal3D_F64(1, 1, 2, 0, 0, 1); + + List points = UtilPoint3D_F64.randomNormals(plane, 2, 100, rand); + + // add one outlier + points.get(12).p.setTo(200, -2394, 93213); + + var alg = new ShapeFittingRobustOps(); + alg.maxIterations = 100; + PlaneGeneral3D_F64 found = alg.lmedsPlaneFromPointNormals(points); + + // See if the points lie on the found plane, except for the outlier + int count = 0; + for (int i = 0; i < points.size(); i++) { + if (Math.abs(UtilPlane3D_F64.evaluate(found, points.get(i).p)) < 1) { + count++; + } + } + + assertEquals(points.size() - 1, count); + } + + @Test void ransac_cylinder_normals() { + var shape = new Cylinder3D_F64(1, 1, 2, -0.25, 0, 1, 0.2); + + List points = UtilPoint3D_F64.randomNormals(shape, 2, 100, rand); + + // add one outlier + points.get(12).p.setTo(200, -2394, 93213); + + var alg = new ShapeFittingRobustOps(); + alg.configRansac(100, 0.1); + Cylinder3D_F64 found = alg.ransacCylinderFromPointNormals(points); + + // See if the points lie on the found plane, except for the outlier + int count = 0; + for (int i = 0; i < points.size(); i++) { + if (Math.abs(Distance3D_F64.distance(found, points.get(i).p)) < 1) { + count++; + } + } + + assertEquals(points.size() - 1, count); + } + + @Test void lmeds_cylinder_normals() { + var shape = new Cylinder3D_F64(1, 1, 2, -0.25, 0, 1, 0.2); + + List points = UtilPoint3D_F64.randomNormals(shape, 2, 100, rand); + + // add one outlier + points.get(12).p.setTo(200, -2394, 93213); + + var alg = new ShapeFittingRobustOps(); + alg.maxIterations = 100; + Cylinder3D_F64 found = alg.lmedsCylinderFromPointNormals(points); + + // See if the points lie on the found plane, except for the outlier + int count = 0; + for (int i = 0; i < points.size(); i++) { + if (Math.abs(Distance3D_F64.distance(found, points.get(i).p)) < 1) { + count++; + } + } + + assertEquals(points.size() - 1, count); + } +} \ No newline at end of file diff --git a/main/test/georegression/fitting/plane/TestGeneratorPlaneFromPlane_F64.java b/main/test/georegression/fitting/plane/TestGeneratorPlaneFromPlane_F64.java index 92055b56..470c9c70 100644 --- a/main/test/georegression/fitting/plane/TestGeneratorPlaneFromPlane_F64.java +++ b/main/test/georegression/fitting/plane/TestGeneratorPlaneFromPlane_F64.java @@ -18,6 +18,8 @@ package georegression.fitting.plane; +import georegression.geometry.UtilPlane3D_F64; +import georegression.struct.plane.PlaneGeneral3D_F64; import georegression.struct.plane.PlaneNormal3D_F64; import org.junit.jupiter.api.Test; @@ -28,14 +30,15 @@ public class TestGeneratorPlaneFromPlane_F64 { @Test void simple() { var alg = new GeneratorPlaneFromPlane_F64(); - var found = new PlaneNormal3D_F64(); - alg.generate(List.of(new PlaneNormal3D_F64(1,2,3,4,5,6)), found); + var input = new PlaneNormal3D_F64(1,2,3,4,5,6); + var found = new PlaneGeneral3D_F64(); + alg.generate(List.of(input), found); - assertEquals(1.0, found.p.x); - assertEquals(2.0, found.p.y); - assertEquals(3.0, found.p.z); - assertEquals(4.0, found.n.x); - assertEquals(5.0, found.n.y); - assertEquals(6.0, found.n.z); + PlaneGeneral3D_F64 expected = UtilPlane3D_F64.convert(input, null); + + assertEquals(expected.A, found.A); + assertEquals(expected.B, found.B); + assertEquals(expected.C, found.C); + assertEquals(expected.D, found.D); } } diff --git a/main/test/georegression/fitting/plane/TestPointNormalFromPlaneGeneral_F64.java b/main/test/georegression/fitting/plane/TestPointNormalFromPlaneGeneral_F64.java new file mode 100644 index 00000000..de0fba27 --- /dev/null +++ b/main/test/georegression/fitting/plane/TestPointNormalFromPlaneGeneral_F64.java @@ -0,0 +1,90 @@ +/* + * 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.ejml.UtilEjml; +import org.junit.jupiter.api.Test; + +import java.util.ArrayList; +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertSame; + +class TestPointNormalFromPlaneGeneral_F64 { + Random rand = new Random(234); + PlaneNormal3D_F64 normal = new PlaneNormal3D_F64(); + PlaneGeneral3D_F64 general = new PlaneGeneral3D_F64(); + + @Test void distance() { + normal.p.setTo(20, 10, 2); + normal.n.setTo(0, 0, 1); + UtilPlane3D_F64.convert(normal, general); + + var alg = new PointNormalDistanceFromPlaneGeneral_F64(); + alg.setModel(general); + assertEquals(0.0, alg.distance(createPoint(20.0, 10.0, 2.0)), UtilEjml.TEST_F64); + assertEquals(0.0, alg.distance(createPoint(0.0, 0.0, 2.0)), UtilEjml.TEST_F64); + assertEquals(0.0, alg.distance(createPoint(-0.5, 4.5, 2.0)), UtilEjml.TEST_F64); + + assertEquals(1.0, alg.distance(createPoint(0.0, 0.0, 3.0)), UtilEjml.TEST_F64); + assertEquals(1.0, alg.distance(createPoint(0.0, 0.0, 1.0)), UtilEjml.TEST_F64); + } + + private PlaneNormal3D_F64 createPoint( double x, double y, double z ) { + var pn = new PlaneNormal3D_F64(); + pn.p.setTo(x, y, z); + pn.n.setTo(rand.nextGaussian(), rand.nextGaussian(), rand.nextGaussian()); + return pn; + } + + @Test void distances() { + normal.p.setTo(20, 10, 2); + normal.n.setTo(0, 0, 1); + UtilPlane3D_F64.convert(normal, general); + + var alg = new PointNormalDistanceFromPlaneGeneral_F64(); + alg.setModel(general); + + var points = new ArrayList(); + for (int i = 0; i < 20; i++) { + points.add(createPoint(rand.nextGaussian(), rand.nextGaussian(), rand.nextGaussian())); + } + + /**/ + double[] errors = new /**/double[points.size()]; + alg.distances(points, errors); + + for (int i = 0; i < points.size(); i++) { + /**/ + double expected = alg.distance(points.get(i)); + assertEquals(expected, errors[i]); + } + } + + @Test void types() { + var alg = new PointNormalDistanceFromPlaneGeneral_F64(); + + assertSame(PlaneNormal3D_F64.class, alg.getPointType()); + assertSame(PlaneGeneral3D_F64.class, alg.getModelType()); + } +} \ No newline at end of file