From 9f3f80334c308697885a0a9bed5b22bd8be84cf0 Mon Sep 17 00:00:00 2001 From: Peter Abeles Date: Thu, 16 Nov 2023 14:52:49 -0800 Subject: [PATCH] feature/cylinder - Added method for estimating surface normals on a point cloud - Added functions for estimating a cylinder from points and surface normals --- change.txt | 7 ++ .../GenerateCylinderFromPointNormals_F64.java | 83 ++++++++++++++ .../PointNormalDistanceFromCylinder_F64.java | 63 ++++++++++ .../points/PointCloudToNormals_F64.java | 108 ++++++++++++++++++ .../points/PointCloudToNormals_MT_F64.java | 55 +++++++++ .../struct/point/PointNormal3D_F64.java | 72 ++++++++++++ .../struct/shapes/Cylinder3D_F64.java | 2 +- ...tGenerateCylinderFromPointNormals_F64.java | 101 ++++++++++++++++ ...stPointNormalDistanceFromCylinder_F64.java | 48 ++++++++ .../points/TestPointCloudToNormals_F64.java | 70 ++++++++++++ .../TestPointCloudToNormals_MT_F64.java | 55 +++++++++ 11 files changed, 663 insertions(+), 1 deletion(-) create mode 100644 main/src/georegression/fitting/cylinder/GenerateCylinderFromPointNormals_F64.java create mode 100644 main/src/georegression/fitting/cylinder/PointNormalDistanceFromCylinder_F64.java create mode 100644 main/src/georegression/fitting/points/PointCloudToNormals_F64.java create mode 100644 main/src/georegression/fitting/points/PointCloudToNormals_MT_F64.java create mode 100644 main/src/georegression/struct/point/PointNormal3D_F64.java create mode 100644 main/test/georegression/fitting/cylinder/TestGenerateCylinderFromPointNormals_F64.java create mode 100644 main/test/georegression/fitting/cylinder/TestPointNormalDistanceFromCylinder_F64.java create mode 100644 main/test/georegression/fitting/points/TestPointCloudToNormals_F64.java create mode 100644 main/test/georegression/fitting/points/TestPointCloudToNormals_MT_F64.java diff --git a/change.txt b/change.txt index 1bda9409..10cc84e9 100644 --- a/change.txt +++ b/change.txt @@ -1,5 +1,12 @@ YEAR-MONTH-DAY +--------------------------------------------- +Date : 2023-?? +Version : 0.27.0 + +- Added method for estimating surface normals on a point cloud +- Added functions for estimating a cylinder from points and surface normals + --------------------------------------------- Date : 2023-Nov-05 Version : 0.26.3 diff --git a/main/src/georegression/fitting/cylinder/GenerateCylinderFromPointNormals_F64.java b/main/src/georegression/fitting/cylinder/GenerateCylinderFromPointNormals_F64.java new file mode 100644 index 00000000..4171408a --- /dev/null +++ b/main/src/georegression/fitting/cylinder/GenerateCylinderFromPointNormals_F64.java @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. + * + * This file is part of Geometric Regression Library (GeoRegression). + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package georegression.fitting.cylinder; + +import georegression.geometry.GeometryMath_F64; +import georegression.metric.ClosestPoint3D_F64; +import georegression.metric.Distance3D_F64; +import georegression.struct.line.LineParametric3D_F64; +import georegression.struct.point.PointNormal3D_F64; +import georegression.struct.shapes.Cylinder3D_F64; +import org.ddogleg.fitting.modelset.ModelGenerator; + +import java.util.List; + +/** + * Given a list of two point and surface normal pairs, first a cylinder using an analytic equation. + */ +public class GenerateCylinderFromPointNormals_F64 implements ModelGenerator { + + LineParametric3D_F64 lineA = new LineParametric3D_F64(); + LineParametric3D_F64 lineB = new LineParametric3D_F64(); + + @Override public boolean generate( List dataSet, Cylinder3D_F64 output ) { + if (dataSet.size() == 2) { + return twoPointFormula(dataSet.get(0), dataSet.get(1), output); + } + + return false; + } + + /** + * If there is no noise and the two points don't lie at the same location or have the same surface normal, then + * the following equation is valid. + * + * @return true if no error detected + */ + public boolean twoPointFormula( PointNormal3D_F64 a, PointNormal3D_F64 b, Cylinder3D_F64 output ) { + + // The closest point between the two lines defined by the surface normals and each point lies on + // the axis of the cylinder + lineA.p = a.p; + lineA.slope = a.n; + lineB.p = b.p; + lineB.slope = b.n; + + if (null == ClosestPoint3D_F64.closestPoint(lineA, lineB, output.line.p)) + return false; + + // The cross product will point along the axis + GeometryMath_F64.cross(a.n, b.n, output.line.slope); + + // Find the radius by averaging the two distances + // Typically using the average of multiple solutions is more stable and accurate. + double ra = Distance3D_F64.distance(output.line, a.p); + double rb = Distance3D_F64.distance(output.line, b.p); + output.radius = (ra + rb)/2.0; + + // If the two points are at the same location then an infinite number of cylinders will match + // If the two points have the same surface normal then the radius can't be determined + + return true; + } + + @Override public int getMinimumPoints() { + return 2; + } +} diff --git a/main/src/georegression/fitting/cylinder/PointNormalDistanceFromCylinder_F64.java b/main/src/georegression/fitting/cylinder/PointNormalDistanceFromCylinder_F64.java new file mode 100644 index 00000000..fe855377 --- /dev/null +++ b/main/src/georegression/fitting/cylinder/PointNormalDistanceFromCylinder_F64.java @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. + * + * This file is part of Geometric Regression Library (GeoRegression). + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package georegression.fitting.cylinder; + +import georegression.metric.Distance3D_F64; +import georegression.struct.point.PointNormal3D_F64; +import georegression.struct.shapes.Cylinder3D_F64; +import org.ddogleg.fitting.modelset.DistanceFromModel; + +import java.util.List; + +/** + * Implementation of {@link DistanceFromModel} for {@link Cylinder3D_F64} and {@link PointNormal3D_F64}. It + * returns the distance the point is from the cylinder's surface. The normal vector is ignored. + * + * @author Peter Abeles + */ +public class PointNormalDistanceFromCylinder_F64 implements DistanceFromModel { + Cylinder3D_F64 cylinder = new Cylinder3D_F64(); + + @Override + public void setModel( Cylinder3D_F64 plane ) { + this.cylinder.setTo(plane); + } + + @Override + public /**/double distance( PointNormal3D_F64 point ) { + return Math.abs(Distance3D_F64.distance(cylinder, point.p)); + } + + @Override + public void distances( List 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; + } + + @Override + public Class getModelType() { + return Cylinder3D_F64.class; + } +} diff --git a/main/src/georegression/fitting/points/PointCloudToNormals_F64.java b/main/src/georegression/fitting/points/PointCloudToNormals_F64.java new file mode 100644 index 00000000..5320b63a --- /dev/null +++ b/main/src/georegression/fitting/points/PointCloudToNormals_F64.java @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. + * + * This file is part of Geometric Regression Library (GeoRegression). + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package georegression.fitting.points; + +import georegression.fitting.plane.FitPlane3D_F64; +import georegression.helper.KdTreePoint3D_F64; +import georegression.struct.point.Point3D_F64; +import georegression.struct.point.Vector3D_F64; +import org.ddogleg.nn.FactoryNearestNeighbor; +import org.ddogleg.nn.NearestNeighbor; +import org.ddogleg.nn.NnData; +import org.ddogleg.struct.DogArray; + +import java.util.ArrayList; +import java.util.List; + +/** + * Takes in a point cloud and returns the same point cloud with surface norms. In this implementation, we use a KDTree + * to find all the N local neighbors of each point. A plane is fit to those neighbors and the normal extracted from that. + * The sign of the normal is arbitrary as additional information is needed. + */ +public class PointCloudToNormals_F64 { + NearestNeighbor nn = FactoryNearestNeighbor.kdtree(new KdTreePoint3D_F64()); + + Helper helper = new Helper(); + + public int numNeighbors = 3; + + public void convert( List input, DogArray output ) { + output.resize(input.size()); + nn.setPoints(input, false); + + convert(0, input.size(), input, output, helper); + + removeInvalidPoints(output); + } + + /** + * Convert all the points within the specified range. + */ + protected void convert( int idx0, int idx1, + List input, DogArray output, Helper helper) { + + final NearestNeighbor.Search search = helper.search; + final DogArray> found = helper.found; + final FitPlane3D_F64 planeFitter = helper.planeFitter; + final List inputForFitter = helper.inputForFitter; + + for (int pointIdx = idx0; pointIdx < idx1; pointIdx++) { + // Find close by points + Point3D_F64 target = input.get(pointIdx); + search.findNearest(target, -1, numNeighbors - 1, found); + + // Put into a format that the plane fitting algorithm understands + inputForFitter.clear(); + for (int foundIdx = 0; foundIdx < found.size; foundIdx++) { + inputForFitter.add(found.get(foundIdx).point); + } + + // Find the normal for this plane, assume the target point is on the plane + Vector3D_F64 normal = output.get(pointIdx); + if (!planeFitter.solvePoint(inputForFitter, target, normal)) { + // Mark it as invalid so that it's filtered later on + normal.x = Double.NaN; + } + } + } + + /** + * Points which could not have a normal computed are removed + */ + static void removeInvalidPoints( DogArray output ) { + for (int i = 0; i < output.size; ) { + Vector3D_F64 normal = output.get(i); + if (Double.isNaN(normal.x)) { + output.removeSwap(i); + } else { + i++; + } + } + } + + /** + * Contains everything that a single thread needs to search for points + */ + protected class Helper { + NearestNeighbor.Search search = nn.createSearch(); + DogArray> found = new DogArray<>(NnData::new); + FitPlane3D_F64 planeFitter = new FitPlane3D_F64(); + List inputForFitter = new ArrayList<>(); + } +} diff --git a/main/src/georegression/fitting/points/PointCloudToNormals_MT_F64.java b/main/src/georegression/fitting/points/PointCloudToNormals_MT_F64.java new file mode 100644 index 00000000..17e24779 --- /dev/null +++ b/main/src/georegression/fitting/points/PointCloudToNormals_MT_F64.java @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. + * + * This file is part of Geometric Regression Library (GeoRegression). + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package georegression.fitting.points; + +import georegression.struct.point.Point3D_F64; +import georegression.struct.point.Vector3D_F64; +import org.ddogleg.DDoglegConcurrency; +import org.ddogleg.struct.DogArray; +import pabeles.concurrency.GrowArray; + +import java.util.List; + +/** + * A concurrent implementation of {@link PointCloudToNormals_F64}. + */ +public class PointCloudToNormals_MT_F64 extends PointCloudToNormals_F64 { + + /** There needs to be at least this many points for it to use the concurrent implementation */ + public int minimumPointsConcurrent = 200; + + GrowArray concurrentHelper = new GrowArray<>(Helper::new); + + @Override + public void convert( List input, DogArray output ) { + output.resize(input.size()); + nn.setPoints(input, false); + + if (input.size() < minimumPointsConcurrent) { + convert(0, input.size(), input, output, helper); + } else { + // Take advantage that every normal is computed independently of each other + DDoglegConcurrency.loopBlocks(0, input.size(), concurrentHelper, ( helper, idx0, idx1 ) -> { + convert(idx0, idx1, input, output, helper); + }); + } + + removeInvalidPoints(output); + } +} diff --git a/main/src/georegression/struct/point/PointNormal3D_F64.java b/main/src/georegression/struct/point/PointNormal3D_F64.java new file mode 100644 index 00000000..0afa7071 --- /dev/null +++ b/main/src/georegression/struct/point/PointNormal3D_F64.java @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. + * + * This file is part of Geometric Regression Library (GeoRegression). + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package georegression.struct.point; + +import org.ejml.UtilEjml; +import org.ejml.ops.MatrixIO; + +import java.text.DecimalFormat; + +/** + * Point and a normal vector. Typically used to define a region on an object's surface. + */ +public class PointNormal3D_F64 { + /** Point in 3D space */ + public Point3D_F64 p = new Point3D_F64(); + + /** Norm of the surface at this point */ + public Vector3D_F64 n = new Vector3D_F64(); + + public PointNormal3D_F64 setTo( PointNormal3D_F64 src ) { + this.p.setTo(src.p); + this.n.setTo(src.n); + return this; + } + + public PointNormal3D_F64 setTo( Point3D_F64 point, Vector3D_F64 norm ) { + this.p.setTo(point); + this.n.setTo(norm); + return this; + } + + public PointNormal3D_F64 setTo( double px, double py, double pz, double nx, double ny, double nz ) { + this.p.setTo(px, py, pz); + this.n.setTo(nx, ny, nz); + return this; + } + + public void zero() { + p.zero(); + n.zero(); + } + + @Override + public String toString() { + DecimalFormat format = new DecimalFormat("#"); + String sx = UtilEjml.fancyString(p.x, format, MatrixIO.DEFAULT_LENGTH, 4); + String sy = UtilEjml.fancyString(p.y, format, MatrixIO.DEFAULT_LENGTH, 4); + String sz = UtilEjml.fancyString(p.z, format, MatrixIO.DEFAULT_LENGTH, 4); + + String nx = UtilEjml.fancyString(n.x, format, MatrixIO.DEFAULT_LENGTH, 4); + String ny = UtilEjml.fancyString(n.y, format, MatrixIO.DEFAULT_LENGTH, 4); + String nz = UtilEjml.fancyString(n.z, format, MatrixIO.DEFAULT_LENGTH, 4); + + return getClass().getSimpleName() + "{ P(" + sx + " , " + sy + " , " + sz + " ), V( " + nx + " , " + ny + " , " + nz + ") }"; + } +} diff --git a/main/src/georegression/struct/shapes/Cylinder3D_F64.java b/main/src/georegression/struct/shapes/Cylinder3D_F64.java index 525f8029..c1f8f376 100644 --- a/main/src/georegression/struct/shapes/Cylinder3D_F64.java +++ b/main/src/georegression/struct/shapes/Cylinder3D_F64.java @@ -89,7 +89,7 @@ public Cylinder3D_F64 setTo( Cylinder3D_F64 o ) { } public void zero() { - this.line.setTo(0, 0, 0, 0, 0, 0); + this.line.zero(); this.radius = 0; } diff --git a/main/test/georegression/fitting/cylinder/TestGenerateCylinderFromPointNormals_F64.java b/main/test/georegression/fitting/cylinder/TestGenerateCylinderFromPointNormals_F64.java new file mode 100644 index 00000000..af570f1c --- /dev/null +++ b/main/test/georegression/fitting/cylinder/TestGenerateCylinderFromPointNormals_F64.java @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. + * + * This file is part of Geometric Regression Library (GeoRegression). + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package georegression.fitting.cylinder; + +import georegression.metric.Distance3D_F64; +import georegression.struct.point.PointNormal3D_F64; +import georegression.struct.point.Vector3D_F64; +import georegression.struct.shapes.Cylinder3D_F64; +import org.ejml.UtilEjml; +import org.junit.jupiter.api.Test; + +import java.util.List; +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class TestGenerateCylinderFromPointNormals_F64 { + Random rand = new Random(2345); + + /** + * Test cases with perfect noise free + */ + @Test void perfect() { + var alg = new GenerateCylinderFromPointNormals_F64(); + var found = new Cylinder3D_F64(); + + for (int i = 0; i < 10; i++) { + var cylinder = new Cylinder3D_F64(); + cylinder.radius = 0.5 + rand.nextDouble()*2; + cylinder.line.p.x = rand.nextGaussian(); + cylinder.line.p.y = rand.nextGaussian(); + cylinder.line.p.z = rand.nextGaussian(); + cylinder.line.slope.x = rand.nextGaussian(); + cylinder.line.slope.y = rand.nextGaussian(); + cylinder.line.slope.z = rand.nextGaussian(); + + // 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); + + // Sanity check + assertEquals(0.0, Distance3D_F64.distance(cylinder, p0.p), UtilEjml.TEST_F64); + assertEquals(0.0, Distance3D_F64.distance(cylinder, p1.p), UtilEjml.TEST_F64); + + assertTrue(alg.generate(List.of(p0, p1), found)); + + // Verify the found cylinder is correct by seeing if the two points lie on it + assertEquals(0.0, Distance3D_F64.distance(found, p0.p), UtilEjml.TEST_F64); + assertEquals(0.0, Distance3D_F64.distance(found, p1.p), UtilEjml.TEST_F64); + } + } + + PointNormal3D_F64 paramToPointOnCylinder( Cylinder3D_F64 cylinder, double axis, double angle ) { + var point = new PointNormal3D_F64(); + + // Put it on a point that's on the axis + point.p.x = cylinder.line.p.x + cylinder.line.slope.x*axis; + point.p.y = cylinder.line.p.y + cylinder.line.slope.y*axis; + point.p.z = cylinder.line.p.z + cylinder.line.slope.z*axis; + + // Define the axis of the new coordinate system + var axisX = new Vector3D_F64(0, 0, 1).crossWith(cylinder.line.slope); + axisX.normalize(); + var axisY = axisX.crossWith(cylinder.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*cylinder.radius; + point.p.y += point.n.y*cylinder.radius; + point.p.z += point.n.z*cylinder.radius; + + return point; + } +} \ No newline at end of file diff --git a/main/test/georegression/fitting/cylinder/TestPointNormalDistanceFromCylinder_F64.java b/main/test/georegression/fitting/cylinder/TestPointNormalDistanceFromCylinder_F64.java new file mode 100644 index 00000000..691b2853 --- /dev/null +++ b/main/test/georegression/fitting/cylinder/TestPointNormalDistanceFromCylinder_F64.java @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. + * + * This file is part of Geometric Regression Library (GeoRegression). + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package georegression.fitting.cylinder; + +import georegression.struct.point.PointNormal3D_F64; +import georegression.struct.shapes.Cylinder3D_F64; +import org.ejml.UtilEjml; +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +class TestPointNormalDistanceFromCylinder_F64 { + @Test void simple() { + var alg = new PointNormalDistanceFromCylinder_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); + + // 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); + } +} \ No newline at end of file diff --git a/main/test/georegression/fitting/points/TestPointCloudToNormals_F64.java b/main/test/georegression/fitting/points/TestPointCloudToNormals_F64.java new file mode 100644 index 00000000..36154355 --- /dev/null +++ b/main/test/georegression/fitting/points/TestPointCloudToNormals_F64.java @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. + * + * This file is part of Geometric Regression Library (GeoRegression). + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package georegression.fitting.points; + +import georegression.struct.point.Point3D_F64; +import georegression.struct.point.Vector3D_F64; +import org.ddogleg.struct.DogArray; +import org.ejml.UtilEjml; +import org.junit.jupiter.api.Test; + +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +class TestPointCloudToNormals_F64 { + private final Random rand = new Random(234); + + /** + * Given a known plane, compute the normals for all the points the plane and compare + */ + @Test void pointsOnAPlane() { + var points = new DogArray<>(Point3D_F64::new); + + for (int i = 0; i < 100; i++) { + points.grow().setTo(rand.nextGaussian(), rand.nextGaussian(), 0); + } + + var found = new DogArray<>(Vector3D_F64::new); + var alg = new PointCloudToNormals_F64(); + alg.convert(points.toList(), found); + + for (int i = 0; i < found.size; i++) { + Vector3D_F64 v = found.get(i); + // make sure z is pointed up + if (v.z < 0) + v.scale(-1); + assertEquals(0.0, v.distance(0.0, 0.0, 1.0), UtilEjml.TEST_F64); + } + } + + @Test void removeInvalidPoints() { + var vectors = new DogArray<>(Vector3D_F64::new); + + for (int i = 0; i < 100; i++) { + vectors.grow().setTo(rand.nextGaussian(), rand.nextGaussian(), rand.nextGaussian()); + } + vectors.get(10).x = Double.NaN; + vectors.get(25).x = Double.NaN; + + PointCloudToNormals_F64.removeInvalidPoints(vectors); + + assertEquals(98, vectors.size); + } +} \ No newline at end of file diff --git a/main/test/georegression/fitting/points/TestPointCloudToNormals_MT_F64.java b/main/test/georegression/fitting/points/TestPointCloudToNormals_MT_F64.java new file mode 100644 index 00000000..855344e0 --- /dev/null +++ b/main/test/georegression/fitting/points/TestPointCloudToNormals_MT_F64.java @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2022, Peter Abeles. All Rights Reserved. + * + * This file is part of Geometric Regression Library (GeoRegression). + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package georegression.fitting.points; + +import georegression.struct.point.Point3D_F64; +import georegression.struct.point.Vector3D_F64; +import org.ddogleg.struct.DogArray; +import org.junit.jupiter.api.Test; + +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class TestPointCloudToNormals_MT_F64 { + private final Random rand = new Random(234); + + @Test void compareToSingleThread() { + var points = new DogArray<>(Point3D_F64::new); + + for (int i = 0; i < 200; i++) { + points.grow().setTo(rand.nextGaussian(), rand.nextGaussian(), rand.nextGaussian()); + } + + var single = new DogArray<>(Vector3D_F64::new); + var multi = new DogArray<>(Vector3D_F64::new); + + new PointCloudToNormals_F64().convert(points.toList(), single); + + var alg = new PointCloudToNormals_MT_F64(); + alg.minimumPointsConcurrent = 0; // make sure it runs it with the threaded code + alg.convert(points.toList(), multi); + + assertEquals(single.size, multi.size); + for (int i = 0; i < single.size; i++) { + assertTrue(single.get(i).isIdentical(multi.get(i), 0.0)); + } + } +} \ No newline at end of file