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; } } } } */ }