/// <summary>
/// calculate stereo correspondence
/// </summary>
/// <param name="state">stereo correspondence state</param>
private static void Update(ThreadStereoCorrespondenceState state)
{
// set the number of features we need, later to be turned into rays
state.correspondence.setRequiredFeatures(state.no_of_stereo_features);
// set the calibration data for this camera
state.correspondence.setCalibration(state.head.calibration[state.stereo_camera_index]);
// run the correspondence algorithm
state.correspondence.loadRawImages(
state.stereo_camera_index,
state.fullres_left,
state.fullres_right,
state.head,
state.no_of_stereo_features,
state.bytes_per_pixel,
state.correspondence_algorithm_type);
// perform scan matching for forwards or rearwards looking cameras
float pan_angle = state.head.pan + state.head.cameraPosition[state.stereo_camera_index].pan;
if ((pan_angle == 0) || (pan_angle == (float)Math.PI))
{
// create a scan matching object if needed
if (state.head.scanmatch[state.stereo_camera_index] == null)
state.head.scanmatch[state.stereo_camera_index] = new scanMatching();
if (state.EnableScanMatching)
{
// perform scan matching
state.head.scanmatch[state.stereo_camera_index].update(
state.correspondence.getRectifiedImage(true),
state.head.calibration[state.stereo_camera_index].leftcam.image_width,
state.head.calibration[state.stereo_camera_index].leftcam.image_height,
state.head.calibration[state.stereo_camera_index].leftcam.camera_FOV_degrees * (float)Math.PI / 180.0f,
state.head.cameraPosition[state.stereo_camera_index].roll,
state.ScanMatchingMaxPanAngleChange * (float)Math.PI / 180.0f);
if (state.head.scanmatch[state.stereo_camera_index].pan_angle_change != scanMatching.NOT_MATCHED)
{
state.scanMatchesFound = true;
if (state.ScanMatchingPanAngleEstimate == scanMatching.NOT_MATCHED)
{
// if this is the first time a match has been found
// use the current pan estimate
state.ScanMatchingPanAngleEstimate = state.pan;
}
else
{
if (pan_angle == 0)
// forward facing camera
state.ScanMatchingPanAngleEstimate -= state.head.scanmatch[state.stereo_camera_index].pan_angle_change;
else
// rearward facing camera
state.ScanMatchingPanAngleEstimate += state.head.scanmatch[state.stereo_camera_index].pan_angle_change;
}
}
}
}
else state.head.scanmatch[state.stereo_camera_index] = null;
}
/// <summary>
/// ThreadStart delegate
/// </summary>
public void Execute()
{
ThreadStereoCorrespondenceState state = (ThreadStereoCorrespondenceState)_data;
Update(state);
state.active = false;
_callback(_data);
}
/// <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;
}
/// <summary>
/// calculate stereo correspondence
/// </summary>
/// <param name="state">stereo correspondence state</param>
private static void Update(ThreadStereoCorrespondenceState state)
{
// set the number of features we need, later to be turned into rays
state.correspondence.setRequiredFeatures(state.no_of_stereo_features);
// set the calibration data for this camera
state.correspondence.setCalibration(state.head.calibration[state.stereo_camera_index]);
// run the correspondence algorithm
state.correspondence.loadRawImages(
state.stereo_camera_index,
state.fullres_left,
state.fullres_right,
state.head,
state.no_of_stereo_features,
state.bytes_per_pixel,
state.correspondence_algorithm_type);
// perform scan matching for forwards or rearwards looking cameras
float pan_angle = state.head.pan + state.head.cameraPosition[state.stereo_camera_index].pan;
if ((pan_angle == 0) || (pan_angle == (float)Math.PI))
{
// create a scan matching object if needed
if (state.head.scanmatch[state.stereo_camera_index] == null)
{
state.head.scanmatch[state.stereo_camera_index] = new scanMatching();
}
if (state.EnableScanMatching)
{
// perform scan matching
state.head.scanmatch[state.stereo_camera_index].update(
state.correspondence.getRectifiedImage(true),
state.head.calibration[state.stereo_camera_index].leftcam.image_width,
state.head.calibration[state.stereo_camera_index].leftcam.image_height,
state.head.calibration[state.stereo_camera_index].leftcam.camera_FOV_degrees * (float)Math.PI / 180.0f,
state.head.cameraPosition[state.stereo_camera_index].roll,
state.ScanMatchingMaxPanAngleChange * (float)Math.PI / 180.0f);
if (state.head.scanmatch[state.stereo_camera_index].pan_angle_change != scanMatching.NOT_MATCHED)
{
state.scanMatchesFound = true;
if (state.ScanMatchingPanAngleEstimate == scanMatching.NOT_MATCHED)
{
// if this is the first time a match has been found
// use the current pan estimate
state.ScanMatchingPanAngleEstimate = state.pan;
}
else
{
if (pan_angle == 0)
{
// forward facing camera
state.ScanMatchingPanAngleEstimate -= state.head.scanmatch[state.stereo_camera_index].pan_angle_change;
}
else
{
// rearward facing camera
state.ScanMatchingPanAngleEstimate += state.head.scanmatch[state.stereo_camera_index].pan_angle_change;
}
}
}
}
}
else
{
state.head.scanmatch[state.stereo_camera_index] = null;
}
}
