public void testAgainstUnweighted(int n, int k) { Point3D_F64 origin = new Point3D_F64(); Vector3D_F64 normal = new Vector3D_F64(); List<Point3D_F64> allPoints = getRandomPoints(n); List<Point3D_F64> selectedPoints = new ArrayList<Point3D_F64>(); double[] weights = new double[n]; while (selectedPoints.size() < k) { int index = rand.nextInt(allPoints.size()); Point3D_F64 point = allPoints.get(index); if (!selectedPoints.contains(point)) { selectedPoints.add(point); weights[index] = 1; } } new FitPlaneWeighted3D_F64().svd(allPoints, weights, origin, normal); PlaneGeneral3D_F64 weightedFit = UtilPlane3D_F64.convert(new PlaneNormal3D_F64(origin, normal), null); new FitPlane3D_F64().svd(selectedPoints, origin, normal); PlaneGeneral3D_F64 fit = UtilPlane3D_F64.convert(new PlaneNormal3D_F64(origin, normal), null); assert(epsilonEquals(weightedFit, fit, eps)); }
public static void main( String args[] ) { PlaneNormal3D_F64 camera0 = new PlaneNormal3D_F64(-0.2442424772296845,0.10157254619123487,1.5003267003526426,0.44682101275127867,-0.4128457418664506,0.7936683035038388); PlaneNormal3D_F64 camera1 = new PlaneNormal3D_F64(0.14894700258459206,0.13854568531017736,1.3003909885204032,-0.5255056708112398,-0.5118114040742201,0.6796270128569084); PlaneNormal3D_F64 camera2 = new PlaneNormal3D_F64(-0.1550633436315736,-0.25647845567954375,1.3157878496097721,0.12087719133464096,0.4789184317698088,0.86949746424368358); cameraToWorld(camera2.p);cameraToWorld(camera2.n); GeometryMath_F64.changeSign(camera2.getN()); PlaneNormal3D_F64 lidar0 = new PlaneNormal3D_F64(0.5247640194038867,-0.39945635873663504,0.38590603161631976, -0.6867638250614645,0.5227724591002402,-0.505039012941807); PlaneNormal3D_F64 lidar1 = new PlaneNormal3D_F64(0.6126300640898954,0.3540583053918326,0.459725832957747, 0.7260256918861299,0.41959322796541115,0.5448194359297699); PlaneNormal3D_F64 lidar2 = new PlaneNormal3D_F64(0.9267633912726818,-0.2607428654010602,-0.45680965716854194, -0.8696916526697748,0.24468585581600652,0.4286785056894092); GeometryMath_F64.changeSign(lidar1.getN()); GeometryMath_F64.changeSign(lidar2.getN());
public static List<PlaneGeneral3D_F64> fit(List<PlaneGeneral3D_F64> planes, List<Point3D_F64> allPoints, ScoringFunction<PlaneGeneral3D_F64, Point3D_F64> scorer, int iterations) { FitPlaneWeighted3D_F64 fitter = new FitPlaneWeighted3D_F64(); Point3D_F64 fitOrigin = new Point3D_F64(); Vector3D_F64 fitNormal = new Vector3D_F64(); double[][] weights = new double[planes.size()][allPoints.size()]; for (; iterations > 0; iterations--) { getWeights(weights, planes, allPoints, scorer); planes.clear(); for (int i = 0; i < weights.length; i++) { fitter.svd(allPoints, weights[i], fitOrigin, fitNormal); planes.add(UtilPlane3D_F64.convert(new PlaneNormal3D_F64(fitOrigin, fitNormal), null)); } } return planes; }
/** * Converts a generalized plane equation into a {@link PlaneNormal3D_F64}. The point on the plane is the mean of the * provided points. Normal is normalized to 1. * * @param original (Input) Generalized plane equation * @param points (Input) List of points on the plane * @param output (Output) Converted normal. */ public static PlaneNormal3D_F64 convert( PlaneGeneral3D_F64 original , List<Point3D_F64> points , PlaneNormal3D_F64 output ) { if (output == null) output = new PlaneNormal3D_F64(); UtilPoint3D_F64.mean(points,output.p); ClosestPoint3D_F64.closestPoint(original,output.p,output.p); output.n.set( original.A, original.B, original.C ); output.n.normalize(); return output; }
PlaneNormal3D_F64 plane = new PlaneNormal3D_F64(); Se3_F64 targetToCamera = detectTarget(copy,intrinsic,gui); if( targetToCamera == null ) {
PlaneNormal3D_F64 plane = new PlaneNormal3D_F64(); Se3_F64 targetToCamera = detectTarget(copy,intrinsic,gui); if( targetToCamera == null ) {