protected void MatchFeatures(
int y,
List <float> left_row_features,
List <float> right_row_features,
float calibration_offset_x,
float calibration_offset_y,
byte[] left_bmp, byte[] right_bmp)
{
int max_disparity_pixels = image_width * max_disparity / 100;
List <possible_match>[] match_probabilities_left = new List <possible_match> [left_row_features.Count / 3];
List <possible_match>[] match_probabilities_right = new List <possible_match> [right_row_features.Count / 3];
// get matching probabilities
int ii = 0;
for (int i = 0; i < left_row_features.Count; i += 3, ii++)
{
int jj = 0;
float x_left = left_row_features[i];
int direction_left = (int)left_row_features[i + 2];
for (int j = 0; j < right_row_features.Count; j += 3, jj++)
{
float x_right = right_row_features[j];
float disparity = x_left - x_right + calibration_offset_x;
//float disparity = x_right - x_left - calibration_offset_x;
if ((disparity >= 0) && (disparity < max_disparity_pixels))
{
int direction_right = (int)right_row_features[j + 2];
if (direction_left == direction_right)
{
int n1 = (y * image_width) + (int)x_left;
int n2 = ((y + (int)calibration_offset_y) * image_width) + (int)x_right;
int v = similarity(n1, n2, (int)disparity, left_bmp, right_bmp);
if ((v > 0) && (v < matching_threshold))
{
possible_match match = new possible_match();
match.feature_index_left = ii;
match.feature_index_right = jj;
match.ssd = v;
match.probability_left = v;
match.probability_right = v;
match.disparity = disparity;
if (match_probabilities_left[ii] == null)
{
match_probabilities_left[ii] = new List <possible_match>();
}
match_probabilities_left[ii].Add(match);
if (match_probabilities_right[jj] == null)
{
match_probabilities_right[jj] = new List <possible_match>();
}
match_probabilities_right[jj].Add(match);
}
}
}
}
}
// update probabilities for left to right search
for (int i = 0; i < match_probabilities_left.Length; i++)
{
if (match_probabilities_left[i] != null)
{
// total SSD
float tot_ssd = 0;
for (int j = match_probabilities_left[i].Count - 1; j >= 0; j--)
{
tot_ssd += match_probabilities_left[i][j].probability_left;
}
// convert SSD values to probabilities
if (tot_ssd > 0)
{
for (int j = match_probabilities_left[i].Count - 1; j >= 0; j--)
{
match_probabilities_left[i][j].probability_left = 1.0f - (match_probabilities_left[i][j].probability_left / tot_ssd);
}
}
}
}
// update probabilities for right to left search
for (int i = 0; i < match_probabilities_right.Length; i++)
{
if (match_probabilities_right[i] != null)
{
// total SSD
float tot_ssd = 0;
for (int j = match_probabilities_right[i].Count - 1; j >= 0; j--)
{
tot_ssd += match_probabilities_right[i][j].probability_right;
}
// convert SSD values to probabilities
if (tot_ssd > 0)
{
for (int j = match_probabilities_right[i].Count - 1; j >= 0; j--)
{
match_probabilities_right[i][j].probability_right = 1.0f - (match_probabilities_right[i][j].probability_right / tot_ssd);
}
}
}
}
int hits = 0;
float average_score = 0;
// combine probabilities from left to right and right to left search
float[] prob = new float[match_probabilities_left.Length];
float[] disp = new float[match_probabilities_left.Length];
for (int i = 0; i < match_probabilities_left.Length; i++)
{
if (match_probabilities_left[i] != null)
{
float best_score = matching_threshold;
float disparity = 0;
for (int j = match_probabilities_left[i].Count - 1; j >= 0; j--)
{
possible_match match = match_probabilities_left[i][j];
float probability = match.probability_left * match.probability_right;
float score = match.ssd * (1.0f - probability);
if ((score > 0) && (score < best_score))
{
best_score = score;
disparity = match.disparity;
}
}
prob[i] = best_score;
disp[i] = disparity;
average_score += best_score;
hits++;
}
}
if (hits > 0)
{
average_score /= hits;
float threshold = average_score * similarity_threshold_percent / 100;
for (int i = 0; i < prob.Length; i++)
{
if ((prob[i] < threshold) && (disp[i] > 0))
{
float x_left = left_row_features[i * 3];
float disparity = disp[i];
features.Add(new StereoFeature(x_left, y, disparity));
}
}
}
}
protected void MatchFeatures(
int y,
List<float> left_row_features,
List<float> right_row_features,
float calibration_offset_x,
float calibration_offset_y,
byte[] left_bmp, byte[] right_bmp)
{
int max_disparity_pixels = image_width * max_disparity / 100;
List<possible_match>[] match_probabilities_left = new List<possible_match>[left_row_features.Count/3];
List<possible_match>[] match_probabilities_right = new List<possible_match>[right_row_features.Count/3];
// get matching probabilities
int ii = 0;
for (int i = 0; i < left_row_features.Count; i += 3, ii++)
{
int jj = 0;
float x_left = left_row_features[i];
int direction_left = (int)left_row_features[i + 2];
for (int j = 0; j < right_row_features.Count; j += 3, jj++)
{
float x_right = right_row_features[j];
float disparity = x_left - x_right + calibration_offset_x;
//float disparity = x_right - x_left - calibration_offset_x;
if ((disparity >= 0) && (disparity < max_disparity_pixels))
{
int direction_right = (int)right_row_features[j + 2];
if (direction_left == direction_right)
{
int n1 = (y * image_width) + (int)x_left;
int n2 = ((y+(int)calibration_offset_y) * image_width) + (int)x_right;
int v = similarity(n1, n2, (int)disparity, left_bmp, right_bmp);
if ((v > 0) && (v < matching_threshold))
{
possible_match match = new possible_match();
match.feature_index_left = ii;
match.feature_index_right = jj;
match.ssd = v;
match.probability_left = v;
match.probability_right = v;
match.disparity = disparity;
if (match_probabilities_left[ii] == null)
match_probabilities_left[ii] = new List<possible_match>();
match_probabilities_left[ii].Add(match);
if (match_probabilities_right[jj] == null)
match_probabilities_right[jj] = new List<possible_match>();
match_probabilities_right[jj].Add(match);
}
}
}
}
}
// update probabilities for left to right search
for (int i = 0; i < match_probabilities_left.Length; i++)
{
if (match_probabilities_left[i] != null)
{
// total SSD
float tot_ssd = 0;
for (int j = match_probabilities_left[i].Count - 1; j >= 0; j--)
tot_ssd += match_probabilities_left[i][j].probability_left;
// convert SSD values to probabilities
if (tot_ssd > 0)
for (int j = match_probabilities_left[i].Count - 1; j >= 0; j--)
match_probabilities_left[i][j].probability_left = 1.0f - (match_probabilities_left[i][j].probability_left / tot_ssd);
}
}
// update probabilities for right to left search
for (int i = 0; i < match_probabilities_right.Length; i++)
{
if (match_probabilities_right[i] != null)
{
// total SSD
float tot_ssd = 0;
for (int j = match_probabilities_right[i].Count - 1; j >= 0; j--)
tot_ssd += match_probabilities_right[i][j].probability_right;
// convert SSD values to probabilities
if (tot_ssd > 0)
for (int j = match_probabilities_right[i].Count - 1; j >= 0; j--)
match_probabilities_right[i][j].probability_right = 1.0f - (match_probabilities_right[i][j].probability_right / tot_ssd);
}
}
int hits = 0;
float average_score = 0;
// combine probabilities from left to right and right to left search
float[] prob = new float[match_probabilities_left.Length];
float[] disp = new float[match_probabilities_left.Length];
for (int i = 0; i < match_probabilities_left.Length; i++)
{
if (match_probabilities_left[i] != null)
{
float best_score = matching_threshold;
float disparity= 0;
for (int j = match_probabilities_left[i].Count - 1; j >= 0; j--)
{
possible_match match = match_probabilities_left[i][j];
float probability = match.probability_left * match.probability_right;
float score = match.ssd * (1.0f - probability);
if ((score > 0) && (score < best_score))
{
best_score = score;
disparity = match.disparity;
}
}
prob[i] = best_score;
disp[i] = disparity;
average_score += best_score;
hits++;
}
}
if (hits > 0)
{
average_score /= hits;
float threshold = average_score * similarity_threshold_percent / 100;
for (int i = 0; i < prob.Length; i++)
{
if ((prob[i] < threshold) && (disp[i] > 0))
{
float x_left = left_row_features[i*3];
float disparity = disp[i];
features.Add(new StereoFeature(x_left, y, disparity));
}
}
}
}
