@Override public Point3d predict(Point3d p) { return p.transform(transform); }
/** * Extract the data from the optimised parameter vector and put it back into * our camera model * * @param point * the optimised parameter vector */ private void updateEstimates(RealVector point) { final CameraIntrinsics intrinsic = cameras.get(0).intrinsicParameters; intrinsic.setFocalLengthX(point.getEntry(0)); intrinsic.setFocalLengthY(point.getEntry(1)); intrinsic.setPrincipalPointX(point.getEntry(2)); intrinsic.setPrincipalPointY(point.getEntry(3)); intrinsic.setSkewFactor(point.getEntry(4)); intrinsic.k1 = point.getEntry(5); intrinsic.k2 = point.getEntry(6); for (int i = 0; i < cameras.size(); i++) { final Camera e = cameras.get(i); final double[] rv = new double[] { point.getEntry(i * 6 + 7), point.getEntry(i * 6 + 8), point.getEntry(i * 6 + 9) }; e.rotation = TransformUtilities.rodrigues(rv); e.translation.setX(point.getEntry(i * 6 + 10)); e.translation.setY(point.getEntry(i * 6 + 11)); e.translation.setZ(point.getEntry(i * 6 + 12)); } }
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(); }
/** * Extract the data from the optimised parameter vector and put it back into * our camera model * * @param point * the optimised parameter vector */ private void updateEstimates(RealVector point) { final CameraIntrinsics intrinsic = cameras.get(0).intrinsicParameters; intrinsic.setFocalLengthX(point.getEntry(0)); intrinsic.setFocalLengthY(point.getEntry(1)); intrinsic.setPrincipalPointX(point.getEntry(2)); intrinsic.setPrincipalPointY(point.getEntry(3)); intrinsic.setSkewFactor(point.getEntry(4)); intrinsic.k1 = point.getEntry(5); intrinsic.k2 = point.getEntry(6); intrinsic.k3 = point.getEntry(7); intrinsic.p1 = point.getEntry(8); intrinsic.p2 = point.getEntry(9); for (int i = 0; i < cameras.size(); i++) { final Camera e = cameras.get(i); final double[] rv = new double[] { point.getEntry(i * 6 + 10), point.getEntry(i * 6 + 11), point.getEntry(i * 6 + 12) }; e.rotation = TransformUtilities.rodrigues(rv); e.translation.setX(point.getEntry(i * 6 + 13)); e.translation.setY(point.getEntry(i * 6 + 14)); e.translation.setZ(point.getEntry(i * 6 + 15)); } }
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); }
/** * 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; }