/// <summary>
/// shows rays within the viewpoint from the front
/// </summary>
/// <param name="img"></param>
/// <param name="scale"></param>
public void showFront(Byte[] img, int img_width, int img_height, int scale,
Byte r, Byte g, Byte b, bool showProbabilities)
{
Byte rr, gg, bb;
int half_width = img_width / 2;
int half_height = img_height / 2;
float start_x, start_z, end_x, end_z;
//img.clear();
for (int cam = 0; cam < rays.Length; cam++)
{
for (int ry = 0; ry < rays[cam].Count; ry++)
{
evidenceRay ray = (evidenceRay)rays[cam][ry];
rr = r;
gg = g;
bb = b;
if (!showProbabilities)
{
start_x = ray.vertices[0].x * half_width / scale;
start_z = ray.vertices[0].z * half_height / scale;
end_x = ray.vertices[1].x * half_width / scale;
end_z = ray.vertices[1].z * half_height / scale;
util.drawLine(img, img_width, img_height, half_width + (int)start_x, half_height + (int)start_z, half_width + (int)end_x, half_height + (int)end_z, rr, gg, bb, 0, true);
}
else
{
float dx = ray.vertices[1].x - ray.vertices[0].x;
float dy = ray.vertices[1].y - ray.vertices[0].y;
float dz = ray.vertices[1].z - ray.vertices[0].z;
float sx = ray.vertices[0].x;
float sz = ray.vertices[0].z;
for (int l = 1; l < 50; l++)
{
float ex = ray.vertices[0].x + (dx * l / 50);
float ey = ray.vertices[0].y + (dy * l / 50);
float ez = ray.vertices[0].z + (dz * l / 50);
int intensity = (int)(ray.probability(ex, ey) * 255);
rr = (Byte)(r * intensity / 255);
gg = (Byte)(g * intensity / 255);
bb = (Byte)(b * intensity / 255);
start_x = sx * half_width / scale;
start_z = sz * half_height / scale;
end_x = ex * half_width / scale;
end_z = ez * half_height / scale;
util.drawLine(img, img_width, img_height, half_width + (int)start_x, half_height + (int)start_z, half_width + (int)end_x, half_height + (int)end_z, rr, gg, bb, 0, true);
sx = ex;
sz = ez;
}
}
}
}
}
/// <summary>
/// shows rays within the viewpoint from above
/// </summary>
/// <param name="img"></param>
/// <param name="scale"></param>
public void showAbove(Byte[] img, int img_width, int img_height, int scale,
Byte r, Byte g, Byte b, bool showProbabilities, int vertical_adjust,
bool lowUncertaintyOnly)
{
Byte rr, gg, bb;
int half_width = img_width / 2;
int half_height = (img_height / 2);
int vertAdjust = vertical_adjust * img_height / 100;
float start_x, start_y, end_x, end_y;
//img.clear();
for (int cam = 0; cam < rays.Length; cam++)
{
for (int ry = 0; ry < rays[cam].Count; ry++)
{
evidenceRay ray = (evidenceRay)rays[cam][ry];
if ((!lowUncertaintyOnly) || ((lowUncertaintyOnly) && (ray.uncertainty < 0.8f)))
{
rr = r;
gg = g;
bb = b;
if (!showProbabilities)
{
start_x = ray.vertices[0].x * half_width / scale;
start_y = ray.vertices[0].y * half_height / scale;
end_x = ray.vertices[1].x * half_width / scale;
end_y = ray.vertices[1].y * half_height / scale;
start_y -= vertAdjust;
end_y -= vertAdjust;
util.drawLine(img, img_width, img_height, half_width + (int)start_x, half_height + (int)start_y, half_width + (int)end_x, half_height + (int)end_y, rr, gg, bb, 0, true);
}
else
{
float dx = ray.vertices[1].x - ray.vertices[0].x;
float dy = ray.vertices[1].y - ray.vertices[0].y;
float sx = ray.vertices[0].x;
float sy = ray.vertices[0].y;
for (int l = 1; l < 50; l++)
{
float ex = ray.vertices[0].x + (dx * l / 50);
float ey = ray.vertices[0].y + (dy * l / 50);
int intensity = (int)(ray.probability(ex, ey) * 255);
rr = (Byte)(r * intensity / 255);
gg = (Byte)(g * intensity / 255);
bb = (Byte)(b * intensity / 255);
start_x = sx * half_width / scale;
start_y = sy * half_height / scale;
end_x = ex * half_width / scale;
end_y = ey * half_height / scale;
start_y -= vertAdjust;
end_y -= vertAdjust;
util.drawLine(img, img_width, img_height, half_width + (int)start_x, half_height + (int)start_y, half_width + (int)end_x, half_height + (int)end_y, rr, gg, bb, 0, true);
sx = ex;
sy = ey;
}
}
}
}
}
}
/// <summary>
/// returns a score indicating how closely matched the two viewpoints are
/// </summary>
/// <param name="other"></param>
/// <param name="intersects"></param>
/// <returns></returns>
public float matchingScore(viewpoint other, float separation_tollerance, int ray_thickness, ArrayList intersects)
{
float separation;
float score = 0;
pos3D intersectPos = new pos3D(0, 0, 0);
if (ray_thickness < 1)
{
ray_thickness = 1;
}
for (int cam1 = 0; cam1 < rays.Length; cam1++)
{
for (int ry1 = 0; ry1 < rays[cam1].Count; ry1++)
{
evidenceRay ray1 = (evidenceRay)rays[cam1][ry1];
int pan_index1 = ray1.pan_index - 1;
int pan_index2 = ray1.pan_index;
int pan_index3 = ray1.pan_index + 1;
if (pan_index1 < 0)
{
pan_index1 = evidenceRay.pan_steps - 1;
}
if (pan_index3 >= evidenceRay.pan_steps)
{
pan_index3 = 0;
}
int cam2 = cam1;
//for (int cam2 = 0; cam2 < other.rays.Length; cam2++)
{
for (int ry2 = 0; ry2 < other.rays[cam2].Count; ry2++)
{
evidenceRay ray2 = (evidenceRay)other.rays[cam2][ry2];
int pan_index4 = ray2.pan_index;
if ((pan_index1 == pan_index4) ||
(pan_index2 == pan_index4) ||
(pan_index3 == pan_index4))
{
//do these rays intersect
separation = 999;
if (stereoModel.raysOverlap(ray1, ray2, intersectPos, separation_tollerance, ray_thickness, ref separation))
{
float p1 = ray1.probability(intersectPos.x, intersectPos.y);
float p2 = ray2.probability(intersectPos.x, intersectPos.y);
float prob = (p1 * p2) + ((1.0f - p1) * (1.0f - p2));
if (intersects != null)
{
// add the intersection to the list
intersects.Add(intersectPos);
intersectPos = new pos3D(0, 0, 0);
}
//increment the matching score
score += 1.0f / (1.0f + ((1.0f - prob) * separation * separation));
//score += 1.0f / (1.0f + (separation * separation) + (ray2.length * ray1.length / 2));
//score += 1.0f / (1.0f + (separation * separation));
}
}
}
}
}
}
return(score);
}