/// <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>
/// match this feature set with another
/// </summary>
/// <param name="other">the other feature set</param>
/// <param name="match_indexes">array which contains the matching indexes</param>
/// <param name="img_width">image width</param>
/// <param name="img_height">image height</param>
/// <param name="max_displacement">maximum horizontal displacement as a percentage 1-100</param>
public void match(stereoFeatures other, int[] match_indexes,
int img_width, int img_height,
int max_displacement_x, int max_displacement_y,
bool overwrite)
{
matchTemporal(other.no_of_features, other.features,
no_of_features, features,
match_indexes, img_width, img_height,
max_displacement_x, max_displacement_y,
overwrite);
}
/// <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>
/// 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);
}
}
/// <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;
}
/// <summary>
/// update the colour of each feature from the given image
/// </summary>
/// <param name="camera_index">stereo camera index</param>
/// <param name="img">colour image</param>
public void updateFeatureColours(int camera_index, byte[] img)
{
// get the dimensions of the colour image
int wdth = calibration[camera_index].leftcam.image_width;
int hght = calibration[camera_index].leftcam.image_height;
for (int f = 0; f < features[camera_index].no_of_features; f++)
{
int idx = f * 3;
stereoFeatures feat = features[camera_index];
// get the coordinate of the feature within the image
int x = (int)feat.features[idx];
int y = (int)feat.features[idx + 1];
// correct the positions using the inverse calibration lookup
if (calibration[camera_index].leftcam.calibration_map_inverse != null)
{
//int x2 = calibration[camera_index].leftcam.calibration_map_inverse[x, y, 0];
//int y2 = calibration[camera_index].leftcam.calibration_map_inverse[x, y, 1];
//x = x2;
//y = y2;
}
// update the colour for this feature
// there's a little averaging here to help reduce noise
if (x > wdth - 2)
{
x = wdth - 2;
}
int n = ((y * wdth) + x) * 3;
feat.colour[f, 2] = (byte)((img[n] + img[n + 3]) / 2);
feat.colour[f, 1] = (byte)((img[n + 1] + img[n + 4]) / 2);
feat.colour[f, 0] = (byte)((img[n + 2] + img[n + 5]) / 2);
/*
* //testing colours
* int middle = x * hght / wdth;
* if (y > middle + (hght / 6))
* {
* feat.colour[f, 2] = (Byte)255;
* feat.colour[f, 1] = 0;
* feat.colour[f, 0] = 0;
* }
* else
* {
* if (y < middle - (hght / 6))
* {
* feat.colour[f, 2] = 0;
* feat.colour[f, 1] = 0;
* feat.colour[f, 0] = (Byte)255;
* }
* else
* {
* feat.colour[f, 2] = 0;
* feat.colour[f, 1] = (Byte)255;
* feat.colour[f, 0] = 0;
* }
* }
*/
}
}
/// <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;
}