@Override public void decode(double[] param, CalibratedPoseAndPoint outputModel) { int paramIndex = 0; // first decode the transformation for (int i = 0; i < numViews; i++) { // don't decode if it is already known if (knownView[i]) continue; Se3_F64 se = outputModel.getWorldToCamera(i); rotation.setParamVector(param[paramIndex++], param[paramIndex++], param[paramIndex++]); RotationMatrixGenerator.rodriguesToMatrix(rotation, se.getR()); Vector3D_F64 T = se.getT(); T.x = param[paramIndex++]; T.y = param[paramIndex++]; T.z = param[paramIndex++]; } // now decode the points for (int i = 0; i < numPoints; i++) { Point3D_F64 p = outputModel.getPoint(i); p.x = param[paramIndex++]; p.y = param[paramIndex++]; p.z = param[paramIndex++]; } }
public static Point3D_F64 createPt(Sphere3D_F64 sphere, double phi, double theta) { Point3D_F64 p = new Point3D_F64(); p.set(0, 0, sphere.radius); Rodrigues_F64 rodX = new Rodrigues_F64(phi, new Vector3D_F64(1, 0, 0)); DenseMatrix64F rotX = ConvertRotation3D_F64.rodriguesToMatrix(rodX, null); Rodrigues_F64 rodZ = new Rodrigues_F64(theta, new Vector3D_F64(0, 0, 1)); DenseMatrix64F rotZ = ConvertRotation3D_F64.rodriguesToMatrix(rodZ, null); GeometryMath_F64.mult(rotX, p, p); GeometryMath_F64.mult(rotZ, p, p); p.x += sphere.center.x; p.y += sphere.center.y; p.z += sphere.center.z; return p; }
/** * Randomly generate points on a plane by randomly selecting two vectors on the plane using cross * products */ private List<Point3D_F64> randPointOnPlane(PlaneNormal3D_F64 plane, int N) { Vector3D_F64 v = new Vector3D_F64(-2, 0, 1); Vector3D_F64 a = UtilTrig_F64.cross(plane.n, v); a.normalize(); Vector3D_F64 b = UtilTrig_F64.cross(plane.n, a); b.normalize(); List<Point3D_F64> ret = new ArrayList<Point3D_F64>(); for (int i = 0; i < N; i++) { double v0 = rand.nextGaussian(); double v1 = rand.nextGaussian(); Point3D_F64 p = new Point3D_F64(); p.x = plane.p.x + v0 * a.x + v1 * b.x; p.y = plane.p.y + v0 * a.y + v1 * b.y; p.z = plane.p.z + v0 * a.z + v1 * b.z; ret.add(p); } return ret; }