private void constructHistograms(double c_x, double c_y, double scale, double orientation) { double c = Math.cos(orientation); double s = Math.sin(orientation); int gridRadius = gridWidth / 2; // This is the distance between samples // The size is computed by finding the width of one block in the grid, then dividing by the // number of samples along that side double sampleUnit = (2.0 * scale * sigmaToRadius) / numSamples; // how wide a grid cell is in pixels double gridCellLength = numSamples * sampleUnit; int gridCellLengthI = (int) (gridCellLength + 0.5); // round to int // System.out.println("-----------------------------------------"); // System.out.println(" cell length "+gridCellLength); // System.out.println(" sampleUnit "+sampleUnit); int allSampleIndex = 0; for (int gy = 0; gy < gridWidth; gy++) { double gridY = (gy - gridRadius) * gridCellLength; for (int gx = 0; gx < gridWidth; gx++) { // top left coordinate of grid in pixels double gridX = (gx - gridRadius) * gridCellLength; // TODO Sample all pixels here for (int sy = 0; sy < numSamples; sy++) { double y = sy * sampleUnit + gridY; for (int sx = 0; sx < numSamples; sx++, allSampleIndex++) { // Sample point in pixels in grid coordinate system double x = sx * sampleUnit + gridX; // Rotate and translate into image pixel coordinates, then round int px = (int) (x * c - y * s + c_x + 0.5); int py = (int) (x * s + y * c + c_y + 0.5); if (image.isInBounds(px, py)) { double dx = derivX.unsafe_get(px, py); double dy = derivY.unsafe_get(px, py); // Gaussian weighting applied to whole sample area double w = gridWeights[allSampleIndex]; // rotate derivative into grid coordinate system double adjX = (dx * c + dy * s) * w; double adjY = (-dx * s + dy * c) * w; addToHistograms( gx - gridRadius, gy - gridRadius, x / gridCellLength, y / gridCellLength, adjX, adjY); } } } } } }
private static BufferedImage disparity( ImageFloat32 src, BufferedImage dst, int minValue, int maxValue, int invalidColor) { float range = maxValue - minValue; for (int y = 0; y < src.height; y++) { for (int x = 0; x < src.width; x++) { float v = src.unsafe_get(x, y); if (v > range) { dst.setRGB(x, y, invalidColor); } else { int r, b; if (v == 0) { r = b = 0; } else { r = (int) (255 * v / maxValue); b = (int) (255 * (maxValue - v) / maxValue); } dst.setRGB(x, y, r << 16 | b); } } } return dst; }