/// <summary>
/// main update routine for contour based stereo correspondence
/// </summary>
/// <param name="left_bmp">left image data</param>
/// <param name="right_bmp">right image data</param>
/// <param name="wdth">width of the images</param>
/// <param name="hght">height of the images</param>
/// <param name="calibration_offset_x">calibration offset to counter for any small vergence angle between the cameras</param>
/// <param name="calibration_offset_y">calibration offset to counter for any small vergence angle between the cameras</param>
/// <param name="reset_attention">reset the attention map</param>
public void update(Byte[] left_bmp, Byte[] right_bmp,
int wdth, int hght,
float calibration_offset_x, float calibration_offset_y,
bool reset_attention)
{
int scale, idx;
int x, y, x2;
if ((wavepoints_left == null) ||
(vertical_compression != prev_vertical_compression) ||
(disparity_map_compression != prev_disparity_map_compression))
{
// create image objects to store the left and right camera data
img_left = new classimage();
img_left.createImage(wdth, hght / vertical_compression);
img_right = new classimage();
img_right.createImage(wdth, hght / vertical_compression);
wavepoints_left = new float[hght / vertical_compression][][];
wavepoints_right = new float[hght / vertical_compression][][];
wavepoints_left_scale = new byte[hght / vertical_compression][];
wavepoints_left_pattern = new byte[hght / vertical_compression][];
wavepoints_right_scale = new byte[hght / vertical_compression][];
wavepoints_right_pattern = new byte[hght / vertical_compression][];
for (int i = 0; i < wavepoints_left.Length; i++)
{
wavepoints_left[i] = new float[wdth / step_size][];
wavepoints_right[i] = new float[wdth / step_size][];
wavepoints_left_scale[i] = new byte[wdth / step_size];
wavepoints_left_pattern[i] = new byte[wdth / step_size];
wavepoints_right_scale[i] = new byte[wdth / step_size];
wavepoints_right_pattern[i] = new byte[wdth / step_size];
for (int j = 0; j < wavepoints_left[i].Length; j++)
{
wavepoints_left[i][j] = new float[3];
wavepoints_right[i][j] = new float[3];
}
}
scalepoints_left = new int[no_of_scales][];
scalepoints_right = new int[no_of_scales][];
scalepoints_lookup = new int[no_of_scales][][];
for (int i = 0; i < no_of_scales; i++)
{
scalepoints_left[i] = new int[wdth + 1];
scalepoints_right[i] = new int[wdth + 1];
scalepoints_lookup[i] = new int[wdth][];
for (int j = 0; j < scalepoints_lookup[i].Length; j++)
{
scalepoints_lookup[i][j] = new int[wdth + 1];
}
}
// create an attention map
attention_map = new bool[wdth, hght];
resetAttention(wdth, hght);
int w = (wdth / (step_size * disparity_map_compression)) + 1;
int h = (hght / (vertical_compression * disparity_map_compression)) + 1;
disparity_map = new float[w][];
disparity_hits = new float[w][];
for (int i = 0; i < w; i++)
{
disparity_map[i] = new float[h];
disparity_hits[i] = new float[h];
}
scale_width = new int[no_of_scales][];
int sc = 2;
for (int s = 0; s < no_of_scales; s++)
{
scale_width[s] = new int[2];
scale_width[s][0] = (int)(wdth * surround_radius_percent * sc / 100);
if (scale_width[s][0] < 2)
{
scale_width[s][0] = 2;
}
scale_width[s][1] = (int)((hght / vertical_compression) * surround_radius_percent * sc / 100);
if (scale_width[s][1] < 2)
{
scale_width[s][1] = 2;
}
sc++;
}
}
if (reset_attention)
{
resetAttention(wdth, hght);
}
// store compression values so that changes in these
// values can be detected
prev_vertical_compression = vertical_compression;
prev_disparity_map_compression = disparity_map_compression;
// set the images
left_image = left_bmp;
img_left.updateFromBitmapVerticalCompression(left_bmp, wdth, hght, vertical_compression, 0, 0);
img_right.updateFromBitmapVerticalCompression(right_bmp, wdth, hght, vertical_compression, (int)calibration_offset_x, (int)calibration_offset_y);
// update integrals
img_left.updateIntegralImage();
img_right.updateIntegralImage();
// update average intensities for each row and column
img_left.updateAverages();
img_right.updateAverages();
// disparity map dimensions
int compressed_wdth = wdth / (step_size * disparity_map_compression);
int compressed_hght = hght / (vertical_compression * disparity_map_compression);
// clear the disparity map
clearDisparityMap(compressed_wdth, compressed_hght);
// update blobs on multiple scales
for (scale = 0; scale < no_of_scales; scale++)
{
// get x and y radius for this scale
int surround_pixels_x = scale_width[scale][0];
int surround_pixels_y = scale_width[scale][1];
// detect blobs at this scale
img_left.detectBlobs(scale, surround_pixels_x, surround_pixels_y, step_size, wavepoints_left, wavepoints_left_scale, wavepoints_left_pattern);
img_right.detectBlobs(scale, surround_pixels_x, surround_pixels_y, step_size, wavepoints_right, wavepoints_right_scale, wavepoints_right_pattern);
}
// update the scale points for fast searching
float min_thresh = 5.0f;
float min_grad = 0.5f;
float left_diff, right_diff;
float prev_left_diff = 0, prev_right_diff = 0;
float prev_left_grad = 0, prev_right_grad = 0;
float left_grad = 0, right_grad = 0;
int max_disp = max_disparity * (wdth / step_size) / 100;
int searchfactor = 4;
int max_disp2 = max_disp / searchfactor;
int max_wdth = wdth / searchfactor;
int max_vertical_edge_difference = hght / 4;
// assorted variables
int no_of_points_left, no_of_points_right;
int disp, x_left, vertical_left, x_left2, x_left3, no_of_candidates;
int prev_pattern_left, next_pattern_left, idx2;
int x_right, vertical_right, x_right2, x_right3, dx, prev_pattern_right, next_pattern_right;
float diff_left, diff_row_left, diff_col_left, min_response_difference;
float confidence, diff_right, response_difference;
// for each row of the image
for (y = 0; y < hght / vertical_compression; y++)
{
for (int sign = 0; sign < 8; sign++)
{
// go through each detection pattern
// at present there are only two patterns: centre/surround and left/right
for (int currPattern = PATTERN_CENTRE_SURROUND; currPattern <= PATTERN_LEFT_RIGHT; currPattern++)
{
// clear the number of points
for (scale = 0; scale < no_of_scales; scale++)
{
scalepoints_left[scale][0] = 0;
scalepoints_right[scale][0] = 0;
for (x = 0; x < max_wdth; x++)
{
scalepoints_lookup[scale][x][0] = 0;
}
}
int ww = wdth / step_size;
for (x = 0; x < ww; x++)
{
int pattern = wavepoints_left_pattern[y][x];
if (pattern == currPattern)
{
// response value
left_diff = wavepoints_left[y][x][0];
right_diff = wavepoints_right[y][x][0];
if ((x > 0) && ((left_diff != 0) || (right_diff != 0)))
{
float left_row_diff = wavepoints_left[y][x][1];
float right_row_diff = wavepoints_right[y][x][1];
// gradient - change in response along the row
left_grad = left_diff - prev_left_diff;
right_grad = right_diff - prev_right_diff;
if (((left_row_diff > 0) && (right_row_diff > 0)) ||
((left_row_diff < 0) && (right_row_diff < 0)))
{
float left_col_diff = wavepoints_left[y][x][2];
float right_col_diff = wavepoints_right[y][x][2];
if (((left_col_diff >= 0) && (right_col_diff >= 0)) ||
((left_col_diff < 0) && (right_col_diff < 0)))
{
float left_horizontal_grad_change = left_grad - prev_left_grad;
float right_horizontal_grad_change = right_grad - prev_right_grad;
if ((left_diff != 0) && ((left_grad < -min_grad) || (left_grad > min_grad)))
{
// combiantions of response and gradient directions
if (((sign == 0) && (left_diff > min_thresh) && (left_grad > 0) && (left_horizontal_grad_change > 0)) ||
((sign == 1) && (left_diff < -min_thresh) && (left_grad > 0) && (left_horizontal_grad_change > 0)) ||
((sign == 2) && (left_diff > min_thresh) && (left_grad <= 0) && (left_horizontal_grad_change > 0)) ||
((sign == 3) && (left_diff < -min_thresh) && (left_grad <= 0) && (left_horizontal_grad_change > 0)) ||
((sign == 4) && (left_diff > min_thresh) && (left_grad > 0) && (left_horizontal_grad_change <= 0)) ||
((sign == 5) && (left_diff < -min_thresh) && (left_grad > 0) && (left_horizontal_grad_change <= 0)) ||
((sign == 6) && (left_diff > min_thresh) && (left_grad <= 0) && (left_horizontal_grad_change <= 0)) ||
((sign == 7) && (left_diff < -min_thresh) && (left_grad <= 0) && (left_horizontal_grad_change <= 0))
)
{
// what is the best responding scale ?
scale = wavepoints_left_scale[y][x];
// get the current index
idx = scalepoints_left[scale][0] + 1;
// set the x position
scalepoints_left[scale][idx] = x;
// increment the index
scalepoints_left[scale][0]++;
}
}
if ((right_diff != 0) && ((right_grad < -min_grad) || (right_grad > min_grad)))
{
// combiantions of response and gradient directions
if (((sign == 0) && (right_diff > min_thresh) && (right_grad > 0) && (right_horizontal_grad_change > 0)) ||
((sign == 1) && (right_diff < -min_thresh) && (right_grad > 0) && (right_horizontal_grad_change > 0)) ||
((sign == 2) && (right_diff > min_thresh) && (right_grad <= 0) && (right_horizontal_grad_change > 0)) ||
((sign == 3) && (right_diff < -min_thresh) && (right_grad <= 0) && (right_horizontal_grad_change > 0)) ||
((sign == 4) && (right_diff > min_thresh) && (right_grad > 0) && (right_horizontal_grad_change <= 0)) ||
((sign == 5) && (right_diff < -min_thresh) && (right_grad > 0) && (right_horizontal_grad_change <= 0)) ||
((sign == 6) && (right_diff > min_thresh) && (right_grad <= 0) && (right_horizontal_grad_change <= 0)) ||
((sign == 7) && (right_diff < -min_thresh) && (right_grad <= 0) && (right_horizontal_grad_change <= 0))
)
{
scale = wavepoints_right_scale[y][x];
// get the current index
idx = scalepoints_right[scale][0] + 1;
// set the x position
scalepoints_right[scale][idx] = x;
// increment the index
scalepoints_right[scale][0]++;
x2 = x / searchfactor;
for (int xx = x2; xx < x2 + max_disp2; xx++)
{
if ((xx > -1) && (xx < max_wdth))
{
idx2 = scalepoints_lookup[scale][xx][0] + 1;
scalepoints_lookup[scale][xx][idx2] = idx;
scalepoints_lookup[scale][xx][0]++;
}
}
}
}
}
}
}
// record previous responses
prev_left_grad = left_grad;
prev_right_grad = right_grad;
prev_left_diff = left_diff;
prev_right_diff = right_diff;
}
}
// stereo match
for (scale = no_of_scales - 1; scale >= 0; scale--)
{
no_of_points_left = scalepoints_left[scale][0];
no_of_points_right = scalepoints_right[scale][0];
//for each possible match in the left image
for (int i = no_of_points_left - 1; i >= 0; i--)
{
disp = -1;
// get the position and response magnitude of the left point
x_left = scalepoints_left[scale][i + 1];
vertical_left = img_left.column_maximal_edge[x_left];
diff_left = wavepoints_left[y][x_left][0];
diff_row_left = wavepoints_left[y][x_left][1];
diff_col_left = wavepoints_left[y][x_left][2];
x_left2 = x_left - 2;
if (x_left2 < 0)
{
x_left2 = 0;
}
x_left3 = x_left + 2;
if (x_left3 >= ww)
{
x_left3 = ww - 1;
}
prev_pattern_left = wavepoints_left_pattern[y][x_left2];
next_pattern_left = wavepoints_left_pattern[y][x_left3];
min_response_difference = match_threshold;
x2 = x_left / searchfactor;
no_of_candidates = scalepoints_lookup[scale][x2][0];
// for each possible match in the right image
// note here that we scan from right to left
for (int j = no_of_candidates - 1; j >= 0; j--)
{
idx2 = scalepoints_lookup[scale][x2][j + 1];
// get the horizontal position of the possible match in the right image
x_right = scalepoints_right[scale][idx2];
// what's the disparity ?
dx = x_left - x_right;
// is the disparity in the range we expect ?
if ((dx > -1) && (dx < max_disp))
{
// vertical context checking
vertical_right = img_left.column_maximal_edge[x_right];
int dv = vertical_left - vertical_right;
if (dv < 0)
{
dv = -dv;
}
// is the vertical context within tollerance ?
if (dv < max_vertical_edge_difference)
{
// check the ordering of patterns
x_right2 = x_right - 2;
if (x_right2 < 0)
{
x_right2 = 0;
}
prev_pattern_right = wavepoints_right_pattern[y][x_right2];
if (prev_pattern_left == prev_pattern_right)
{
x_right3 = x_right + 2;
if (x_right3 >= ww)
{
x_right3 = ww - 1;
}
next_pattern_right = wavepoints_right_pattern[y][x_right3];
if (next_pattern_left == next_pattern_right)
{
// check the response magnitude difference
diff_right = wavepoints_right[y][x_right][0];
response_difference = diff_right - diff_left;
if (response_difference < 0)
{
response_difference = -response_difference;
}
response_difference *= dv;
// is the magnitude difference the best that we've found so far ? ?
if (response_difference < min_response_difference)
{
// record the disparity and minimum difference
disp = dx;
min_response_difference = response_difference;
}
}
}
}
}
// if the horizontal difference is too large then we may
// as well abandon the search
if (dx > max_disp)
{
break;
}
}
if (disp > -1)
{
// how confident are we in this match ?
confidence = 1.0f - (min_response_difference / match_threshold);
confidence /= (no_of_scales - scale);
confidence *= confidence;
// get the position on the disparity map
int mx = (x_left + disp) / disparity_map_compression;
int my = y / disparity_map_compression;
// update the dispalrity map using a gaussian
// probability distribution
updateDisparityMap(mx, my,
compressed_wdth, compressed_hght,
scale, disp * step_size, confidence);
}
}
}
}
}
}
// update disparity map
float disparity_value;
for (y = compressed_hght; y >= 0; y--)
{
for (x = compressed_wdth - 1; x >= 0; x--)
{
disparity_value = disparity_map[x][y];
if (disparity_value < 0)
{
disparity_value = 0;
}
else
{
disparity_value /= disparity_hits[x][y];
disparity_map[x][y] = disparity_value;
}
}
}
// get a fixed quantity of features which may
// subsequently be used to create ray models
getSelectedFeatures(wdth, hght);
}
/// <summary>
/// main update routine for contour based stereo correspondence
/// </summary>
/// <param name="left_bmp">left image data</param>
/// <param name="right_bmp">right image data</param>
/// <param name="wdth">width of the images</param>
/// <param name="hght">height of the images</param>
/// <param name="calibration_offset_x">calibration offset to counter for any small vergence angle between the cameras</param>
/// <param name="calibration_offset_y">calibration offset to counter for any small vergence angle between the cameras</param>
/// <param name="reset_attention">reset the attention map</param>
public void update(Byte[] left_bmp, Byte[] right_bmp,
int wdth, int hght,
float calibration_offset_x, float calibration_offset_y,
bool reset_attention)
{
int scale, idx;
int x, y, x2;
if ((wavepoints_left == null) ||
(vertical_compression != prev_vertical_compression) ||
(disparity_map_compression != prev_disparity_map_compression))
{
// create image objects to store the left and right camera data
img_left = new classimage();
img_left.createImage(wdth, hght / vertical_compression);
img_right = new classimage();
img_right.createImage(wdth, hght / vertical_compression);
wavepoints_left = new float[hght / vertical_compression][][];
wavepoints_right = new float[hght / vertical_compression][][];
wavepoints_left_scale = new byte[hght / vertical_compression][];
wavepoints_left_pattern = new byte[hght / vertical_compression][];
wavepoints_right_scale = new byte[hght / vertical_compression][];
wavepoints_right_pattern = new byte[hght / vertical_compression][];
for (int i = 0; i < wavepoints_left.Length; i++)
{
wavepoints_left[i] = new float[wdth / step_size][];
wavepoints_right[i] = new float[wdth / step_size][];
wavepoints_left_scale[i] = new byte[wdth / step_size];
wavepoints_left_pattern[i] = new byte[wdth / step_size];
wavepoints_right_scale[i] = new byte[wdth / step_size];
wavepoints_right_pattern[i] = new byte[wdth / step_size];
for (int j = 0; j < wavepoints_left[i].Length; j++)
{
wavepoints_left[i][j] = new float[3];
wavepoints_right[i][j] = new float[3];
}
}
scalepoints_left = new int[no_of_scales][];
scalepoints_right = new int[no_of_scales][];
scalepoints_lookup = new int[no_of_scales][][];
for (int i = 0; i < no_of_scales; i++)
{
scalepoints_left[i] = new int[wdth + 1];
scalepoints_right[i] = new int[wdth + 1];
scalepoints_lookup[i] = new int[wdth][];
for (int j = 0; j < scalepoints_lookup[i].Length; j++)
{
scalepoints_lookup[i][j] = new int[wdth + 1];
}
}
// create an attention map
attention_map = new bool[wdth, hght];
resetAttention(wdth, hght);
int w = (wdth / (step_size * disparity_map_compression)) + 1;
int h = (hght / (vertical_compression * disparity_map_compression)) + 1;
disparity_map = new float[w][];
disparity_hits = new float[w][];
for (int i = 0; i < w; i++)
{
disparity_map[i] = new float[h];
disparity_hits[i] = new float[h];
}
scale_width = new int[no_of_scales][];
int sc = 2;
for (int s = 0; s < no_of_scales; s++)
{
scale_width[s] = new int[2];
scale_width[s][0] = (int)(wdth * surround_radius_percent * sc / 100);
if (scale_width[s][0] < 2) scale_width[s][0] = 2;
scale_width[s][1] = (int)((hght / vertical_compression) * surround_radius_percent * sc / 100);
if (scale_width[s][1] < 2) scale_width[s][1] = 2;
sc++;
}
}
if (reset_attention) resetAttention(wdth, hght);
// store compression values so that changes in these
// values can be detected
prev_vertical_compression = vertical_compression;
prev_disparity_map_compression = disparity_map_compression;
// set the images
left_image = left_bmp;
img_left.updateFromBitmapVerticalCompression(left_bmp, wdth, hght, vertical_compression, 0, 0);
img_right.updateFromBitmapVerticalCompression(right_bmp, wdth, hght, vertical_compression, (int)calibration_offset_x, (int)calibration_offset_y);
// update integrals
img_left.updateIntegralImage();
img_right.updateIntegralImage();
// update average intensities for each row and column
img_left.updateAverages();
img_right.updateAverages();
// disparity map dimensions
int compressed_wdth = wdth / (step_size * disparity_map_compression);
int compressed_hght = hght / (vertical_compression * disparity_map_compression);
// clear the disparity map
clearDisparityMap(compressed_wdth, compressed_hght);
// update blobs on multiple scales
for (scale = 0; scale < no_of_scales; scale++)
{
// get x and y radius for this scale
int surround_pixels_x = scale_width[scale][0];
int surround_pixels_y = scale_width[scale][1];
// detect blobs at this scale
img_left.detectBlobs(scale, surround_pixels_x, surround_pixels_y, step_size, wavepoints_left, wavepoints_left_scale, wavepoints_left_pattern);
img_right.detectBlobs(scale, surround_pixels_x, surround_pixels_y, step_size, wavepoints_right, wavepoints_right_scale, wavepoints_right_pattern);
}
// update the scale points for fast searching
float min_thresh = 5.0f;
float min_grad = 0.5f;
float left_diff, right_diff;
float prev_left_diff = 0, prev_right_diff = 0;
float prev_left_grad = 0, prev_right_grad = 0;
float left_grad = 0, right_grad = 0;
int max_disp = max_disparity * (wdth / step_size) / 100;
int searchfactor = 4;
int max_disp2 = max_disp / searchfactor;
int max_wdth = wdth / searchfactor;
int max_vertical_edge_difference = hght / 4;
// assorted variables
int no_of_points_left, no_of_points_right;
int disp, x_left, vertical_left, x_left2, x_left3, no_of_candidates;
int prev_pattern_left, next_pattern_left, idx2;
int x_right, vertical_right, x_right2, x_right3, dx, prev_pattern_right, next_pattern_right;
float diff_left, diff_row_left, diff_col_left, min_response_difference;
float confidence, diff_right, response_difference;
// for each row of the image
for (y = 0; y < hght / vertical_compression; y++)
{
for (int sign = 0; sign < 8; sign++)
{
// go through each detection pattern
// at present there are only two patterns: centre/surround and left/right
for (int currPattern = PATTERN_CENTRE_SURROUND; currPattern <= PATTERN_LEFT_RIGHT; currPattern++)
{
// clear the number of points
for (scale = 0; scale < no_of_scales; scale++)
{
scalepoints_left[scale][0] = 0;
scalepoints_right[scale][0] = 0;
for (x = 0; x < max_wdth; x++)
scalepoints_lookup[scale][x][0] = 0;
}
int ww = wdth / step_size;
for (x = 0; x < ww; x++)
{
int pattern = wavepoints_left_pattern[y][x];
if (pattern == currPattern)
{
// response value
left_diff = wavepoints_left[y][x][0];
right_diff = wavepoints_right[y][x][0];
if ((x > 0) && ((left_diff != 0) || (right_diff != 0)))
{
float left_row_diff = wavepoints_left[y][x][1];
float right_row_diff = wavepoints_right[y][x][1];
// gradient - change in response along the row
left_grad = left_diff - prev_left_diff;
right_grad = right_diff - prev_right_diff;
if (((left_row_diff > 0) && (right_row_diff > 0)) ||
((left_row_diff < 0) && (right_row_diff < 0)))
{
float left_col_diff = wavepoints_left[y][x][2];
float right_col_diff = wavepoints_right[y][x][2];
if (((left_col_diff >= 0) && (right_col_diff >= 0)) ||
((left_col_diff < 0) && (right_col_diff < 0)))
{
float left_horizontal_grad_change = left_grad - prev_left_grad;
float right_horizontal_grad_change = right_grad - prev_right_grad;
if ((left_diff != 0) && ((left_grad < -min_grad) || (left_grad > min_grad)))
{
// combiantions of response and gradient directions
if (((sign == 0) && (left_diff > min_thresh) && (left_grad > 0) && (left_horizontal_grad_change > 0)) ||
((sign == 1) && (left_diff < -min_thresh) && (left_grad > 0) && (left_horizontal_grad_change > 0)) ||
((sign == 2) && (left_diff > min_thresh) && (left_grad <= 0) && (left_horizontal_grad_change > 0)) ||
((sign == 3) && (left_diff < -min_thresh) && (left_grad <= 0) && (left_horizontal_grad_change > 0)) ||
((sign == 4) && (left_diff > min_thresh) && (left_grad > 0) && (left_horizontal_grad_change <= 0)) ||
((sign == 5) && (left_diff < -min_thresh) && (left_grad > 0) && (left_horizontal_grad_change <= 0)) ||
((sign == 6) && (left_diff > min_thresh) && (left_grad <= 0) && (left_horizontal_grad_change <= 0)) ||
((sign == 7) && (left_diff < -min_thresh) && (left_grad <= 0) && (left_horizontal_grad_change <= 0))
)
{
// what is the best responding scale ?
scale = wavepoints_left_scale[y][x];
// get the current index
idx = scalepoints_left[scale][0] + 1;
// set the x position
scalepoints_left[scale][idx] = x;
// increment the index
scalepoints_left[scale][0]++;
}
}
if ((right_diff != 0) && ((right_grad < -min_grad) || (right_grad > min_grad)))
{
// combiantions of response and gradient directions
if (((sign == 0) && (right_diff > min_thresh) && (right_grad > 0) && (right_horizontal_grad_change > 0)) ||
((sign == 1) && (right_diff < -min_thresh) && (right_grad > 0) && (right_horizontal_grad_change > 0)) ||
((sign == 2) && (right_diff > min_thresh) && (right_grad <= 0) && (right_horizontal_grad_change > 0)) ||
((sign == 3) && (right_diff < -min_thresh) && (right_grad <= 0) && (right_horizontal_grad_change > 0)) ||
((sign == 4) && (right_diff > min_thresh) && (right_grad > 0) && (right_horizontal_grad_change <= 0)) ||
((sign == 5) && (right_diff < -min_thresh) && (right_grad > 0) && (right_horizontal_grad_change <= 0)) ||
((sign == 6) && (right_diff > min_thresh) && (right_grad <= 0) && (right_horizontal_grad_change <= 0)) ||
((sign == 7) && (right_diff < -min_thresh) && (right_grad <= 0) && (right_horizontal_grad_change <= 0))
)
{
scale = wavepoints_right_scale[y][x];
// get the current index
idx = scalepoints_right[scale][0] + 1;
// set the x position
scalepoints_right[scale][idx] = x;
// increment the index
scalepoints_right[scale][0]++;
x2 = x / searchfactor;
for (int xx = x2; xx < x2 + max_disp2; xx++)
{
if ((xx > -1) && (xx < max_wdth))
{
idx2 = scalepoints_lookup[scale][xx][0] + 1;
scalepoints_lookup[scale][xx][idx2] = idx;
scalepoints_lookup[scale][xx][0]++;
}
}
}
}
}
}
}
// record previous responses
prev_left_grad = left_grad;
prev_right_grad = right_grad;
prev_left_diff = left_diff;
prev_right_diff = right_diff;
}
}
// stereo match
for (scale = no_of_scales - 1; scale >= 0; scale--)
{
no_of_points_left = scalepoints_left[scale][0];
no_of_points_right = scalepoints_right[scale][0];
//for each possible match in the left image
for (int i = no_of_points_left - 1; i >= 0; i--)
{
disp = -1;
// get the position and response magnitude of the left point
x_left = scalepoints_left[scale][i + 1];
vertical_left = img_left.column_maximal_edge[x_left];
diff_left = wavepoints_left[y][x_left][0];
diff_row_left = wavepoints_left[y][x_left][1];
diff_col_left = wavepoints_left[y][x_left][2];
x_left2 = x_left - 2;
if (x_left2 < 0) x_left2 = 0;
x_left3 = x_left + 2;
if (x_left3 >= ww) x_left3 = ww - 1;
prev_pattern_left = wavepoints_left_pattern[y][x_left2];
next_pattern_left = wavepoints_left_pattern[y][x_left3];
min_response_difference = match_threshold;
x2 = x_left / searchfactor;
no_of_candidates = scalepoints_lookup[scale][x2][0];
// for each possible match in the right image
// note here that we scan from right to left
for (int j = no_of_candidates - 1; j >= 0; j--)
{
idx2 = scalepoints_lookup[scale][x2][j + 1];
// get the horizontal position of the possible match in the right image
x_right = scalepoints_right[scale][idx2];
// what's the disparity ?
dx = x_left - x_right;
// is the disparity in the range we expect ?
if ((dx > -1) && (dx < max_disp))
{
// vertical context checking
vertical_right = img_left.column_maximal_edge[x_right];
int dv = vertical_left - vertical_right;
if (dv < 0) dv = -dv;
// is the vertical context within tollerance ?
if (dv < max_vertical_edge_difference)
{
// check the ordering of patterns
x_right2 = x_right - 2;
if (x_right2 < 0) x_right2 = 0;
prev_pattern_right = wavepoints_right_pattern[y][x_right2];
if (prev_pattern_left == prev_pattern_right)
{
x_right3 = x_right + 2;
if (x_right3 >= ww) x_right3 = ww - 1;
next_pattern_right = wavepoints_right_pattern[y][x_right3];
if (next_pattern_left == next_pattern_right)
{
// check the response magnitude difference
diff_right = wavepoints_right[y][x_right][0];
response_difference = diff_right - diff_left;
if (response_difference < 0) response_difference = -response_difference;
response_difference *= dv;
// is the magnitude difference the best that we've found so far ? ?
if (response_difference < min_response_difference)
{
// record the disparity and minimum difference
disp = dx;
min_response_difference = response_difference;
}
}
}
}
}
// if the horizontal difference is too large then we may
// as well abandon the search
if (dx > max_disp) break;
}
if (disp > -1)
{
// how confident are we in this match ?
confidence = 1.0f - (min_response_difference / match_threshold);
confidence /= (no_of_scales - scale);
confidence *= confidence;
// get the position on the disparity map
int mx = (x_left + disp) / disparity_map_compression;
int my = y / disparity_map_compression;
// update the dispalrity map using a gaussian
// probability distribution
updateDisparityMap(mx, my,
compressed_wdth, compressed_hght,
scale, disp * step_size, confidence);
}
}
}
}
}
}
// update disparity map
float disparity_value;
for (y = compressed_hght; y >= 0; y--)
{
for (x = compressed_wdth - 1; x >= 0; x--)
{
disparity_value = disparity_map[x][y];
if (disparity_value < 0)
{
disparity_value = 0;
}
else
{
disparity_value /= disparity_hits[x][y];
disparity_map[x][y] = disparity_value;
}
}
}
// get a fixed quantity of features which may
// subsequently be used to create ray models
getSelectedFeatures(wdth, hght);
}