public void Show(classimage_mono img)
{
int prev_x = 0, prev_y = 0;
img.clear();
for (int i = 0; i < no_of_levels; i++)
{
int x = i * img.width / no_of_levels;
int y = img.height - (level[i] * (img.height * 8 / 10) / maxval);
if (i > 0)
{
img.drawLine(prev_x, prev_y, x, y, 0);
}
prev_x = x;
prev_y = y;
}
}
public void Show(classimage_mono img)
{
int prev_x = 0, prev_y = 0;
img.clear();
for (int i = 0; i < no_of_levels; i++)
{
int x = i * img.width / no_of_levels;
int y = img.height - (level[i] * (img.height * 8 / 10) / maxval);
if (i > 0)
{
img.drawLine(prev_x, prev_y, x, y, 0);
}
prev_x = x;
prev_y = y;
}
}
public void ShowTriangles(classimage_mono img)
{
int i, j;
for (i = 0; i < scene.selected_feature_list.Count; i++)
{
Feature f1 = (Feature)scene.selected_feature_list[i];
if ((f1.get_successful_measurement_flag()) &&
(f1.get_feature_measurement_model().fully_initialised_flag))
{
Vector measured_image_position1 = f1.get_z(); //measured position within the image (bottom up)
//scale into the given image dimensions
measured_image_position1[0] = (measured_image_position1[0] * img.width / CAMERA_WIDTH);
measured_image_position1[1] = (measured_image_position1[1] * img.height / CAMERA_HEIGHT);
for (j = i+1; j < scene.selected_feature_list.Count; j++)
{
Feature f2 = (Feature)scene.selected_feature_list[j];
if ((f2.get_successful_measurement_flag()) &&
(f2.get_feature_measurement_model().fully_initialised_flag))
{
Vector measured_image_position2 = f2.get_z(); //measured position within the image (bottom up)
//scale into the given image dimensions
measured_image_position2[0] = (measured_image_position2[0] * img.width / CAMERA_WIDTH);
measured_image_position2[1] = (measured_image_position2[1] * img.height / CAMERA_HEIGHT);
img.drawLine((int)measured_image_position1[0], (int)measured_image_position1[1],
(int)measured_image_position2[0], (int)measured_image_position2[1], 0);
}
}
}
}
}
public void ShowOverheadView(classimage_mono img, float world_dimension_x, float world_dimension_z)
{
int xx, yy, x, z, feature_size_x, feature_size_z,i,t,prev_x=0,prev_z=0;
Vector position;
float half_x = world_dimension_x/2;
float half_z = world_dimension_z/2;
img.clear();
//draw the camera trace
for (i = 0; i < TRACE_LENGTH; i++)
{
t = trace_index - i;
if (t < 0) t += TRACE_LENGTH;
if (!((camera_trace[t, 0] == 0) && (camera_trace[t, 1] == 0) && (camera_trace[t, 2] == 0)))
{
x = img.width-(int)((camera_trace[t,0] + half_x) / world_dimension_x * img.width);
z = img.height-(int)((camera_trace[t, 2] + half_z) / world_dimension_z * img.height);
if (i > 1)
{
img.drawLine(x, z, prev_x, prev_z, 0);
}
prev_x = x;
prev_z = z;
}
}
//draw the camera
ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();
threed_motion_model.func_xp(scene.get_xv());
threed_motion_model.func_r(threed_motion_model.get_xpRES());
threed_motion_model.func_q(threed_motion_model.get_xpRES());
//Quaternion qWO = threed_motion_model.get_qRES() * qRO;
Vector3D r_local = threed_motion_model.get_rRES();
position = scene.get_xv();
float scale_factor = 1.0f + ((position[1] + (world_dimension_x/2))/world_dimension_x);
feature_size_x = (int)(img.width * scale_factor / 50);
feature_size_z = (int)(img.height * scale_factor / 50);
x = (int)((r_local.GetX() + half_x) / world_dimension_x * img.width);
if ((x > 0) && (x < img.width))
{
z = (int)((r_local.GetZ() + half_z) / world_dimension_z * img.height);
if ((z > 0) && (z < img.height))
{
for (xx = x - feature_size_x; xx < x + feature_size_x; xx++)
{
if ((xx > 0) && (xx < img.width))
{
for (yy = z - feature_size_z; yy < z + feature_size_z; yy++)
{
if ((yy > 0) && (yy < img.height))
{
img.image[img.width - xx, img.height - yy] = 255;
}
}
}
}
}
}
//draw ranged features
/*
scale_factor = 1;
feature_size_x = (int)(img.width * scale_factor / 50);
feature_size_z = (int)(img.height * scale_factor / 50);
foreach (Feature v in ranged_features)
{
position = v.get_y();
x = (int)((position[0] + half_x) / world_dimension_x * img.width);
if ((x > 0) && (x < img.width))
{
z = (int)((position[2] + half_z) / world_dimension_z * img.height);
if ((z > 0) && (z < img.height))
{
for (xx = x - feature_size_x; xx < x + feature_size_x; xx++)
{
if ((xx > 0) && (xx < img.width))
{
for (yy = z - feature_size_z; yy < z + feature_size_z; yy++)
{
if ((yy > 0) && (yy < img.height))
{
img.image[img.width - xx, img.height - yy] = (Byte)v.colour[0];
}
}
}
}
}
}
}
*/
//draw the features
scale_factor = 1;
feature_size_x = (int)(img.width * scale_factor / 50);
feature_size_z = (int)(img.height * scale_factor / 50);
foreach (Feature f in scene.feature_list)
{
//if (f.get_fully_initialised_feature_measurement_model() != null)
if (f.get_successful_measurement_flag())
{
position = f.get_y();
x = (int)((position[0] + half_x) / world_dimension_x * img.width);
if ((x > 0) && (x < img.width))
{
z = (int)((position[2] + half_z) / world_dimension_z * img.height);
if ((z > 0) && (z < img.height))
{
for (xx = x - feature_size_x; xx < x + feature_size_x; xx++)
{
if ((xx > 0) && (xx < img.width))
{
for (yy = z - feature_size_z; yy < z + feature_size_z; yy++)
{
if ((yy > 0) && (yy < img.height))
{
img.image[img.width - xx, img.height - yy] = 255;
}
}
}
}
}
}
}
}
//show the particles associated with partially initialised features
/*
foreach (FeatureInitInfo feat in scene.get_feature_init_info_vector())
{
foreach (Particle p in feat.particle_vector)
{
position = p.get_h;
if (position != null)
{
x = (int)((position[0] + half_x) / world_dimension_x * img.width);
z = (int)((position[2] + half_z) / world_dimension_z * img.height);
if ((x > 0) && (x < img.width) && (z > 0) && (z < img.height))
{
for (c = 0; c < 3; c++)
img.image[img.width - x, img.height - z, c] = 255;
}
}
}
}
*/
}
