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