/// <summary>
/// update the robot's map
/// </summary>
/// <param name="state">robot state</param>
private static void Update(ThreadMappingState state)
{
// object used for taking benchmark timings
stopwatch clock = new stopwatch();
clock.Start();
// update all current poses with the observed rays
state.motion.LocalGrid = state.grid;
state.motion.AddObservation(state.stereo_rays, false);
clock.Stop();
state.benchmark_observation_update = clock.time_elapsed_mS;
// what's the relative position of the robot inside the grid ?
pos3D relative_position = new pos3D(state.pose.x - state.grid.x, state.pose.y - state.grid.y, 0);
relative_position.pan = state.pose.pan - state.grid.pan;
clock.Start();
// garbage collect dead occupancy hypotheses
state.grid.GarbageCollect();
clock.Stop();
state.benchmark_garbage_collection = clock.time_elapsed_mS;
}
/// <summary>
/// save the occupancy grid to file and show some benchmarks
/// </summary>
/// <param name="filename"></param>
private void SaveGrid(String filename)
{
FileStream fp = new FileStream(filename, FileMode.Create);
BinaryWriter binfile = new BinaryWriter(fp);
lstBenchmarks.Items.Clear();
stopwatch grid_timer = new stopwatch();
grid_timer.Start();
Byte[] grid_data = sim.rob.SaveGrid();
grid_timer.Stop();
lstBenchmarks.Items.Add("Distillation time " + Convert.ToString(grid_timer.time_elapsed_mS) + " mS");
grid_timer.Start();
Byte[] compressed_grid_data =
AcedDeflator.Instance.Compress(grid_data, 0, grid_data.Length,
AcedCompressionLevel.Fast, 0, 0);
grid_timer.Stop();
lstBenchmarks.Items.Add("Compression ratio " + Convert.ToString(100-(int)(compressed_grid_data.Length * 100 / grid_data.Length)) + " %");
lstBenchmarks.Items.Add("Compression time " + Convert.ToString(grid_timer.time_elapsed_mS) + " mS");
grid_timer.Start();
binfile.Write(compressed_grid_data);
grid_timer.Stop();
lstBenchmarks.Items.Add("Disk write time " + Convert.ToString(grid_timer.time_elapsed_mS) + " mS");
binfile.Close();
fp.Close();
}
private void LoadGrid(String filename)
{
if (File.Exists(filename))
{
stopwatch grid_timer = new stopwatch();
// read the data into a byte array
grid_timer.Start();
Byte[] grid_data = File.ReadAllBytes(filename);
grid_timer.Stop();
lstBenchmarks.Items.Add("Disk read time " + Convert.ToString(grid_timer.time_elapsed_mS) + " mS");
// decompress the data
grid_timer.Start();
Byte[] decompressed_grid_data =
AcedInflator.Instance.Decompress(grid_data, 0, 0, 0);
grid_timer.Stop();
lstBenchmarks.Items.Add("Decompression time " + Convert.ToString(grid_timer.time_elapsed_mS) + " mS");
grid_timer.Start();
sim.rob.LoadGrid(decompressed_grid_data);
grid_timer.Stop();
lstBenchmarks.Items.Add("Creation time " + Convert.ToString(grid_timer.time_elapsed_mS) + " mS");
}
}
/// <summary>
/// load stereo images from a list
/// </summary>
/// <param name="images">list of images (byte arrays) in left/right order</param>
private void loadImages(List<byte[]> images)
{
stopwatch correspondence_time = new stopwatch();
correspondence_time.Start();
bool scanMatchesFound = false;
// create a set of threads
Thread[] correspondence_thread = new Thread[no_of_stereo_cameras];
List<ThreadStereoCorrespondenceState> activeThreads = new List<ThreadStereoCorrespondenceState>();
for (int stereo_camera_index = 0; stereo_camera_index < images.Count / 2; stereo_camera_index++)
{
byte[] left_image = images[stereo_camera_index * 2];
byte[] right_image = images[(stereo_camera_index * 2) + 1];
// create a state for the thread
ThreadStereoCorrespondenceState state = new ThreadStereoCorrespondenceState();
state.stereo_camera_index = stereo_camera_index;
state.correspondence = correspondence[stereo_camera_index];
state.correspondence_algorithm_type = correspondence_algorithm_type;
state.fullres_left = left_image;
state.fullres_right = right_image;
state.bytes_per_pixel = 3;
state.head = head;
state.no_of_stereo_features = inverseSensorModel.no_of_stereo_features;
state.EnableScanMatching = EnableScanMatching;
state.ScanMatchingMaxPanAngleChange = ScanMatchingMaxPanAngleChange;
state.ScanMatchingPanAngleEstimate = ScanMatchingPanAngleEstimate;
state.pan = pan;
state.active = true;
// add this state to the threads to be processed
ThreadStereoCorrespondence correspondence_update = new ThreadStereoCorrespondence(new WaitCallback(StereoCorrespondenceCallback), state);
correspondence_thread[stereo_camera_index] = new Thread(new ThreadStart(correspondence_update.Execute));
correspondence_thread[stereo_camera_index].Name = "stereo correspondence " + stereo_camera_index.ToString();
correspondence_thread[stereo_camera_index].Priority = ThreadPriority.AboveNormal;
activeThreads.Add(state);
}
// start all stereo correspondence threads
for (int th = 0; th < correspondence_thread.Length; th++)
correspondence_thread[th].Start();
// now sit back and wait for all threads to complete
while (activeThreads.Count > 0)
{
for (int th = activeThreads.Count - 1; th >= 0; th--)
{
// is this thread still active?
ThreadStereoCorrespondenceState state = (ThreadStereoCorrespondenceState)activeThreads[th];
if (!state.active)
{
if (state.scanMatchesFound) scanMatchesFound = true;
// remove from the list of active threads
activeThreads.RemoveAt(th);
}
}
}
// if no scan matches were found set the robots pan angle estimate accordingly
if (!scanMatchesFound) ScanMatchingPanAngleEstimate = scanMatching.NOT_MATCHED;
correspondence_time.Stop();
benchmark_stereo_correspondence = correspondence_time.time_elapsed_mS;
}
