public void RaysIntersection()
{
int debug_img_width = 640;
int debug_img_height = 480;
byte[] debug_img = new byte[debug_img_width * debug_img_height * 3];
for (int i = (debug_img_width * debug_img_height * 3)-1; i >= 0; i--)
debug_img[i] = 255;
Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
float min_x = float.MaxValue, max_x = float.MinValue;
float min_y = 0, max_y = float.MinValue;
float ray_uncertainty = 0.5f;
List<float> x_start = new List<float>();
List<float> y_start = new List<float>();
List<float> x_end = new List<float>();
List<float> y_end = new List<float>();
List<float> x_left = new List<float>();
List<float> y_left = new List<float>();
List<float> x_right = new List<float>();
List<float> y_right = new List<float>();
float disparity = 7;
float x1 = 640/2;
float x2 = x1 - disparity;
int grid_dimension = 2000;
float focal_length = 5;
float sensor_pixels_per_mm = 100;
float baseline = 100;
stereoModel inverseSensorModel = new stereoModel();
inverseSensorModel.image_width = 640;
inverseSensorModel.image_height = 480;
for (disparity = 15; disparity >= 15; disparity-=5)
{
for (int example = 0; example < 640 / 40; example++)
{
x1 = example * 40;
x2 = x1 - disparity;
float distance = stereoModel.DisparityToDistance(disparity, focal_length, sensor_pixels_per_mm, baseline);
float curr_x_start = 0;
float curr_y_start = 0;
float curr_x_end = 0;
float curr_y_end = 0;
float curr_x_left = 0;
float curr_y_left = 0;
float curr_x_right = 0;
float curr_y_right = 0;
inverseSensorModel.raysIntersection(
x1, x2,
grid_dimension, ray_uncertainty,
distance,
ref curr_x_start, ref curr_y_start,
ref curr_x_end, ref curr_y_end,
ref curr_x_left, ref curr_y_left,
ref curr_x_right, ref curr_y_right);
/*
curr_y_start = -curr_y_start;
curr_y_end = -curr_y_end;
curr_y_left = -curr_y_left;
curr_y_right = -curr_y_right;
*/
x_start.Add(curr_x_start);
y_start.Add(curr_y_start);
x_end.Add(curr_x_end);
y_end.Add(curr_y_end);
x_left.Add(curr_x_left);
y_left.Add(curr_y_left);
x_right.Add(curr_x_right);
y_right.Add(curr_y_right);
if (curr_x_end < min_x) min_x = curr_x_end;
if (curr_x_end > max_x) max_x = curr_x_end;
if (curr_x_left < min_x) min_x = curr_x_left;
if (curr_x_right > max_x) max_x = curr_x_right;
if (curr_y_start < min_y) min_y = curr_y_start;
if (curr_y_end > max_y) max_y = curr_y_end;
Console.WriteLine("curr_y_start: " + curr_y_start.ToString());
}
}
for (int i = 0; i < x_start.Count; i++)
{
float curr_x_start = (x_start[i] - min_x) * debug_img_width / (max_x - min_x);
float curr_y_start = (y_start[i] - min_y) * debug_img_height / (max_y - min_y);
float curr_x_end = (x_end[i] - min_x) * debug_img_width / (max_x - min_x);
float curr_y_end = (y_end[i] - min_y) * debug_img_height / (max_y - min_y);
float curr_x_left = (x_left[i] - min_x) * debug_img_width / (max_x - min_x);
float curr_y_left = (y_left[i] - min_y) * debug_img_height / (max_y - min_y);
float curr_x_right = (x_right[i] - min_x) * debug_img_width / (max_x - min_x);
float curr_y_right = (y_right[i] - min_y) * debug_img_height / (max_y - min_y);
curr_y_start = debug_img_height - 1 - curr_y_start;
curr_y_end = debug_img_height - 1 - curr_y_end;
curr_y_left = debug_img_height - 1 - curr_y_left;
curr_y_right = debug_img_height - 1 - curr_y_right;
//Console.WriteLine("max: " + max.ToString());
drawing.drawLine(debug_img, debug_img_width, debug_img_height, (int)curr_x_start, (int)curr_y_start, (int)curr_x_left, (int)curr_y_left, 0,0,0,0,false);
drawing.drawLine(debug_img, debug_img_width, debug_img_height, (int)curr_x_end, (int)curr_y_end, (int)curr_x_left, (int)curr_y_left, 0,0,0,0,false);
drawing.drawLine(debug_img, debug_img_width, debug_img_height, (int)curr_x_end, (int)curr_y_end, (int)curr_x_right, (int)curr_y_right, 0,0,0,0,false);
drawing.drawLine(debug_img, debug_img_width, debug_img_height, (int)curr_x_start, (int)curr_y_start, (int)curr_x_right, (int)curr_y_right, 0,0,0,0,false);
}
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_RaysIntersection.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
}
