@Override public boolean generate(List<PointVectorNN> dataSet, Sphere3D_F64 output) { PointVectorNN pa = dataSet.get(0); PointVectorNN pb = dataSet.get(1); PointVectorNN pc = dataSet.get(2); lineA.p = pa.p; lineA.slope = pa.normal; lineB.p = pb.p; lineB.slope = pb.normal; // All points and their normal pass through the sphere's center ClosestPoint3D_F64.closestPoint(lineA, lineB, output.center); double ra = output.center.distance(pa.p); double rb = output.center.distance(pb.p); double rc = output.center.distance(pc.p); // radius is set to average distance output.radius = (ra + rb) / 2.0; if (!check.valid(output)) return false; // check the solution if (Math.abs(ra - output.radius) > tolDistance) return false; if (Math.abs(rb - output.radius) > tolDistance) return false; if (Math.abs(rc - output.radius) > tolDistance) return false; return checkAngles(output, pa, pb, pc); }
/** * 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; }
ClosestPoint3D_F64.closestPoint(lineA, lineB, output.line.p);
/** * Adjusts normals of each plane so that they point outwards. Norms of each plane will be normalized to 1. The point on * each plane must not be the same for each plane. * * @param a plane * @param b plane * @param c plane */ public static void adjustBoxNormals( PlaneNormal3D_F64 a , PlaneNormal3D_F64 b , PlaneNormal3D_F64 c ) { Point3D_F64 closest = new Point3D_F64(); LineParametric3D_F64 lineA = new LineParametric3D_F64(a.p,a.n); LineParametric3D_F64 lineB = new LineParametric3D_F64(b.p,b.n); LineParametric3D_F64 lineC = new LineParametric3D_F64(c.p,c.n); ClosestPoint3D_F64.closestPoint(lineA,lineB,closest); a.n.set( a.p.x - closest.x,a.p.y - closest.y,a.p.z - closest.z); a.n.normalize(); b.n.set( b.p.x - closest.x,b.p.y - closest.y,b.p.z - closest.z); b.n.normalize(); ClosestPoint3D_F64.closestPoint(lineC,closest,closest); c.n.set( c.p.x - closest.x,c.p.y - closest.y,c.p.z - closest.z); c.n.normalize(); } }
/** * <p> * Given two observations of the same point from two views and a known motion between the * two views, triangulate the point's 3D position in camera 'a' reference frame. * </p> * * @param a Observation from camera view 'a' in normalized coordinates. Not modified. * @param b Observation from camera view 'b' in normalized coordinates. Not modified. * @param fromAtoB Transformation from camera view 'a' to 'b' Not modified. * @param foundInA (Output) Found 3D position of the point in reference frame 'a'. Modified. */ public void triangulate( Point2D_F64 a , Point2D_F64 b , Se3_F64 fromAtoB , Point3D_F64 foundInA ) { // set camera B's principle point Vector3D_F64 t = fromAtoB.getT(); rayB.p.set(-t.x, -t.y, -t.z); // rotate observation in B into camera A's view GeometryMath_F64.multTran(fromAtoB.getR(),rayB.p,rayB.p); GeometryMath_F64.multTran(fromAtoB.getR(),b,rayB.slope); rayA.slope.set(a.x,a.y,1); ClosestPoint3D_F64.closestPoint(rayA,rayB,foundInA); } }
ClosestPoint3D_F64.closestPoint(rayA,rayB,foundInA);
ClosestPoint3D_F64.closestPoint(plane, points.get(0), origin);
ClosestPoint3D_F64.closestPoint(plane,points.get(0),getCenter());