public unsafe void stereoMatch(Byte[] left_bmp, Byte[] right_bmp,
int width, int height, bool colourImages)
{
if (!initialised)
{
initialised = true;
}
int BytesPerPixel = 3;
if (!colourImages) BytesPerPixel = 1;
stereointerface.loadImage(left_bmp, width, height, true, BytesPerPixel);
stereointerface.loadImage(right_bmp, width, height, false, BytesPerPixel);
setMaxDisparity(max_disparity_percent);
setRequiredFeatures(required_features);
// do the stereo correspondence
stereointerface.stereoMatchRun(0, peaks_per_row, correspondence_algorithm_type);
// retrieve features
no_of_disparities = stereointerface.getSelectedPointFeatures(disparities);
// track the features
features = new stereoFeatures(no_of_disparities);
if (correspondence_algorithm_type == 0)
{
for (int i = 0; i < no_of_disparities * 3; i++) features.features[i] = disparities[i];
tracking.update(features, width, height);
for (int i = 2; i < no_of_disparities; i += 3) disparities[i] = features.features[i];
}
//robot_head.features[0] = features;
// update radar
radar.update(width, height, features);
// dynamically adjust the difference threshold to try to
// maintain a constant number of stereo features
if (no_of_disparities < required_features*9/10)
{
difference_threshold -= 1;
if (difference_threshold < 10)
{
difference_threshold = 10;
}
setDifferenceThreshold(difference_threshold);
if (no_of_disparities < required_features * 8 / 10)
if (peaks_per_row < 10) peaks_per_row++;
}
else
{
if (no_of_disparities >= required_features*9/10)
{
if (peaks_per_row > 5)
peaks_per_row--;
else
{
difference_threshold += 2;
if (difference_threshold > 10000) difference_threshold = 10000;
setDifferenceThreshold(difference_threshold);
}
}
}
prev_features = features;
}
/// <summary>
/// load stereo features for all cameras
/// </summary>
/// <param name="camera_index">stereo camera index</param>
/// <param name="feat">stereo camera features</param>
public void setStereoFeatures(int camera_index, stereoFeatures feat)
{
features[camera_index] = feat;
}
/// <summary>
/// load stereo features for all cameras
/// </summary>
/// <param name="binfile">file to load from</param>
public void loadStereoFeatures(BinaryReader binfile)
{
for (int i = 0; i < no_of_stereo_cameras; i++)
{
// create a new object
stereoFeatures feat = new stereoFeatures(1);
// load features for this camera
feat.load(binfile);
// update the head with these features
setStereoFeatures(i, feat);
}
}
/// <summary>
/// assign stereo features to the given camera
/// </summary>
/// <param name="camera_index">stereo camera index</param>
/// <param name="featurelist">stereo features</param>
/// <param name="uncertainties">governs the standard deviation used when ray throwing</param>
/// <param name="no_of_features">number of stereo features</param>
public void setStereoFeatures(int camera_index,
float[] featurelist,
float[] uncertainties,
int no_of_features)
{
if (features[camera_index] == null)
features[camera_index] = new stereoFeatures(no_of_features);
if (featurelist != null)
{
features[camera_index].no_of_features = no_of_features;
features[camera_index].features = (float[])featurelist.Clone();
if (uncertainties != null)
features[camera_index].uncertainties = (float[])uncertainties.Clone();
}
else
{
features[camera_index].no_of_features = 0;
features[camera_index].features = null;
}
}
/// <summary>
/// load a pair of images
/// </summary>
private float loadImages(int stereo_cam_index, Byte[] fullres_left, Byte[] fullres_right, stereoHead head, int no_of_stereo_features, int bytes_per_pixel,
int algorithm_type)
{
stereointerface.loadImage(fullres_left, head.calibration[stereo_cam_index].leftcam.image_width, head.calibration[stereo_cam_index].leftcam.image_height, true, bytes_per_pixel);
stereointerface.loadImage(fullres_right, head.calibration[stereo_cam_index].leftcam.image_width, head.calibration[stereo_cam_index].leftcam.image_height, false, bytes_per_pixel);
// calculate stereo disparity features
int peaks_per_row = 5;
stereointerface.stereoMatchRun(0, peaks_per_row, algorithm_type);
// retrieve the features
stereoFeatures feat = new stereoFeatures(no_of_stereo_features);
int no_of_selected_features = 0;
stereoFeatures features;
no_of_selected_features = stereointerface.getSelectedPointFeatures(feat.features);
if (no_of_selected_features > 0)
{
if (no_of_selected_features == no_of_stereo_features)
{
features = feat;
}
else
{
features = new stereoFeatures(no_of_selected_features);
for (int f = 0; f < no_of_selected_features * 3; f++)
features.features[f] = feat.features[f];
}
// update the head with these features
head.setStereoFeatures(stereo_cam_index, features);
// update the colours for each feature
head.updateFeatureColours(stereo_cam_index, fullres_left);
}
if (no_of_selected_features > 0) //no_of_stereo_features * 4 / 10)
return (stereointerface.getAverageMatchingScore());
else
return (-1);
}
public unsafe void stereoMatch(Byte[] left_bmp, Byte[] right_bmp,
int width, int height, bool colourImages)
{
if (!initialised)
{
initialised = true;
}
int BytesPerPixel = 3;
if (!colourImages)
{
BytesPerPixel = 1;
}
stereointerface.loadImage(left_bmp, width, height, true, BytesPerPixel);
stereointerface.loadImage(right_bmp, width, height, false, BytesPerPixel);
setMaxDisparity(max_disparity_percent);
setRequiredFeatures(required_features);
// do the stereo correspondence
stereointerface.stereoMatchRun(0, peaks_per_row, correspondence_algorithm_type);
// retrieve features
no_of_disparities = stereointerface.getSelectedPointFeatures(disparities);
// track the features
features = new stereoFeatures(no_of_disparities);
if (correspondence_algorithm_type == 0)
{
for (int i = 0; i < no_of_disparities * 3; i++)
{
features.features[i] = disparities[i];
}
tracking.update(features, width, height);
for (int i = 2; i < no_of_disparities; i += 3)
{
disparities[i] = features.features[i];
}
}
//robot_head.features[0] = features;
// update radar
radar.update(width, height, features);
// dynamically adjust the difference threshold to try to
// maintain a constant number of stereo features
if (no_of_disparities < required_features * 9 / 10)
{
difference_threshold -= 1;
if (difference_threshold < 10)
{
difference_threshold = 10;
}
setDifferenceThreshold(difference_threshold);
if (no_of_disparities < required_features * 8 / 10)
{
if (peaks_per_row < 10)
{
peaks_per_row++;
}
}
}
else
{
if (no_of_disparities >= required_features * 9 / 10)
{
if (peaks_per_row > 5)
{
peaks_per_row--;
}
else
{
difference_threshold += 2;
if (difference_threshold > 10000)
{
difference_threshold = 10000;
}
setDifferenceThreshold(difference_threshold);
}
}
}
prev_features = features;
}
/// <summary>
/// update the radar
/// </summary>
/// <param name="img_width"></param>
/// <param name="img_height"></param>
/// <param name="features"></param>
public void update(int img_width, int img_height,
stereoFeatures features)
{
if (ranges_horizontal == null)
{
ranges_horizontal = new float[img_width];
ranges_vertical = new float[img_height];
rawranges_horizontal = new float[img_width];
tempranges_horizontal = new float[img_width];
rawranges_vertical = new float[img_height];
variation_horizontal = new float[img_width];
variation_vertical = new float[img_height];
//av_variation_horizontal = new float[img_width];
hits_horizontal = new int[img_width];
hits_vertical = new int[img_height];
img = new Byte[img_width * img_height * 3];
for (int h = 0; h < img_height; h++)
{
rawranges_vertical[h] = 0;
hits_vertical[h] = 0;
}
for (int w = 0; w < img_width; w++)
{
rawranges_horizontal[w] = 0;
hits_horizontal[w] = 0;
//av_variation_horizontal[w] = 0;
}
}
for (int h = 0; h < img_height; h++)
{
variation_vertical[h] = ranges_vertical[h];
rawranges_vertical[h] = 0;
hits_vertical[h] = 0;
}
for (int w = 0; w < img_width; w++)
{
variation_horizontal[w] = ranges_horizontal[w];
rawranges_horizontal[w] = 0;
hits_horizontal[w] = 0;
}
for (int i = 0; i < features.no_of_features; i++)
{
int x = (int)features.features[i * 3];
int y = (int)features.features[(i * 3) + 1];
float disp = features.features[(i * 3) + 2];
if (disp > 0)
{
float dist = 1 / disp;
rawranges_horizontal[x] += dist;
hits_horizontal[x]++;
rawranges_vertical[y] += dist;
hits_vertical[y]++;
}
}
// clear the image
for (int i = 0; i < img_width * img_height * 3; i++) img[i] = 0;
for (int w = 0; w < img_width; w++)
{
if (hits_horizontal[w] > 0)
{
rawranges_horizontal[w] = rawranges_horizontal[w] * scale / hits_horizontal[w];
variation_horizontal[w] -= rawranges_horizontal[w];
ranges_horizontal[w] = rawranges_horizontal[w];
tempranges_horizontal[w] = ranges_horizontal[w];
}
}
// smooth out noise
int prev_range_index = -1;
int prev_range_index2 = -1;
for (int smooth = 0; smooth < smoothing_steps; smooth++)
{
for (int w = 1; w < img_width-1; w++)
if (hits_horizontal[w] > 0)
{
if ((prev_range_index > -1) && (prev_range_index2 > -1))
{
ranges_horizontal[prev_range_index] = (tempranges_horizontal[w] + tempranges_horizontal[prev_range_index] + tempranges_horizontal[prev_range_index2]) / 3.0f;
}
prev_range_index2 = prev_range_index;
prev_range_index = w;
}
for (int w = 1; w < img_width-1; w++)
if (hits_horizontal[w] > 0)
tempranges_horizontal[w] = ranges_horizontal[w];
}
prev_range_index = -1;
prev_range_index2 = -1;
for (int w = 0; w < img_width; w++)
if (hits_horizontal[w] > 0)
{
if (variation_horizontal[w] < 0) variation_horizontal[w] = -variation_horizontal[w];
if (ranges_horizontal[w]>0.01f)
hits_horizontal[w] = 1;
else
hits_horizontal[w] = 0;
prev_range_index2 = prev_range_index;
prev_range_index = w;
}
prev_range_index = -1;
prev_range_index2 = -1;
for (int h = 0; h < img_height; h++)
if (hits_vertical[h] > 0)
{
rawranges_vertical[h] = rawranges_vertical[h] * scale / hits_vertical[h];
variation_vertical[h] -= rawranges_vertical[h];
ranges_vertical[h] = rawranges_vertical[h];
if ((h > 0) && (h < img_height - 1) &&
(prev_range_index > -1) && (prev_range_index2 > -1))
{
ranges_vertical[prev_range_index] = (rawranges_vertical[h] + rawranges_vertical[prev_range_index] + rawranges_vertical[prev_range_index2]) / 3.0f;
}
if (variation_vertical[h] < 0) variation_vertical[h] = -variation_vertical[h];
if (ranges_vertical[h] > 0.01f)
hits_vertical[h] = 1;
else
hits_vertical[h] = 0;
prev_range_index2 = prev_range_index;
prev_range_index = h;
}
int prev_x = 0;
int prev_y = 0;
//overhead
for (int w = 0; w < img_width; w++)
{
if ((hits_horizontal[w] > 0) && (ranges_horizontal[w] > 0.01f))
{
if ((variation_horizontal[w] < 50) && (variation_horizontal[prev_x] < 50))
{
if (prev_x > 0)
{
for (int xx = prev_x+1; xx < w; xx++)
{
ranges_horizontal[xx] = ranges_horizontal[prev_x] + ((xx - prev_x) * (ranges_horizontal[w] - ranges_horizontal[prev_x]) / (w - prev_x));
hits_horizontal[xx] = 1;
}
if (display_type == 0)
{
if (((int)ranges_horizontal[w] > 0) && ((int)ranges_horizontal[w] < img_height))
drawing.drawLine(img, img_width, img_height, w, img_height - 1 - (int)ranges_horizontal[w], prev_x, prev_y, 0, 255, 0, 1, false);
}
}
prev_x = w;
prev_y = img_height - 1 - (int)ranges_horizontal[w];
}
}
}
//side
prev_x = 0;
prev_y = 0;
for (int h = 0; h < img_height; h++)
{
if ((hits_vertical[h] > 0) && (ranges_vertical[h] > 0.01f))
{
if ((variation_vertical[h] < 50) && (variation_vertical[prev_y] < 50))
{
if (prev_y > 0)
{
for (int yy = prev_y + 1; yy < h; yy++)
{
ranges_vertical[yy] = ranges_vertical[prev_y] + ((yy - prev_y) * (ranges_vertical[h] - ranges_vertical[prev_y]) / (h - prev_y));
hits_vertical[yy] = 1;
}
if (display_type == 1)
{
if (((int)ranges_vertical[h] > 0) && ((int)ranges_vertical[h] < img_width))
drawing.drawLine(img, img_width, img_height, (int)ranges_vertical[h], h, prev_x, prev_y, 0, 255, 0, 1, false);
}
}
prev_x = (int)ranges_vertical[h];
prev_y = h;
}
}
}
//calculate average disparity
int hits = 0;
float av_disparity = 0;
for (int w = img_width / 4; w < img_width - (img_width / 4); w++)
if (hits_horizontal[w] > 0)
{
av_disparity += (ranges_horizontal[w] / scale);
hits++;
}
if (hits > 0) av_disparity /= hits;
if (av_disparity > 0) av_disparity = 1.0f / av_disparity;
average_disparity = (average_disparity * 0.7f) + (av_disparity * 0.3f);
if (display_type == 2)
{
for (int xx = 0; xx < img_width; xx++)
{
if (hits_horizontal[xx] > 0)
for (int yy = 0; yy < img_height; yy++)
{
if (hits_vertical[yy] > 0)
{
int intensity = 255 - (int)((ranges_horizontal[xx] + ranges_vertical[yy]) * 255 / img_width);
if (intensity < 0) intensity = 0;
for (int col=0;col<3;col++)
img[(((yy * img_width) + xx)*3) + col] = (Byte)intensity;
}
}
}
}
}
/// <summary>
/// update stereo feature tracking
/// </summary>
/// <param name="features">stereo features</param>
/// <param name="img_width">image width</param>
/// <param name="img_height">image height</param>
public void update(stereoFeatures features,
int img_width, int img_height)
{
stereoFeatures prev_features = null;
if (matched_index_list.Count == 0)
{
IDs = new sentienceTrackingFeature[history_steps, 1000];
for (int h = 0; h < history_steps; h++)
{
matched_index_list.Add(new int[1000]);
for (int i = 0; i < 1000; i++)
IDs[h, i] = null;
}
}
int prev_step = curr_step - 1;
if (prev_step < 0) prev_step += history_steps;
int[] matched_indexes = (int[])matched_index_list[curr_step];
int[] prev_matched_indexes = (int[])matched_index_list[prev_step];
if (prev_feature_list.Count > 1)
{
int prev_step2 = curr_step - 2;
if (prev_step2 < 0) prev_step2 += history_steps;
prev_features = (stereoFeatures)prev_feature_list[prev_step];
stereoFeatures prev_features2 = (stereoFeatures)prev_feature_list[prev_step2];
// predict the positions of features using the velocity values
for (int i = 0; i < prev_features.no_of_features; i++)
{
if (IDs[prev_step, i] != null)
{
if ((IDs[prev_step, i].vx != 0) || ((IDs[prev_step, i].vy != 0)))
{
prev_features.features[(i * 3)] = IDs[prev_step, i].predicted_x;
prev_features.features[(i * 3) + 1] = IDs[prev_step, i].predicted_y;
}
}
}
// match predicted feature positions with the currently observed ones
features.match(prev_features, matched_indexes,
img_width, img_height,
max_displacement_x, max_displacement_y, true);
// fill in any gaps
features.match(prev_features2, prev_matched_indexes,
img_width, img_height,
max_displacement_x, max_displacement_y, true);
for (int i = 0; i < features.no_of_features; i++)
{
// if the feature has been matched with a previous one update its ID
if (matched_indexes[i] > -1)
IDs[curr_step, i] = IDs[prev_step, matched_indexes[i]];
else
{
if (prev_matched_indexes[i] > -1)
IDs[curr_step, i] = IDs[prev_step2, prev_matched_indexes[i]];
else
IDs[curr_step, i] = null;
}
}
}
// update the persistence and average disparity for observed features
for (int i = 0; i < features.no_of_features; i++)
{
int x = (int)features.features[i * 3];
int y = (int)features.features[(i * 3) + 1];
float disp = features.features[(i * 3) + 2];
if (IDs[curr_step, i] != null)
{
int dx = (int)(x - IDs[curr_step, i].predicted_x);
if (dx < 0) dx = -dx;
IDs[curr_step, i].updatePosition(x, y);
float av_disparity = 0;
if (dx < 50)
{
// if the feature has not moved very much
IDs[curr_step, i].persistence++;
IDs[curr_step, i].total_disparity += disp;
av_disparity = IDs[curr_step, i].total_disparity / IDs[curr_step, i].persistence;
}
else
{
av_disparity = (IDs[curr_step, i].average_disparity * 0.9f) +
(disp * 0.1f);
}
if (av_disparity > 0)
{
float disp_change = (av_disparity - disp) / av_disparity;
float disp_confidence = 1.0f / (1.0f + (disp_change * disp_change));
int idx = matched_indexes[i];
if (idx > -1)
{
features.uncertainties[i] = prev_features.uncertainties[idx] -
((prev_features.uncertainties[idx] * disp_confidence * uncertainty_gain));
if (features.uncertainties[i] < 0.2f) features.uncertainties[i] = 0.2f;
features.uncertainties[i] *= 1.02f;
if (features.uncertainties[i] > 1) features.uncertainties[i] = 1;
}
}
IDs[curr_step, i].average_disparity = av_disparity;
features.features[(i * 3) + 2] = IDs[curr_step, i].average_disparity;
}
// create a new tracking feature
if (IDs[curr_step, i] == null)
{
IDs[curr_step, i] = new sentienceTrackingFeature(max_ID, x, y, disp);
max_ID++;
if (max_ID > 30000) max_ID = 1;
}
}
curr_step++;
if (curr_step >= history_steps) curr_step = 0;
if (prev_feature_list.Count < history_steps)
prev_feature_list.Add(features);
else
prev_feature_list[curr_step] = features;
//prev_features = features;
}
