public void CreateRay()
{
stereoModel model = new stereoModel();
evidenceRay ray = model.createRay(20, 10, 5, 0, 255, 255, 255);
Assert.IsNotNull(ray.vertices);
Assert.IsNotNull(ray.vertices[0]);
Assert.IsNotNull(ray.vertices[1]);
}
public void EvidenceRayRotation()
{
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);
int cellSize_mm = 32;
int image_width = 320;
int image_height = 240;
Console.WriteLine("Creating sensor models");
stereoModel inverseSensorModel = new stereoModel();
inverseSensorModel.createLookupTable(cellSize_mm, image_width, image_height);
// create a ray
float FOV_horizontal = 78 * (float)Math.PI / 180.0f;
inverseSensorModel.FOV_horizontal = FOV_horizontal;
inverseSensorModel.FOV_vertical = FOV_horizontal * image_height / image_width;
evidenceRay ray =
inverseSensorModel.createRay(
image_width / 2, image_height / 2, 4,
0, 255, 255, 255);
Assert.AreNotEqual(null, ray, "No ray was created");
Assert.AreNotEqual(null, ray.vertices, "No ray vertices were created");
pos3D[] start_vertices = (pos3D[])ray.vertices.Clone();
Console.WriteLine("x,y,z: " + start_vertices[0].x.ToString() + ", " + start_vertices[0].y.ToString() + ", " + start_vertices[0].z.ToString());
for (int i = 0; i < ray.vertices.Length; i++)
{
int j = i + 1;
if (j == ray.vertices.Length)
{
j = 0;
}
int x0 = (debug_img_width / 2) + (int)ray.vertices[i].x / 50;
int y0 = (debug_img_height / 2) + (int)ray.vertices[i].y / 50;
int x1 = (debug_img_width / 2) + (int)ray.vertices[j].x / 50;
int y1 = (debug_img_height / 2) + (int)ray.vertices[j].y / 50;
drawing.drawLine(debug_img, debug_img_width, debug_img_height, x0, y0, x1, y1, 0, 255, 0, 0, false);
}
float angle_degrees = 30;
float angle_radians = angle_degrees / 180.0f * (float)Math.PI;
pos3D rotation = new pos3D(0, 0, 0);
rotation.pan = angle_degrees;
ray.translateRotate(rotation);
Console.WriteLine("x,y,z: " + ray.vertices[0].x.ToString() + ", " + ray.vertices[0].y.ToString() + ", " + ray.vertices[0].z.ToString());
for (int i = 0; i < ray.vertices.Length; i++)
{
int j = i + 1;
if (j == ray.vertices.Length)
{
j = 0;
}
int x0 = (debug_img_width / 2) + (int)ray.vertices[i].x / 50;
int y0 = (debug_img_height / 2) + (int)ray.vertices[i].y / 50;
int x1 = (debug_img_width / 2) + (int)ray.vertices[j].x / 50;
int y1 = (debug_img_height / 2) + (int)ray.vertices[j].y / 50;
drawing.drawLine(debug_img, debug_img_width, debug_img_height, x0, y0, x1, y1, 255, 0, 0, 0, false);
}
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_EvidenceRayRotation.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
}