Skip to content

Commit

Permalink
[Sick VisionaryT] update loop (#158)
Browse files Browse the repository at this point in the history 
* fix minimal sample

* implement thread that constantly fetches frames from the camera
  • Loading branch information
@jangernert @mischastik
jangernert authored and mischastik committed Jul 25, 2018
1 parent 4ccc238 commit a5acf22
Show file tree
Hide file tree
Showing 3 changed files with 131 additions and 85 deletions.
164 changes: 105 additions & 59 deletions BetaCameras/Sick.VisionaryT/VisionaryT.cs
View file
Expand Up @@ -2,8 +2,10 @@
// MetriCam 2 is licensed under the MIT license. See License.txt for full license text.

using System;
using System.Threading;
using MetriCam2.Cameras.Internal.Sick;
using Metrilus.Util;
using MetriCam2.Exceptions;

namespace MetriCam2.Cameras
{
Expand All @@ -14,6 +16,12 @@ public class VisionaryT : Camera
{
#region Private Variables
private const int NumFrameRetries = 3;
private Thread _updateThread;
private CancellationTokenSource _cancelUpdateThreadSource = new CancellationTokenSource();
private AutoResetEvent _frameAvailable = new AutoResetEvent(false);
private object _frontLock = new object();
private object _backLock = new object();
private string _updateThreadError = null; // Hand errors from update loop thread to application thread

// ipAddress to connect to
private string ipAddress;
Expand All @@ -22,7 +30,8 @@ public class VisionaryT : Camera
private Control _control;

// image data contains information about the current frame e.g. width and height
private FrameData _frameData;
private FrameData _frontFrameData;
private FrameData _backFrameData;
// camera properties
private int width;
private int height;
Expand Down Expand Up @@ -145,11 +154,13 @@ private ParamDesc<int> HeightDesc
public VisionaryT()
: base()
{
ipAddress = "";
device = null;
_frameData = null;
width = 0;
height = 0;
ipAddress = "";
device = null;
_frontFrameData = null;
_backFrameData = null;
_updateThread = new Thread(new ThreadStart(UpdateLoop));
width = 0;
height = 0;
}
#endregion

Expand Down Expand Up @@ -183,6 +194,8 @@ protected override void LoadAllAvailableChannels()
/// <remarks>This method is implicitly called by <see cref="Camera.Connect"/> inside a camera lock.</remarks>
protected override void ConnectImpl()
{
_updateThreadError = null;

if (string.IsNullOrWhiteSpace(ipAddress))
{
string msg = string.Format("IP address is not set. It must be set before connecting.");
Expand All @@ -199,8 +212,7 @@ protected override void ConnectImpl()
ActivateChannel(ChannelNames.Intensity);
SelectChannel(ChannelNames.Intensity);

// Call update once
UpdateImpl();
_updateThread.Start();
}

/// <summary>
Expand All @@ -209,6 +221,8 @@ protected override void ConnectImpl()
/// <remarks>This method is implicitly called by <see cref="Camera.Disconnect"/> inside a camera lock.</remarks>
protected override void DisconnectImpl()
{
_cancelUpdateThreadSource.Cancel();
_updateThread.Join();
_control.Close();
_control = null;
device.Disconnect();
Expand All @@ -221,27 +235,24 @@ protected override void DisconnectImpl()
/// <remarks>This method is implicitly called by <see cref="Camera.Update"/> inside a camera lock.</remarks>
protected override void UpdateImpl()
{
int consecutiveFailCounter = 0;
while (true)
_frameAvailable.WaitOne();

if (null != _updateThreadError)
{
try
{
byte[] imageBuffer = device.GetFrameData();
_frameData = new FrameData(imageBuffer, this, log);
break;
}
catch
{
consecutiveFailCounter++;
if (consecutiveFailCounter > NumFrameRetries)
{
throw;
}
}
Disconnect();
throw new ImageAcquisitionFailedException(_updateThreadError);
}

width = _frameData.Width;
height = _frameData.Height;
lock (_backLock)
lock (_frontLock)
{
_frontFrameData = _backFrameData;
_backFrameData = null;
_frameAvailable.Reset();
}

width = _frontFrameData.Width;
height = _frontFrameData.Height;
}

/// <summary>Computes (image) data for a given channel.</summary>
Expand Down Expand Up @@ -270,24 +281,59 @@ protected override CameraImage CalcChannelImpl(string channelName)
#endregion

#region Internal Methods
private void UpdateLoop()
{
int consecutiveFailCounter = 0;
while (!_cancelUpdateThreadSource.Token.IsCancellationRequested)
{
try
{
lock (_backLock)
{
byte[] imageBuffer = device.GetFrameData();
_backFrameData = new FrameData(imageBuffer, this, log);
_frameAvailable.Set();
}
}
catch
{
consecutiveFailCounter++;
if (consecutiveFailCounter > NumFrameRetries)
{
string msg = $"{Name}: Receive failed more than {NumFrameRetries} times in a row. Shutting down update loop.";
log.Error(msg);
_updateThreadError = msg;
_frameAvailable.Set();
break;
}
}

// reset counter after sucessfull fetch
consecutiveFailCounter = 0;
} // while

_cancelUpdateThreadSource = new CancellationTokenSource();
}


/// <summary>
/// Calculates the intensity channel.
/// </summary>
/// <returns>Intensity image</returns>
private FloatCameraImage CalcIntensity()
{
FloatCameraImage result;
lock (cameraLock)
lock (_frontLock)
{
result = new FloatCameraImage(_frameData.Width, _frameData.Height);
result.TimeStamp = (long)_frameData.TimeStamp;
int start = _frameData.IntensityStartOffset;
for (int i = 0; i < _frameData.Height; ++i)
result = new FloatCameraImage(_frontFrameData.Width, _frontFrameData.Height);
result.TimeStamp = (long)_frontFrameData.TimeStamp;
int start = _frontFrameData.IntensityStartOffset;
for (int i = 0; i < _frontFrameData.Height; ++i)
{
for (int j = 0; j < _frameData.Width; ++j)
for (int j = 0; j < _frontFrameData.Width; ++j)
{
// take two bytes and create integer (little endian)
uint value = (uint)_frameData.ImageBuffer[start + 1] << 8 | (uint)_frameData.ImageBuffer[start + 0];
uint value = (uint)_frontFrameData.ImageBuffer[start + 1] << 8 | (uint)_frontFrameData.ImageBuffer[start + 0];
result[i, j] = (float)value;
start += 2;
}
Expand All @@ -304,17 +350,17 @@ private FloatCameraImage CalcDistance()
{
FloatCameraImage result;
// FIXME: distance calculation
lock (cameraLock)
lock (_frontLock)
{
result = new FloatCameraImage(_frameData.Width, _frameData.Height);
result.TimeStamp = (long)_frameData.TimeStamp;
int start = _frameData.DistanceStartOffset;
for (int i = 0; i < _frameData.Height; ++i)
result = new FloatCameraImage(_frontFrameData.Width, _frontFrameData.Height);
result.TimeStamp = (long)_frontFrameData.TimeStamp;
int start = _frontFrameData.DistanceStartOffset;
for (int i = 0; i < _frontFrameData.Height; ++i)
{
for (int j = 0; j < _frameData.Width; ++j)
for (int j = 0; j < _frontFrameData.Width; ++j)
{
// take two bytes and create integer (little endian)
uint value = (uint)_frameData.ImageBuffer[start + 1] << 8 | (uint)_frameData.ImageBuffer[start + 0];
uint value = (uint)_frontFrameData.ImageBuffer[start + 1] << 8 | (uint)_frontFrameData.ImageBuffer[start + 0];
result[i, j] = (float)value * 0.001f;
start += 2;
}
Expand All @@ -330,17 +376,17 @@ private FloatCameraImage CalcDistance()
private UShortCameraImage CalcRawConfidenceMap()
{
UShortCameraImage result;
lock (cameraLock)
lock (_frontLock)
{
result = new UShortCameraImage(_frameData.Width, _frameData.Height);
result.TimeStamp = (long)_frameData.TimeStamp;
int start = _frameData.ConfidenceStartOffset;
for (int i = 0; i < _frameData.Height; ++i)
result = new UShortCameraImage(_frontFrameData.Width, _frontFrameData.Height);
result.TimeStamp = (long)_frontFrameData.TimeStamp;
int start = _frontFrameData.ConfidenceStartOffset;
for (int i = 0; i < _frontFrameData.Height; ++i)
{
for (int j = 0; j < _frameData.Width; ++j)
for (int j = 0; j < _frontFrameData.Width; ++j)
{
// take two bytes and create integer (little endian)
result[i, j] = (ushort)((ushort)_frameData.ImageBuffer[start + 1] << 8 | (ushort)_frameData.ImageBuffer[start + 0]);
result[i, j] = (ushort)((ushort)_frontFrameData.ImageBuffer[start + 1] << 8 | (ushort)_frontFrameData.ImageBuffer[start + 0]);
start += 2;
}
}
Expand Down Expand Up @@ -379,24 +425,24 @@ private FloatCameraImage CalcConfidenceMap(UShortCameraImage rawConfidenceMap)
private Point3fCameraImage Calc3D()
{
Point3fCameraImage result;
lock (cameraLock)
lock (_frontLock)
{
result = new Point3fCameraImage(_frameData.Width, _frameData.Height);
result.TimeStamp = (long)_frameData.TimeStamp;
result = new Point3fCameraImage(_frontFrameData.Width, _frontFrameData.Height);
result.TimeStamp = (long)_frontFrameData.TimeStamp;

float cx = _frameData.CX;
float cy = _frameData.CY;
float fx = _frameData.FX;
float fy = _frameData.FY;
float k1 = _frameData.K1;
float k2 = _frameData.K2;
float f2rc = _frameData.F2RC;
float cx = _frontFrameData.CX;
float cy = _frontFrameData.CY;
float fx = _frontFrameData.FX;
float fy = _frontFrameData.FY;
float k1 = _frontFrameData.K1;
float k2 = _frontFrameData.K2;
float f2rc = _frontFrameData.F2RC;

FloatCameraImage distances = CalcDistance();

for (int i = 0; i < _frameData.Height; ++i)
for (int i = 0; i < _frontFrameData.Height; ++i)
{
for (int j = 0; j < _frameData.Width; ++j)
for (int j = 0; j < _frontFrameData.Width; ++j)
{
int depth = (int)(distances[i, j] * 1000);

Expand Down
6 changes: 3 additions & 3 deletions Samples/MinimalSample/MinimalSample.csproj
View file
Expand Up @@ -87,9 +87,9 @@
<Compile Include="Properties\AssemblyInfo.cs" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\..\BetaCameras\RealSense2\RealSense2.csproj">
<Project>{3032e9b1-f572-4ecb-906a-f7a2b45ef125}</Project>
<Name>RealSense2</Name>
<ProjectReference Include="..\..\BetaCameras\Sick.VisionaryT\Sick.VisionaryT.csproj">
<Project>{de18d143-c624-4941-b8a1-0f91262d3350}</Project>
<Name>Sick.VisionaryT</Name>
</ProjectReference>
<ProjectReference Include="..\..\MetriCam2\MetriCam2.csproj">
<Project>{342f049e-668b-4351-b21d-75e4bef5a1e4}</Project>
Expand Down
46 changes: 23 additions & 23 deletions Samples/MinimalSample/Program.cs
View file
Expand Up @@ -22,10 +22,10 @@ static void Main(string[] args)
Console.WriteLine("------------------------------------------");

// Create camera object
RealSense2 camera;
VisionaryT camera;
try
{
camera = new RealSense2();
camera = new VisionaryT();
}
catch (Exception e)
{
Expand All @@ -36,36 +36,36 @@ static void Main(string[] args)
return;
}

camera.IPAddress = "192.168.1.10";

// Connect, get one frame, disconnect
Console.WriteLine("Connecting camera");
camera.Connect();

//while (true) { }

Console.WriteLine("Fetching one frame");
camera.Update();
for(int i = 0; i < 100; i++)
{
camera.Update();

ProjectiveTransformationZhang ptrans = (ProjectiveTransformationZhang)camera.GetIntrinsics(ChannelNames.ZImage);
try
{
Console.WriteLine("Accessing distance data");
FloatCameraImage distancesData = (FloatCameraImage)camera.CalcChannel(ChannelNames.Intensity);
FloatImage fImg = new FloatImage(ref distancesData);
string tmp = fImg.ShowInDebug;
}
catch (ArgumentException ex)
{
Console.WriteLine(String.Format("Error getting channel {0}: {1}.", ChannelNames.ZImage, ex.Message));
}
}



try
{
Console.WriteLine("Accessing color data");
ColorCameraImage img = (ColorCameraImage)camera.CalcChannel(ChannelNames.Color);
Bitmap rgbBitmapData = img.ToBitmap();
}
catch (ArgumentException ex)
{
Console.WriteLine(String.Format("Error getting channel {0}: {1}.", ChannelNames.Color, ex.Message));
}

try
{
Console.WriteLine("Accessing distance data");
FloatCameraImage distancesData = (FloatCameraImage)camera.CalcChannel(ChannelNames.ZImage);
}
catch (ArgumentException ex)
{
Console.WriteLine(String.Format("Error getting channel {0}: {1}.", ChannelNames.ZImage, ex.Message));
}


Console.WriteLine("Disconnecting camera");
camera.Disconnect();
Expand Down

0 comments on commit a5acf22

Please sign in to comment.