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;
}