/// <summary>
/// load a new image from the camera
/// </summary>
/// <param name="imageptr">camera image</param>
public virtual void load_new_image(classimage_mono imageptr, classimage imageptr_colour, classimage_mono outputimageptr, bool show_ellipses)
{
image = imageptr;
image_colour = imageptr_colour;
outputimage = outputimageptr;
this.show_ellipses = show_ellipses;
}
//------------------------------------------------------------------------------------------------------------------------
//update from another image
//------------------------------------------------------------------------------------------------------------------------
public void updateFromImage(classimage img)
{
int x, y, xx, yy;
for (x = 0; x < width; x++)
{
xx = (x * img.width) / width;
for (y = 0; y < height; y++)
{
yy = (y * img.height) / height;
image[x, y] = img.image[xx, yy, 0];
}
}
}
//---------------------------------------------------------------------------------------------
//copy from an image
//---------------------------------------------------------------------------------------------
public void copyImage(classimage source_img)
{
int x, y;
Byte v;
for (x = 0; x < width; x++)
{
for (y = 0; y < height; y++)
{
v = (Byte)((source_img.image[x, y, 0] +
source_img.image[x, y, 1] +
source_img.image[x, y, 2]) / 3);
image[x, y] = v;
}
}
}
/// <summary>
/// show the histogram in an image object
/// </summary>
/// <param name="img"></param>
public void Show(classimage 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, 255, 0, 0);
}
prev_x = x;
prev_y = y;
}
}
//------------------------------------------------------------------------------------------------------------------------
// sample the given image within the given bounding box
//------------------------------------------------------------------------------------------------------------------------
public void sampleFromImage(classimage example_img, int tx, int ty, int bx, int by)
{
int x, y, xx, yy, dx, dy;
dx = bx - tx;
dy = by - ty;
for (x = 0; x < width; x++)
{
xx = tx + ((x * dx) / width);
for (y = 0; y < height; y++)
{
yy = ty + ((y * dy) / height);
image[x, y] = example_img.image[xx, yy, 0];
}
}
//updateIntegralImage();
}
private void timUpdate_Tick(object sender, EventArgs e)
{
bool image_loaded;
int wdth;
int hght;
if ((left_imageloaded) || (left_camera_running))
{
//get images from the two cameras
captureCameraImages();
//record depth images if necessary
//recordDepthImages();
if ((!test.frame_rate_warning) || (!test.enable_mapping))
{
if (!test.enable_mapping)
cmdBeginTracking.Visible = true;
else
cmdBeginTracking.Visible = false;
lblTracking.Visible = !cmdBeginTracking.Visible;
lblFrameRateWarning.Visible = false;
}
else
{
lblTracking.Visible = false;
lblFrameRateWarning.Visible = true;
}
if (simulation_mode)
{
//update from loaded images
image_loaded = loadVideoFrame(video_path, video_frame_number);
if (image_loaded)
{
wdth = picLeftImage.Image.Width;
hght = picLeftImage.Image.Height;
if (global_variables.left_bmp == null)
{
global_variables.left_bmp = new Byte[wdth * hght * 3];
raw_image = new classimage_mono();
raw_image.createImage(wdth, hght);
raw_image_colour = new classimage();
raw_image_colour.createImage(wdth, hght);
}
updatebitmap((Bitmap)picLeftImage.Image, global_variables.left_bmp);
raw_image_colour.updateFromBitmap(global_variables.left_bmp, 1, wdth, hght);
raw_image.copyImage(raw_image_colour);
if (!outputInitialised)
{
picOutput1.Image = new Bitmap(global_variables.standard_width, global_variables.standard_height, PixelFormat.Format24bppRgb);
background_bitmap = new Bitmap(global_variables.standard_width, global_variables.standard_height, PixelFormat.Format24bppRgb);
disp_bmp_data = new Byte[global_variables.standard_width * global_variables.standard_height * 3];
output_image = new classimage_mono();
output_image.createImage(global_variables.standard_width,global_variables.standard_height);
outputInitialised = true;
}
output_image.updateFromImage(raw_image);
txtCaptureTime.Text = Convert.ToString(endStopWatch());
beginStopWatch();
test.GoOneStep(raw_image, raw_image_colour, output_image);
txtProcessingTime.Text = Convert.ToString(endStopWatch());
txtfps.Text = Convert.ToString(test.frames_per_second);
txtSpeed.Text = Convert.ToString(test.speed);
beginStopWatch();
test.ShowFeatures(output_image, selected_display);
output_image.saveToBitmap(disp_bmp_data, global_variables.standard_width, global_variables.standard_height);
// either show the output image in the picturebox, or transfer it
// to a separate background bitmap for use during 3D rendering
if ((selected_display != MonoSLAM.DISPLAY_AUGMENTED_REALITY) ||
((selected_display == MonoSLAM.DISPLAY_AUGMENTED_REALITY) && (!(initialised_3D && test.enable_mapping))))
{
updatebitmap(disp_bmp_data, (Bitmap)picOutput1.Image);
picOutput1.Refresh();
}
else
{
updatebitmap(disp_bmp_data, background_bitmap);
}
// render 3D objects if augmented reality is enabled
render3D(d3dDevice);
if (!single_step) video_frame_number++;
}
else
{
video_frame_number = 1;
test.init();
}
}
else
{
//update from cameras
if (captureState[0] == 2)
{
captureState[0] = 0;
captureState[1] = 0;
wdth = picLeftImage.Image.Width;
hght = picLeftImage.Image.Height;
if (raw_image == null)
{
// initialise raw image object
raw_image = new classimage_mono();
raw_image.createImage(wdth, hght);
raw_image_colour = new classimage();
raw_image_colour.createImage(wdth, hght);
}
// shove the bitmap from the camera into an image object
raw_image_colour.updateFromBitmap(global_variables.left_bmp, 1, wdth, hght);
raw_image.copyImage(raw_image_colour);
if (!outputInitialised)
{
// initialise image bitmaps
picOutput1.Image = new Bitmap(global_variables.standard_width, global_variables.standard_height, PixelFormat.Format24bppRgb);
background_bitmap = new Bitmap(global_variables.standard_width, global_variables.standard_height, PixelFormat.Format24bppRgb);
disp_bmp_data = new Byte[global_variables.standard_width * global_variables.standard_height * 3];
output_image = new classimage_mono();
output_image.createImage(global_variables.standard_width, global_variables.standard_height);
outputInitialised = true;
}
output_image.updateFromImage(raw_image);
txtCaptureTime.Text = Convert.ToString(endStopWatch());
beginStopWatch();
// one processing cycle update
test.GoOneStep(raw_image, raw_image_colour, output_image);
//show info
txtProcessingTime.Text = Convert.ToString(endStopWatch());
txtfps.Text = Convert.ToString(test.frames_per_second);
txtSpeed.Text = Convert.ToString(test.speed);
beginStopWatch();
// show crosshair features or overhead map
test.ShowFeatures(output_image, selected_display);
output_image.saveToBitmap(disp_bmp_data, global_variables.standard_width, global_variables.standard_height);
// either show the output image in the picturebox, or transfer it
// to a separate background bitmap for use during 3D rendering
if ((selected_display != MonoSLAM.DISPLAY_AUGMENTED_REALITY) ||
((selected_display == MonoSLAM.DISPLAY_AUGMENTED_REALITY) && (!(initialised_3D && test.enable_mapping))))
{
updatebitmap(disp_bmp_data, (Bitmap)picOutput1.Image);
picOutput1.Refresh();
}
else
{
updatebitmap(disp_bmp_data, background_bitmap);
}
// render 3D objects if augmented reality is enabled
render3D(d3dDevice);
if (reset)
{
test.init();
reset = false;
}
}
}
}
}
/// <summary>
/// one update
/// </summary>
/// <param name="img"></param>
public void GoOneStep(classimage_mono img, classimage img_colour, classimage_mono output_img)
{
float delta_t;
MAXIMUM_ANGLE_DIFFERENCE = 3.1415927f * (field_of_vision / 2) / 180.0f;
//set calibration parameters
if (calibrating)
{
monoslaminterface.set_camera_calibration(9e-06f, lens_distortion, lens_distortion, centre_of__distortion_x, centre_of__distortion_y, 1);
}
//load in the current raw image
Robot r = monoslaminterface.GetRobot();
r.calibrating = (!enable_mapping) | calibrating;
r.load_new_image(img, img_colour, output_img, show_ellipses);
stopwatch.Stop();
//get the elapsed time in seconds
delta_t = stopwatch.ElapsedMilliseconds / 1000.0f;
//if using the simulation set the frame rate at a fixed value
if (simulation_mode)
{
delta_t = 1.0f / 20.0f;
}
if (delta_t > 0)
{
frames_per_second = (int)((1.0f / delta_t) * 10) / 10.0f;
//report if its taking a long time
frame_rate_warning = false;
if (delta_t > 0.2)
{
frame_rate_warning = true;
Debug.WriteLine("Time between frames (sec): " + Convert.ToString(delta_t));
}
//update the state of the system
monoslaminterface.GoOneStep(delta_t, enable_mapping);
}
stopwatch.Reset();
stopwatch.Start();
speed = (int)(monoslaminterface.Speed() * 100) / 100.0f;
if ((enable_mapping) && (monoslaminterface.number_of_matched_features < 2))
{
enable_mapping = false;
init();
}
if ((!enable_mapping) && (monoslaminterface.number_of_matched_features >= 3) && (!calibrating))
{
registered_seconds += delta_t;
if (registered_seconds > 1)
{
Debug.WriteLine("Ready to go");
enable_mapping = true;
registered_seconds = 0;
}
}
else
{
if (registered_seconds > 0)
{
Debug.WriteLine("Waiting for initial registration");
}
registered_seconds = 0;
}
}
/// <summary>
/// one update
/// </summary>
/// <param name="img"></param>
public void GoOneStep(classimage_mono img, classimage img_colour, classimage_mono output_img)
{
float delta_t;
MAXIMUM_ANGLE_DIFFERENCE = 3.1415927f * (field_of_vision/2) / 180.0f;
//set calibration parameters
if (calibrating)
monoslaminterface.set_camera_calibration(9e-06f, lens_distortion, lens_distortion, centre_of__distortion_x, centre_of__distortion_y, 1);
//load in the current raw image
Robot r = monoslaminterface.GetRobot();
r.calibrating = (!enable_mapping) | calibrating;
r.load_new_image(img, img_colour, output_img, show_ellipses);
stopwatch.Stop();
//get the elapsed time in seconds
delta_t = stopwatch.ElapsedMilliseconds / 1000.0f;
//if using the simulation set the frame rate at a fixed value
if (simulation_mode) delta_t = 1.0f / 20.0f;
if (delta_t > 0)
{
frames_per_second = (int)((1.0f / delta_t) * 10) / 10.0f;
//report if its taking a long time
frame_rate_warning = false;
if (delta_t > 0.2)
{
frame_rate_warning = true;
Debug.WriteLine("Time between frames (sec): " + Convert.ToString(delta_t));
}
//update the state of the system
monoslaminterface.GoOneStep(delta_t, enable_mapping);
}
stopwatch.Reset();
stopwatch.Start();
speed = (int)(monoslaminterface.Speed() * 100) / 100.0f;
if ((enable_mapping) && (monoslaminterface.number_of_matched_features < 2))
{
enable_mapping = false;
init();
}
if ((!enable_mapping) && (monoslaminterface.number_of_matched_features >= 3) && (!calibrating))
{
registered_seconds += delta_t;
if (registered_seconds > 1)
{
Debug.WriteLine("Ready to go");
enable_mapping = true;
registered_seconds = 0;
}
}
else
{
if (registered_seconds > 0) Debug.WriteLine("Waiting for initial registration");
registered_seconds = 0;
}
}
