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/cylinder/GenerateCylinderFromPointNormals_F64.java b/main/src/georegression/fitting/cylinder/GenerateCylinderFromPointNormals_F64.java index 4171408a..a6157d79 100644 --- a/main/src/georegression/fitting/cylinder/GenerateCylinderFromPointNormals_F64.java +++ b/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 { +public class GenerateCylinderFromPointNormals_F64 implements ModelGenerator { LineParametric3D_F64 lineA = new LineParametric3D_F64(); LineParametric3D_F64 lineB = new LineParametric3D_F64(); - @Override public boolean generate( List dataSet, Cylinder3D_F64 output ) { + @Override public boolean generate( List 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 { +public class PointNormalDistanceFromCylinder_F64 implements DistanceFromModel { 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 list, /**/double[] errors ) { + public void distances( List 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 getPointType() { - return PointNormal3D_F64.class; + public Class getPointType() { + return PlaneNormal3D_F64.class; } @Override diff --git a/main/src/georegression/fitting/plane/GeneratorPlaneFromPlane_F64.java b/main/src/georegression/fitting/plane/GeneratorPlaneFromPlane_F64.java new file mode 100644 index 00000000..8ff17191 --- /dev/null +++ b/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 { + @Override public boolean generate( List 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; + } +} 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/src/georegression/struct/GeoTuple2D_F64.java b/main/src/georegression/struct/GeoTuple2D_F64.java index e730780c..f6e0f8f4 100644 --- a/main/src/georegression/struct/GeoTuple2D_F64.java +++ b/main/src/georegression/struct/GeoTuple2D_F64.java @@ -232,14 +232,16 @@ public void print() { } @Override + @SuppressWarnings("EqualsGetClass") public boolean equals( Object obj ) { if (this == obj) return true; + // if (this.getClass() != obj.getClass()) return false; - var o = (GeoTuple2D_F64)obj; + var o = (GeoTuple2D_F64)obj; return Double.compare(x, o.x) == 0 && Double.compare(y, o.y) == 0; } diff --git a/main/src/georegression/struct/GeoTuple3D_F64.java b/main/src/georegression/struct/GeoTuple3D_F64.java index a86c97ed..94ab0c26 100644 --- a/main/src/georegression/struct/GeoTuple3D_F64.java +++ b/main/src/georegression/struct/GeoTuple3D_F64.java @@ -285,6 +285,7 @@ public boolean isNaN() { } @Override + @SuppressWarnings("EqualsGetClass") public boolean equals( Object obj ) { if (this == obj) return true; @@ -292,7 +293,7 @@ public boolean equals( Object obj ) { if (this.getClass() != obj.getClass()) return false; - var o = (GeoTuple3D_F64)obj; + var o = (GeoTuple3D_F64)obj; return Double.compare(x, o.x) == 0 && Double.compare(y, o.y) == 0 && Double.compare(z, o.z) == 0; } diff --git a/main/src/georegression/struct/plane/PlaneNormal3D_F64.java b/main/src/georegression/struct/plane/PlaneNormal3D_F64.java index 01d722bd..bd3d5217 100644 --- a/main/src/georegression/struct/plane/PlaneNormal3D_F64.java +++ b/main/src/georegression/struct/plane/PlaneNormal3D_F64.java @@ -21,8 +21,11 @@ import georegression.struct.point.Point3D_F64; import georegression.struct.point.Vector3D_F64; import lombok.Getter; +import org.ejml.UtilEjml; +import org.ejml.ops.MatrixIO; import java.io.Serializable; +import java.text.DecimalFormat; import java.util.Objects; /** @@ -31,11 +34,12 @@ * * @author Peter Abeles */ +@Getter public class PlaneNormal3D_F64 implements Serializable { /** An arbitrary point in the plane */ - @Getter public Point3D_F64 p = new Point3D_F64(); + public Point3D_F64 p = new Point3D_F64(); /** The plane's normal */ - @Getter public Vector3D_F64 n = new Vector3D_F64(); + public Vector3D_F64 n = new Vector3D_F64(); public PlaneNormal3D_F64( PlaneNormal3D_F64 o ) { setTo(o); @@ -82,24 +86,26 @@ public void setN( Vector3D_F64 n ) { this.n.setTo(n); } - @Override - public String toString() { - return getClass().getSimpleName() + "{" + - "p=" + p + - ", n=" + n + - '}'; + @Override public String toString() { + var 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 + ") }"; } - @Override - public boolean equals( Object o ) { + @Override public boolean equals( Object o ) { if (this == o) return true; - if (!(o instanceof PlaneNormal3D_F64)) return false; - PlaneNormal3D_F64 that = (PlaneNormal3D_F64)o; + if (!(o instanceof PlaneNormal3D_F64 that)) return false; return p.equals(that.p) && n.equals(that.n); } - @Override - public int hashCode() { + @Override public int hashCode() { return Objects.hash(p, n); } } diff --git a/main/src/georegression/struct/point/PointNormal3D_F64.java b/main/src/georegression/struct/point/PointNormal3D_F64.java deleted file mode 100644 index 0afa7071..00000000 --- a/main/src/georegression/struct/point/PointNormal3D_F64.java +++ /dev/null @@ -1,72 +0,0 @@ -/* - * 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 + ") }"; - } -} 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/cylinder/TestGenerateCylinderFromPointNormals_F64.java b/main/test/georegression/fitting/cylinder/TestGenerateCylinderFromPointNormals_F64.java index af570f1c..c203ebb3 100644 --- a/main/test/georegression/fitting/cylinder/TestGenerateCylinderFromPointNormals_F64.java +++ b/main/test/georegression/fitting/cylinder/TestGenerateCylinderFromPointNormals_F64.java @@ -19,7 +19,7 @@ package georegression.fitting.cylinder; import georegression.metric.Distance3D_F64; -import georegression.struct.point.PointNormal3D_F64; +import georegression.struct.plane.PlaneNormal3D_F64; import georegression.struct.point.Vector3D_F64; import georegression.struct.shapes.Cylinder3D_F64; import org.ejml.UtilEjml; @@ -53,8 +53,8 @@ class TestGenerateCylinderFromPointNormals_F64 { // Create two points that should be good for estimating the cylinder. They can't be close to each other. double theta = 0;// rand.nextDouble()*Math.PI*2.0; - PointNormal3D_F64 p0 = paramToPointOnCylinder(cylinder, rand.nextGaussian(), theta); - PointNormal3D_F64 p1 = paramToPointOnCylinder(cylinder, rand.nextGaussian(), theta + Math.PI/2.0); + PlaneNormal3D_F64 p0 = paramToPointOnCylinder(cylinder, rand.nextGaussian(), theta); + PlaneNormal3D_F64 p1 = paramToPointOnCylinder(cylinder, rand.nextGaussian(), theta + Math.PI/2.0); // Sanity check assertEquals(0.0, Distance3D_F64.distance(cylinder, p0.p), UtilEjml.TEST_F64); @@ -68,8 +68,8 @@ class TestGenerateCylinderFromPointNormals_F64 { } } - PointNormal3D_F64 paramToPointOnCylinder( Cylinder3D_F64 cylinder, double axis, double angle ) { - var point = new PointNormal3D_F64(); + PlaneNormal3D_F64 paramToPointOnCylinder( Cylinder3D_F64 cylinder, double axis, double angle ) { + var point = new PlaneNormal3D_F64(); // Put it on a point that's on the axis point.p.x = cylinder.line.p.x + cylinder.line.slope.x*axis; diff --git a/main/test/georegression/fitting/cylinder/TestPointNormalDistanceFromCylinder_F64.java b/main/test/georegression/fitting/cylinder/TestPointNormalDistanceFromCylinder_F64.java index 691b2853..90b3bbca 100644 --- a/main/test/georegression/fitting/cylinder/TestPointNormalDistanceFromCylinder_F64.java +++ b/main/test/georegression/fitting/cylinder/TestPointNormalDistanceFromCylinder_F64.java @@ -18,7 +18,7 @@ package georegression.fitting.cylinder; -import georegression.struct.point.PointNormal3D_F64; +import georegression.struct.plane.PlaneNormal3D_F64; import georegression.struct.shapes.Cylinder3D_F64; import org.ejml.UtilEjml; import org.junit.jupiter.api.Test; @@ -31,18 +31,18 @@ class TestPointNormalDistanceFromCylinder_F64 { alg.setModel(new Cylinder3D_F64().setTo(0, 0, 1, 0, 0, 1, 3.0)); // see if it's invariant to points along the axis - assertEquals(3.0, alg.distance(new PointNormal3D_F64().setTo(0, 0, 0,0, 0, -1)), UtilEjml.TEST_F64); - assertEquals(3.0, alg.distance(new PointNormal3D_F64().setTo(0, 0, 10,0, 0, -1)), UtilEjml.TEST_F64); - assertEquals(3.0, alg.distance(new PointNormal3D_F64().setTo(0, 0, -10,0, 0, -1)), UtilEjml.TEST_F64); - assertEquals(1, alg.distance(new PointNormal3D_F64().setTo(2, 0, 0,0, 0, -1)), UtilEjml.TEST_F64); - assertEquals(1, alg.distance(new PointNormal3D_F64().setTo(-2, 0, 10,0, 0, -1)), UtilEjml.TEST_F64); - assertEquals(1, alg.distance(new PointNormal3D_F64().setTo(0, 2, -10,0, 0, -1)), UtilEjml.TEST_F64); - assertEquals(1, alg.distance(new PointNormal3D_F64().setTo(0, 4, -10,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(3.0, alg.distance(new PlaneNormal3D_F64().setTo(0, 0, 0,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(3.0, alg.distance(new PlaneNormal3D_F64().setTo(0, 0, 10,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(3.0, alg.distance(new PlaneNormal3D_F64().setTo(0, 0, -10,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(1, alg.distance(new PlaneNormal3D_F64().setTo(2, 0, 0,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(1, alg.distance(new PlaneNormal3D_F64().setTo(-2, 0, 10,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(1, alg.distance(new PlaneNormal3D_F64().setTo(0, 2, -10,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(1, alg.distance(new PlaneNormal3D_F64().setTo(0, 4, -10,0, 0, -1)), UtilEjml.TEST_F64); // Point in another direction alg.setModel(new Cylinder3D_F64().setTo(0, 0, 1, 0, 1, 0, 3.0)); - assertEquals(1, alg.distance(new PointNormal3D_F64().setTo(2, 0, 1,0, 0, -1)), UtilEjml.TEST_F64); - assertEquals(1, alg.distance(new PointNormal3D_F64().setTo(-2, 0, 1,0, 0, -1)), UtilEjml.TEST_F64); - assertEquals(1, alg.distance(new PointNormal3D_F64().setTo(2, 10, 1,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(1, alg.distance(new PlaneNormal3D_F64().setTo(2, 0, 1,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(1, alg.distance(new PlaneNormal3D_F64().setTo(-2, 0, 1,0, 0, -1)), UtilEjml.TEST_F64); + assertEquals(1, alg.distance(new PlaneNormal3D_F64().setTo(2, 10, 1,0, 0, -1)), UtilEjml.TEST_F64); } } \ 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 new file mode 100644 index 00000000..470c9c70 --- /dev/null +++ b/main/test/georegression/fitting/plane/TestGeneratorPlaneFromPlane_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.junit.jupiter.api.Test; + +import java.util.List; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class TestGeneratorPlaneFromPlane_F64 { + @Test void simple() { + var alg = new GeneratorPlaneFromPlane_F64(); + var input = new PlaneNormal3D_F64(1,2,3,4,5,6); + var found = new PlaneGeneral3D_F64(); + alg.generate(List.of(input), found); + + 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..8ece60fb --- /dev/null +++ b/main/test/georegression/fitting/plane/TestPointNormalFromPlaneGeneral_F64.java @@ -0,0 +1,88 @@ +/* + * 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 diff --git a/main/test/georegression/struct/point/TestPoint2D_F64.java b/main/test/georegression/struct/point/TestPoint2D_F64.java index 486f7452..585b73af 100644 --- a/main/test/georegression/struct/point/TestPoint2D_F64.java +++ b/main/test/georegression/struct/point/TestPoint2D_F64.java @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011-2020, Peter Abeles. All Rights Reserved. + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. * * This file is part of Geometric Regression Library (GeoRegression). * @@ -20,7 +20,7 @@ import org.junit.jupiter.api.Test; -import static org.junit.jupiter.api.Assertions.assertNotEquals; +import static org.junit.jupiter.api.Assertions.*; /** * @author Peter Abeles @@ -33,10 +33,17 @@ public TestPoint2D_F64() { @Test void equals_vector() { - Point2D_F64 a = new Point2D_F64(); - Vector2D_F64 b = new Vector2D_F64(); + var a = new Point2D_F64(); + var b = new Vector2D_F64(); // numerically they are identical, but not the same type so this should fail assertNotEquals(a, b); } + + @Test void equals() { + var a = new Point2D_F64(1, 2); + assertFalse(a.equals(new Point2D_F64(1, 3))); + assertTrue(a.equals(new Point2D_F64(1, 2))); + assertFalse(a.equals(new Vector2D_F64(1, 2))); + } }