static void Main(string[] args)
{
using (var ctx = new Context())
{
DeviceList devices = ctx.QueryDevices();
if (devices.Count == 0)
{
Console.WriteLine("RealSense devices are not connected.");
return;
}
using (var pipeline = new Pipeline(ctx))
using (var config = new Config())
{
// Add pose stream
config.EnableStream(Stream.Pose, Format.SixDOF);
// Start pipeline with chosen configuration
using (var profile = pipeline.Start(config))
using (var streamprofile = profile.GetStream(Stream.Pose).As <PoseStreamProfile>())
{
Console.WriteLine($"\nDevice : {profile.Device.Info[CameraInfo.Name]}");
Console.WriteLine($" Serial number: {profile.Device.Info[CameraInfo.SerialNumber]}");
Console.WriteLine($" Firmware version: {profile.Device.Info[CameraInfo.FirmwareVersion]}");
Console.WriteLine($" Pose stream framerate: {streamprofile.Framerate}\n");
}
while (true)
{
// Wait for the next set of frames from the camera
using (FrameSet frameset = pipeline.WaitForFrames())
// Get a frame from the pose stream
using (PoseFrame frame = frameset.PoseFrame)
{
// Get pose frame data
Pose data = frame.PoseData;
// Print the x, y, z values of the translation, relative to initial position
Console.Write("\r" + new String(' ', 80));
Console.Write("\rDevice Position: {0} {1} {2} (meters)", data.translation.x.ToString("N3"), data.translation.y.ToString("N3"), data.translation.z.ToString("N3"));
}
}
}
}
}
private void ResetToFactoryCalibration(Device dev = null)
{
using (var ctx = new Context())
{
dev = dev ?? ctx.QueryDevices()[0];
if (!IsTheDeviceD400Series(dev))
{
return;
}
Console.WriteLine($"{Environment.NewLine}Reset to factory settings device an {dev.Info[CameraInfo.Name]}" +
$"{Environment.NewLine}\tSerial number: {dev.Info[CameraInfo.SerialNumber]}" +
$"{Environment.NewLine}\tFirmware version: {dev.Info[CameraInfo.FirmwareVersion]}");
var aCalibratedDevice = AutoCalibratedDevice.FromDevice(dev);
aCalibratedDevice.reset_to_factory_calibration();
Console.WriteLine($"Device calibration has been reset to factory defaults");
}
}
static void Main(string[] args)
{
FrameQueue q = new FrameQueue();
using (var ctx = new Context())
{
var devices = ctx.QueryDevices();
Console.WriteLine("There are " + devices.Count + " connected RealSense devices.");
if (devices.Count == 0)
{
return;
}
var dev = devices[0];
Console.WriteLine("\nUsing device 0, an {0}", dev.Info[CameraInfo.Name]);
Console.WriteLine(" Serial number: {0}", dev.Info[CameraInfo.SerialNumber]);
Console.WriteLine(" Firmware version: {0}", dev.Info[CameraInfo.FirmwareVersion]);
var depthSensor = dev.Sensors[0];
var sp = depthSensor.VideoStreamProfiles
.Where(p => p.Stream == Stream.Depth)
.OrderByDescending(p => p.Framerate)
.Where(p => p.Width == 640 && p.Height == 480)
.First();
depthSensor.Open(sp);
depthSensor.Start(q);
int one_meter = (int)(1f / depthSensor.DepthScale);
var run = true;
Console.CancelKeyPress += (s, e) =>
{
e.Cancel = true;
run = false;
};
ushort[] depth = new ushort[sp.Width * sp.Height];
while (run)
{
using (var f = q.WaitForFrame() as VideoFrame)
{
f.CopyTo(depth);
}
var buffer = new char[(640 / 10 + 1) * (480 / 20)];
var coverage = new int[64];
int b = 0;
for (int y = 0; y < 480; ++y)
{
for (int x = 0; x < 640; ++x)
{
ushort d = depth[x + y * 640];
if (d > 0 && d < one_meter)
{
++coverage[x / 10];
}
}
if (y % 20 == 19)
{
for (int i = 0; i < coverage.Length; i++)
{
int c = coverage[i];
buffer[b++] = " .:nhBXWW"[c / 25];
coverage[i] = 0;
}
buffer[b++] = '\n';
}
}
Console.SetCursorPosition(0, 0);
Console.WriteLine();
Console.Write(buffer);
}
depthSensor.Stop();
depthSensor.Close();
}
}
public CaptureWindow()
{
InitializeComponent();
try
{
Action <VideoFrame> updateDepth;
Action <VideoFrame> updateColor;
pipeline = new Pipeline();
colorizer = new Colorizer();
var depthWidth = 640;
var depthHeight = 480;
var depthFrames = 30;
var depthFormat = Format.Z16;
var colorWidth = 640;
var colorHeight = 480;
var colorFrames = 30;
using (var ctx = new Context())
{
var devices = ctx.QueryDevices();
var dev = devices[0];
Console.WriteLine("\nUsing device 0, an {0}", dev.Info[CameraInfo.Name]);
Console.WriteLine(" Serial number: {0}", dev.Info[CameraInfo.SerialNumber]);
Console.WriteLine(" Firmware version: {0}", dev.Info[CameraInfo.FirmwareVersion]);
var sensors = dev.QuerySensors();
var depthSensor = sensors[0];
var colorSensor = sensors[1];
var depthProfiles = depthSensor.StreamProfiles
.Where(p => p.Stream == Stream.Depth)
.OrderBy(p => p.Framerate)
.Select(p => p.As <VideoStreamProfile>());
VideoStreamProfile colorProfile = null;
// select color profile to have frameset equal or closer to depth frameset to syncer work smooth
foreach (var depthProfile in depthProfiles)
{
depthWidth = depthProfile.Width;
depthHeight = depthProfile.Height;
depthFrames = depthProfile.Framerate;
depthFormat = depthProfile.Format;
colorProfile = colorSensor.StreamProfiles
.Where(p => p.Stream == Stream.Color)
.OrderByDescending(p => p.Framerate)
.Select(p => p.As <VideoStreamProfile>())
.FirstOrDefault(p => p.Framerate == depthFrames);
if (colorProfile != null)
{
colorWidth = colorProfile.Width;
colorHeight = colorProfile.Height;
colorFrames = colorProfile.Framerate;
break;
}
}
if (colorProfile == null)
{
// if no profile with the same framerate found, takes the first
colorProfile = colorSensor.StreamProfiles
.Where(p => p.Stream == Stream.Color)
.OrderByDescending(p => p.Framerate)
.Select(p => p.As <VideoStreamProfile>()).FirstOrDefault();
if (colorProfile == null)
{
throw new InvalidOperationException($"Error while finding appropriate depth and color profiles");
}
colorWidth = colorProfile.Width;
colorHeight = colorProfile.Height;
colorFrames = colorProfile.Framerate;
}
}
var cfg = new Config();
cfg.EnableStream(Stream.Depth, depthWidth, depthHeight, depthFormat, depthFrames);
cfg.EnableStream(Stream.Color, colorWidth, colorHeight, Format.Rgb8, colorFrames);
var profile = pipeline.Start(cfg);
SetupWindow(profile, out updateDepth, out updateColor);
// Setup the SW device and sensors
var software_dev = new SoftwareDevice();
var depth_sensor = software_dev.AddSensor("Depth");
var depth_profile = depth_sensor.AddVideoStream(new SoftwareVideoStream
{
type = Stream.Depth,
index = 0,
uid = 100,
width = depthWidth,
height = depthHeight,
fps = depthFrames,
bpp = 2,
format = depthFormat,
intrinsics = profile.GetStream(Stream.Depth).As <VideoStreamProfile>().GetIntrinsics()
});
depth_sensor.AddReadOnlyOption(Option.DepthUnits, 1.0f / 5000);
var color_sensor = software_dev.AddSensor("Color");
var color_profile = color_sensor.AddVideoStream(new SoftwareVideoStream
{
type = Stream.Color,
index = 0,
uid = 101,
width = colorWidth,
height = colorHeight,
fps = colorFrames,
bpp = 3,
format = Format.Rgb8,
intrinsics = profile.GetStream(Stream.Color).As <VideoStreamProfile>().GetIntrinsics()
});
// Note about the Syncer: If actual FPS is significantly different from reported FPS in AddVideoStream
// this can confuse the syncer and prevent it from producing synchronized pairs
software_dev.SetMatcher(Matchers.Default);
var sync = new Syncer();
// The raw depth->metric units translation scale is required for Colorizer to work
var realDepthSensor = profile.Device.QuerySensors().First(s => s.Is(Extension.DepthSensor));
depth_sensor.AddReadOnlyOption(Option.DepthUnits, realDepthSensor.DepthScale);
depth_sensor.Open(depth_profile);
color_sensor.Open(color_profile);
// Push the SW device frames to the syncer
depth_sensor.Start(sync.SubmitFrame);
color_sensor.Start(sync.SubmitFrame);
var token = tokenSource.Token;
ushort[] depthData = null;
byte[] colorData = null;
var t = Task.Factory.StartNew(() =>
{
while (!token.IsCancellationRequested)
{
// We use the frames that are captured from live camera as the input data for the SW device
using (var frames = pipeline.WaitForFrames())
{
var depthFrame = frames.DepthFrame.DisposeWith(frames);
var colorFrame = frames.ColorFrame.DisposeWith(frames);
depthData = depthData ?? new ushort[depthFrame.Width * depthFrame.Height];
depthFrame.CopyTo(depthData);
depth_sensor.AddVideoFrame(depthData, depthFrame.Stride, depthFrame.BitsPerPixel / 8, depthFrame.Timestamp,
depthFrame.TimestampDomain, (int)depthFrame.Number, depth_profile);
colorData = colorData ?? new byte[colorFrame.Stride * colorFrame.Height];
colorFrame.CopyTo(colorData);
color_sensor.AddVideoFrame(colorData, colorFrame.Stride, colorFrame.BitsPerPixel / 8, colorFrame.Timestamp,
colorFrame.TimestampDomain, (int)colorFrame.Number, color_profile);
}
// Dispaly the frames that come from the SW device after synchronization
using (var new_frames = sync.WaitForFrames())
{
if (new_frames.Count == 2)
{
var colorFrame = new_frames.ColorFrame.DisposeWith(new_frames);
var depthFrame = new_frames.DepthFrame.DisposeWith(new_frames);
var colorizedDepth = colorizer.Process <VideoFrame>(depthFrame).DisposeWith(new_frames);
// Render the frames.
Dispatcher.Invoke(DispatcherPriority.Render, updateDepth, colorizedDepth);
Dispatcher.Invoke(DispatcherPriority.Render, updateColor, colorFrame);
}
}
}
}, token);
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
Application.Current.Shutdown();
}
}
static void Main(string[] args)
{
using (var ctx = new Context())
{
var devices = ctx.QueryDevices();
Console.WriteLine("There are {0} connected RealSense devices.", devices.Count);
if (devices.Count == 0)
{
return;
}
var dev = devices[0];
Console.WriteLine("\nUsing device 0, an {0}", dev.Info[CameraInfo.Name]);
Console.WriteLine(" Serial number: {0}", dev.Info[CameraInfo.SerialNumber]);
Console.WriteLine(" Firmware version: {0}", dev.Info[CameraInfo.FirmwareVersion]);
var depthSensor = dev.QuerySensors <Sensor>()[0];
var sp = depthSensor.StreamProfiles
.Where(p => p.Stream == Stream.Depth)
.OrderByDescending(p => p.Framerate)
.Select(p => p.As <VideoStreamProfile>())
.First(p => p.Width == 640 && p.Height == 480);
depthSensor.Open(sp);
int one_meter = (int)(1f / depthSensor.DepthScale);
ushort[] depth = new ushort[640 * 480];
char[] buffer = new char[(640 / 10 + 1) * (480 / 20)];
int[] coverage = new int[64];
depthSensor.Start(f =>
{
using (var vf = f.As <VideoFrame>())
vf.CopyTo(depth);
int b = 0;
for (int y = 0; y < 480; ++y)
{
for (int x = 0; x < 640; ++x)
{
ushort d = depth[x + y * 640];
if (d > 0 && d < one_meter)
{
++coverage[x / 10];
}
}
if (y % 20 == 19)
{
for (int i = 0; i < coverage.Length; i++)
{
int c = coverage[i];
buffer[b++] = " .:nhBXWW"[c / 25];
coverage[i] = 0;
}
buffer[b++] = '\n';
}
}
Console.SetCursorPosition(0, 0);
Console.WriteLine();
Console.Write(buffer);
});
AutoResetEvent stop = new AutoResetEvent(false);
Console.CancelKeyPress += (s, e) =>
{
e.Cancel = true;
stop.Set();
};
stop.WaitOne();
depthSensor.Stop();
depthSensor.Close();
}
}
public CaptureWindow()
{
InitializeComponent();
try
{
Action <VideoFrame> updateDepth;
Action <VideoFrame> updateColor;
// The colorizer processing block will be used to visualize the depth frames.
colorizer = new Colorizer();
// Create and config the pipeline to strem color and depth frames.
pipeline = new Pipeline();
using (var ctx = new Context())
{
var devices = ctx.QueryDevices();
var dev = devices[0];
Console.WriteLine("\nUsing device 0, an {0}", dev.Info[CameraInfo.Name]);
Console.WriteLine(" Serial number: {0}", dev.Info[CameraInfo.SerialNumber]);
Console.WriteLine(" Firmware version: {0}", dev.Info[CameraInfo.FirmwareVersion]);
var sensors = dev.QuerySensors();
var depthSensor = sensors[0];
var colorSensor = sensors[1];
var depthProfile = depthSensor.StreamProfiles
.Where(p => p.Stream == Stream.Depth)
.OrderBy(p => p.Framerate)
.Select(p => p.As <VideoStreamProfile>()).First();
var colorProfile = colorSensor.StreamProfiles
.Where(p => p.Stream == Stream.Color)
.OrderBy(p => p.Framerate)
.Select(p => p.As <VideoStreamProfile>()).First();
if (!testLoadSettingsJson.LoadSettingsJson(dev))
{
return;
}
var cfg = new Config();
cfg.EnableDevice(dev.Info.GetInfo(CameraInfo.SerialNumber));
cfg.EnableStream(Stream.Depth, depthProfile.Width, depthProfile.Height, depthProfile.Format, depthProfile.Framerate);
cfg.EnableStream(Stream.Color, colorProfile.Width, colorProfile.Height, colorProfile.Format, colorProfile.Framerate);
var pp = pipeline.Start(cfg);
SetupWindow(pp, out updateDepth, out updateColor);
// more device info
Console.WriteLine($"--------------------------");
foreach (var item in pp.Device.Info.ToArray())
{
Console.WriteLine($"{item.Key} - {item.Value}");
}
Console.WriteLine($"--------------------------");
}
Task.Factory.StartNew(() =>
{
while (!tokenSource.Token.IsCancellationRequested)
{
// We wait for the next available FrameSet and using it as a releaser object that would track
// all newly allocated .NET frames, and ensure deterministic finalization
// at the end of scope.
using (var frames = pipeline.WaitForFrames())
{
var colorFrame = frames.ColorFrame.DisposeWith(frames);
var depthFrame = frames.DepthFrame.DisposeWith(frames);
// We colorize the depth frame for visualization purposes
var colorizedDepth = colorizer.Process <VideoFrame>(depthFrame).DisposeWith(frames);
// Render the frames.
Dispatcher.Invoke(DispatcherPriority.Render, updateDepth, colorizedDepth);
Dispatcher.Invoke(DispatcherPriority.Render, updateColor, colorFrame);
Dispatcher.Invoke(new Action(() =>
{
String depth_dev_sn = depthFrame.Sensor.Info[CameraInfo.SerialNumber];
txtTimeStamp.Text = depth_dev_sn + " : " + String.Format("{0,-20:0.00}", depthFrame.Timestamp) + "(" + depthFrame.TimestampDomain.ToString() + ")";
}));
}
}
}, tokenSource.Token);
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
Application.Current.Shutdown();
}
}
public CaptureWindow()
{
InitializeComponent();
try
{
Action <VideoFrame> updateDepth;
Action <VideoFrame> updateColor;
// The colorizer processing block will be used to visualize the depth frames.
colorizer = new Colorizer();
// Create and config the pipeline to strem color and depth frames.
pipeline = new Pipeline();
var cfg = new Config();
using (var ctx = new Context())
{
var devices = ctx.QueryDevices();
if ((devices.Count != 1) || (!ExampleAutocalibrateDevice.IsTheDeviceD400Series(devices[0])))
{
Console.WriteLine("The tutorial {0} requires a single Realsense D400 device to run.\nFix the setup and rerun",
System.Diagnostics.Process.GetCurrentProcess().ProcessName);
Environment.Exit(1);
}
var dev = devices[0];
Console.WriteLine("Using device 0, an {0}", dev.Info[CameraInfo.Name]);
Console.WriteLine(" Serial number: {0}", dev.Info[CameraInfo.SerialNumber]);
Console.WriteLine(" Firmware version: {0}", dev.Info[CameraInfo.FirmwareVersion]);
var sensors = dev.QuerySensors();
var depthProfile = sensors
.SelectMany(s => s.StreamProfiles)
.Where(sp => sp.Stream == Stream.Depth)
.Select(sp => sp.As <VideoStreamProfile>())
.OrderBy(p => p.Framerate)
.First();
var colorProfile = sensors
.SelectMany(s => s.StreamProfiles)
.Where(sp => sp.Stream == Stream.Color)
.Select(sp => sp.As <VideoStreamProfile>())
.OrderBy(p => p.Framerate)
.First();
cfg.EnableDevice(dev.Info[CameraInfo.SerialNumber]);
cfg.EnableStream(Stream.Depth, depthProfile.Width, depthProfile.Height, depthProfile.Format, depthProfile.Framerate);
cfg.EnableStream(Stream.Color, colorProfile.Width, colorProfile.Height, colorProfile.Format, colorProfile.Framerate);
var pp = pipeline.Start(cfg);
SetupWindow(pp, out updateDepth, out updateColor);
}
// Rendering task
var renderingPause = false;
var rendering = Task.Factory.StartNew(() =>
{
while (!tokenSource.Token.IsCancellationRequested)
{
if (renderingPause)
{
continue; //pause the rendering
}
// We wait for the next available FrameSet and using it as a releaser object that would track
// all newly allocated .NET frames, and ensure deterministic finalization
// at the end of scope.
using (var frames = pipeline.WaitForFrames())
{
var colorFrame = frames.ColorFrame.DisposeWith(frames);
var depthFrame = frames.DepthFrame.DisposeWith(frames);
// Render the frames.
if (depthFrame != null)
{
// We colorize the depth frame for visualization purposes
var colorizedDepth = colorizer.Process <VideoFrame>(depthFrame).DisposeWith(frames);
Dispatcher.Invoke(DispatcherPriority.Render, updateDepth, colorizedDepth);
}
if (colorFrame != null)
{
Dispatcher.Invoke(DispatcherPriority.Render, updateColor, colorFrame);
}
if (depthFrame != null)
{
Dispatcher.Invoke(new Action(() =>
{
String depth_dev_sn = depthFrame.Sensor.Info[CameraInfo.SerialNumber];
txtTimeStamp.Text = $"{depth_dev_sn} : {depthFrame.Timestamp,-20:0.00}({depthFrame.TimestampDomain})" +
$"{Environment.NewLine}To start Auto-Calibration flow, switch focus to the application console and press C";
}));
}
}
}
}, tokenSource.Token);
// Input to calibration mode task
Task.Factory.StartNew(() =>
{
while (!tokenSource.Token.IsCancellationRequested)
{
if (ConsoleKey.C == ExampleAutocalibrateDevice.ConsoleGetKey(new[] { ConsoleKey.C },
"To start Auto-Calibration flow, switch focus to the application console and press C"))
{
renderingPause = true;
Console.WriteLine($"{Environment.NewLine}Stopping rendering pipeline...");
pipeline.Stop();
new ExampleAutocalibrateDevice().Start();
Console.WriteLine($"{Environment.NewLine}Starting rendering pipeline...");
pipeline.Start(cfg);
renderingPause = false;
}
}
}, tokenSource.Token);
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
Application.Current.Shutdown();
}
}
public ProcessingWindow()
{
InitializeComponent();
try
{
var cfg = new Config();
using (var ctx = new Context())
{
var devices = ctx.QueryDevices();
var dev = devices[0];
Console.WriteLine("\nUsing device 0, an {0}", dev.Info[CameraInfo.Name]);
Console.WriteLine(" Serial number: {0}", dev.Info[CameraInfo.SerialNumber]);
Console.WriteLine(" Firmware version: {0}", dev.Info[CameraInfo.FirmwareVersion]);
var sensors = dev.QuerySensors();
var depthSensor = sensors[0];
var colorSensor = sensors[1];
var depthProfile = depthSensor.StreamProfiles
.Where(p => p.Stream == Stream.Depth)
.OrderBy(p => p.Framerate)
.Select(p => p.As <VideoStreamProfile>()).First();
var colorProfile = colorSensor.StreamProfiles
.Where(p => p.Stream == Stream.Color)
.OrderBy(p => p.Framerate)
.Select(p => p.As <VideoStreamProfile>()).First();
cfg.EnableStream(Stream.Depth, depthProfile.Width, depthProfile.Height, depthProfile.Format, depthProfile.Framerate);
cfg.EnableStream(Stream.Color, colorProfile.Width, colorProfile.Height, colorProfile.Format, colorProfile.Framerate);
}
var pp = pipeline.Start(cfg);
// Get the recommended processing blocks for the depth sensor
var sensor = pp.Device.QuerySensors().First(s => s.Is(Extension.DepthSensor));
var blocks = sensor.ProcessingBlocks.ToList();
// Allocate bitmaps for rendring.
// Since the sample aligns the depth frames to the color frames, both of the images will have the color resolution
using (var p = pp.GetStream(Stream.Color).As <VideoStreamProfile>())
{
imgColor.Source = new WriteableBitmap(p.Width, p.Height, 96d, 96d, PixelFormats.Rgb24, null);
imgDepth.Source = new WriteableBitmap(p.Width, p.Height, 96d, 96d, PixelFormats.Rgb24, null);
}
var updateColor = UpdateImage(imgColor);
var updateDepth = UpdateImage(imgDepth);
// Create custom processing block
// For demonstration purposes it will:
// a. Get a frameset
// b. Run post-processing on the depth frame
// c. Combine the result back into a frameset
// Processing blocks are inherently thread-safe and play well with
// other API primitives such as frame-queues,
// and can be used to encapsulate advanced operations.
// All invocations are, however, synchronious so the high-level threading model
// is up to the developer
block = new CustomProcessingBlock((f, src) =>
{
// We create a FrameReleaser object that would track
// all newly allocated .NET frames, and ensure deterministic finalization
// at the end of scope.
using (var releaser = new FramesReleaser())
{
foreach (ProcessingBlock p in blocks)
{
f = p.Process(f).DisposeWith(releaser);
}
f = f.ApplyFilter(align).DisposeWith(releaser);
f = f.ApplyFilter(colorizer).DisposeWith(releaser);
var frames = f.As <FrameSet>().DisposeWith(releaser);
var colorFrame = frames[Stream.Color, Format.Rgb8].DisposeWith(releaser);
var colorizedDepth = frames[Stream.Depth, Format.Rgb8].DisposeWith(releaser);
// Combine the frames into a single result
var res = src.AllocateCompositeFrame(colorizedDepth, colorFrame).DisposeWith(releaser);
// Send it to the next processing stage
src.FrameReady(res);
}
});
// Register to results of processing via a callback:
block.Start(f =>
{
using (var frames = f.As <FrameSet>())
{
var colorFrame = frames.ColorFrame.DisposeWith(frames);
var colorizedDepth = frames.First <VideoFrame>(Stream.Depth, Format.Rgb8).DisposeWith(frames);
Dispatcher.Invoke(DispatcherPriority.Render, updateDepth, colorizedDepth);
Dispatcher.Invoke(DispatcherPriority.Render, updateColor, colorFrame);
}
});
var token = tokenSource.Token;
var t = Task.Factory.StartNew(() =>
{
while (!token.IsCancellationRequested)
{
using (var frames = pipeline.WaitForFrames())
{
// Invoke custom processing block
block.Process(frames);
}
}
}, token);
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
Application.Current.Shutdown();
}
InitializeComponent();
}
