public int xSideLength(HashMap<Integer, Point3d> coOrdinates) { int length=0; Set<Point3d> values = new TreeSet<Point>(new Comparator<Point3d>() { @Override public int compare(Point3d e1, Point3d e2) { return e1.getX().compareTo(e2.getX()); } }); values.addAll(coOrdinates.values()); return values.size(); }
@Override public void copyFrom(Point3d p) { this.x = p.getX(); this.y = p.getY(); this.z = p.getZ(); }
@Override public void translate(Point3d v) { this.x += v.getX(); this.y += v.getY(); this.z += v.getZ(); }
@Override public boolean equals(Object o) { if (!(o instanceof Point3d)) return false; final Point3d p = (Point3d) o; return p.getX() == this.x && p.getY() == this.y && p.getZ() == this.getZ(); }
@Override public Point3d minus(Point3d a) { return new Point3dImpl(this.x - a.getX(), this.y - a.getY(), this.z - a.getZ()); }
private RealVector buildInitialVector() { final CameraIntrinsics intrinsic = cameras.get(0).intrinsicParameters; final double[] vector = new double[7 + cameras.size() * 6]; vector[0] = intrinsic.getFocalLengthX(); vector[1] = intrinsic.getFocalLengthY(); vector[2] = intrinsic.getPrincipalPointX(); vector[3] = intrinsic.getPrincipalPointY(); vector[4] = intrinsic.getSkewFactor(); vector[5] = intrinsic.k1; vector[6] = intrinsic.k2; for (int i = 0; i < cameras.size(); i++) { final Camera e = cameras.get(i); final double[] rv = TransformUtilities.rodrigues(e.rotation); vector[i * 6 + 7] = rv[0]; vector[i * 6 + 8] = rv[1]; vector[i * 6 + 9] = rv[2]; vector[i * 6 + 10] = e.translation.getX(); vector[i * 6 + 11] = e.translation.getY(); vector[i * 6 + 12] = e.translation.getZ(); } return new ArrayRealVector(vector, false); }
vector[i * 6 + 11] = rv[1]; vector[i * 6 + 12] = rv[2]; vector[i * 6 + 13] = e.translation.getX(); vector[i * 6 + 14] = e.translation.getY(); vector[i * 6 + 15] = e.translation.getZ();
/** * Project a 3d point onto the image plane * * @param pt * the point to project in world coordinates * @return the image coordinates */ public Point2d project(Point3d pt) { final Matrix ptm = new Matrix(new double[][] { { pt.getX() }, { pt.getY() }, { pt.getZ() }, { 1 } }); final double[][] rv = rotation.getArray(); final Matrix Rt = new Matrix(new double[][] { { rv[0][0], rv[0][1], rv[0][2], translation.getX() }, { rv[1][0], rv[1][1], rv[1][2], translation.getY() }, { rv[2][0], rv[2][1], rv[2][2], translation.getZ() }, }); final Matrix ARt = intrinsicParameters.calibrationMatrix.times(Rt); final Matrix pr = ARt.times(ptm); final Point2dImpl p = new Point2dImpl(pr.get(0, 0) / pr.get(2, 0), pr.get(1, 0) / pr.get(2, 0)); return intrinsicParameters.applyDistortion(p); } }
/** * Compute the homography of this camera to the z=0 plane based on it's * parameters: H = KA, where A = [r1 r2 t] and r1 and r2 are the first and * second columns of the rotation matrix, and K is the calibration matrix. * * @return the camera's homography */ public Matrix computeHomography() { final Matrix A = rotation.copy(); A.set(0, 2, translation.getX()); A.set(1, 2, translation.getY()); A.set(2, 2, translation.getZ()); final Matrix H = intrinsicParameters.calibrationMatrix.times(A); MatrixUtils.times(H, 1.0 / H.get(2, 2)); return H; }