//#define CalibrationTestAndBurns2FW
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Text;
using System.Text.RegularExpressions;
namespace Intel.RealSense
{
class ExampleAutocalibrateDevice
{
#region Consts
private enum CalibrationSpeed
{
VeryFast, // (0.66 seconds), for small depth degradation
Fast, // (1.33 seconds), medium depth degradation.This is the recommended setting.
Medium, // (2.84 seconds), medium depth degradation.
Slow, // (Default) (2.84 seconds), for significant depth degradation.
};
private enum CalibrationScanParameter
{
Intrinsic, // (default) Intrinsic calibration correction. The intrinsic distortion comes about through microscopic shifts in the lens positions
Extrinsic, // Extrinsic calibration correction. Extrinsic distortions are related to microscopic bending and twisting of the stiffener on which the two stereo sensors are mounted
}
private enum CalibrationDataSampling
{
WindowsAndLinux,// default is 0 which uses a polling approach that works on Windows and Linux
Windows, // Windows it is possible to select 1 which is interrupt data sampling which is slightly faster
}
private Dictionary<CalibrationSpeed, int> _timeoutForCalibrationSpeed = new Dictionary<CalibrationSpeed, int> {
{ CalibrationSpeed.VeryFast, Convert.ToInt32(TimeSpan.FromSeconds(0.66).TotalMilliseconds) },
{ CalibrationSpeed.Fast, Convert.ToInt32(TimeSpan.FromMinutes(1.33).TotalMilliseconds) },
{ CalibrationSpeed.Medium, Convert.ToInt32(TimeSpan.FromMinutes(2.84).TotalMilliseconds) },
{ CalibrationSpeed.Slow, Convert.ToInt32(TimeSpan.FromMinutes(2.84).TotalMilliseconds) },
};
private enum DeviceHealth
{
Good,
CouldBePerformed,
NeedRecalibration,
Failed
}
private Dictionary<DeviceHealth, string> _deviceHealthDescription = new Dictionary<DeviceHealth, string> {
{DeviceHealth.Good, "Good" },
{DeviceHealth.CouldBePerformed, "Could be performed" },
{DeviceHealth.NeedRecalibration, "Need recalibration" },
{DeviceHealth.Failed, "Failed" },
};
#endregion Consts
public void Start(Device dev = null)
{
PrintStartProdures();
var key = ConsoleGetKey(new[] { ConsoleKey.D1, ConsoleKey.D2, ConsoleKey.D3, ConsoleKey.D4 },
$"{Environment.NewLine}" +
$"Please choose:{Environment.NewLine}" +
$"1 - Calibrate{Environment.NewLine}" +
$"2 - Reset calibration to factory defaults{Environment.NewLine}" +
$"3 - Calibration modes test{Environment.NewLine}" +
$"4 - Exit{Environment.NewLine}");
switch (key)
{
case ConsoleKey.D1:
Calibrate(dev);
break;
case ConsoleKey.D2:
ResetToFactoryCalibration(dev);
break;
case ConsoleKey.D3:
TestCalibrationModes(dev);
break;
default:
return;
}
}
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");
}
}
public void Calibrate(Device dev = null)
{
try
{
if (dev == null)
{
Console.WriteLine($"{Environment.NewLine}Getting context...");
dev = new Context().QueryDevices().First();
}
if (!IsTheDeviceD400Series(dev))
return;
Console.WriteLine($"{Environment.NewLine}Calibration device {dev.Info[CameraInfo.Name]}" +
$"{Environment.NewLine}\tSerial number: {dev.Info[CameraInfo.SerialNumber]}" +
$"{Environment.NewLine}\tFirmware version: {dev.Info[CameraInfo.FirmwareVersion]}");
VideoStreamProfile depthProfile = dev.QuerySensors()
.SelectMany(s => s.StreamProfiles)
.Where(sp => sp.Stream == Stream.Depth)
.Select(sp => sp.As<VideoStreamProfile>())
.Where(p =>
p.Width == 256
&& p.Height == 144
&& p.Framerate == 90
&& p.Format == Format.Z16
).Last();
Console.WriteLine($"{Environment.NewLine}Starting pipeline for calibration mode");
using (var pipeline = StartPipeline(depthProfile, dev))
{
var aCalibratedDevice = AutoCalibratedDevice.FromDevice(dev);
// 1. Calibration table before running on-chip calibration.
Console.WriteLine($"{Environment.NewLine}1. Calibration table before running on-chip calibration.");
var calTableBefore = aCalibratedDevice.CalibrationTable;
Console.WriteLine("Step 1 : done");
// 2. Runs the on-chip self-calibration routine that returns pointer to new calibration table.
//calibratoin config
var calibrationSpeed = CalibrationSpeed.Slow;
var calibrationScanParameter = CalibrationScanParameter.Intrinsic;
var calibrationDataSampling = CalibrationDataSampling.WindowsAndLinux;
string calibrationConfig = GetCalibrationConfig(calibrationSpeed, calibrationScanParameter, calibrationDataSampling);
var timeout = _timeoutForCalibrationSpeed[calibrationSpeed];
float health = 100;
var succeedOnChipCalibration = false;
var abortRequested = false;
byte[] calTableAfter = null;
while (!succeedOnChipCalibration && !abortRequested)
{
Console.WriteLine($"{Environment.NewLine}2. Runs the on-chip self-calibration routine that returns pointer to new calibration table.");
Console.WriteLine($"\t Format : {depthProfile.Format}");
Console.WriteLine($"\t Framerate : {depthProfile.Framerate}");
Console.WriteLine($"\t Width : {depthProfile.Width}");
Console.WriteLine($"\t Height : {depthProfile.Height}");
Console.WriteLine($"\t Calibration Speed : {calibrationSpeed}");
Console.WriteLine($"\t Calibration Scan Parameter: {calibrationScanParameter}");
Console.WriteLine($"\t Calibration Data Sampling : {calibrationDataSampling}");
Console.WriteLine($"\t Calibration Timeout : {TimeSpan.FromMilliseconds(timeout).ToString(@"mm\:ss\.fff")} (min:sec.millisec)");
var sw = new Stopwatch();
var thisCalibrationfault = false;
try
{
sw.Start();
ProgressCallback pc = (x) => { Console.WriteLine("Progress: {0} percents",x); };
// The following line performs the same calibration flow but does not report progress"
//calTableAfter = aCalibratedDevice.RunOnChipCalibration(calibrationConfig, out health, timeout);
calTableAfter = aCalibratedDevice.RunOnChipCalibration(calibrationConfig, out health, pc, timeout);
sw.Stop();
}
catch (Exception ex)
{
sw.Stop();
thisCalibrationfault = true;
Console.WriteLine($"\n\t Error during calibration: {ex.Message.Replace("\n", "\t")}");
Console.WriteLine($"\t Please try to change distance to target or light conditions{Environment.NewLine}");
if (ConsoleKey.N == ConsoleGetKey(new[] { ConsoleKey.Y, ConsoleKey.N },
@"Let's try calibrate one more time? (Y\N)"
))
{
Console.WriteLine("");
Console.WriteLine($"Calibration failed");
Console.WriteLine($"Stopping calibration pipeline...");
pipeline.Stop();
return;
}
}
if (!thisCalibrationfault)
{
Console.WriteLine($"\n\t Time spend: {sw.Elapsed.ToString(@"mm\:ss\.fff")} (min:sec.millisec)");
Console.WriteLine($"\t Device health: {health} ({_deviceHealthDescription[GetDeviceHealth(health)]})");
var res = ConsoleGetKey(new[] { ConsoleKey.Y, ConsoleKey.N, ConsoleKey.A }, @"Accept calibration ? Yes/No/Abort");
Console.WriteLine("User's selection = {0}", res);
if (res == ConsoleKey.A)
abortRequested = true;
else
succeedOnChipCalibration = (res == ConsoleKey.Y);
}
}
Console.WriteLine("Step 2 : done");
// 3. Toggle between calibration tables to assess which is better. This is optional.
Console.WriteLine($"{Environment.NewLine}3. Toggle between calibration tables to assess which is better.");
aCalibratedDevice.CalibrationTable = calTableAfter;
Console.WriteLine("Step 3 : done");
// 4. burns the new calibration to FW persistently.
Console.WriteLine("");
if (ConsoleKey.Y == ConsoleGetKey(new[] { ConsoleKey.Y, ConsoleKey.N },
@"4. Burns the new calibration to FW persistently. (Y\N)"))
{
aCalibratedDevice.WriteCalibration();
Console.WriteLine("Step 4 : done");
}
else
Console.WriteLine("Step 4 : skipped");
Console.WriteLine("Calibration complete");
Console.WriteLine($"{Environment.NewLine}Stopping calibration pipeline...");
pipeline.Stop();
}
//}
}
catch (Exception ex)
{
Console.WriteLine($"{Environment.NewLine} Error during calibration:{Environment.NewLine} {ex.Message}");
}
}
#region TestCalibrationModes
public void TestCalibrationModes(Device dev = null)
{
try
{
if (dev == null)
{
Console.WriteLine($"{Environment.NewLine}Getting context...");
dev = new Context().QueryDevices().First();
}
if (!IsTheDeviceD400Series(dev))
return;
Console.WriteLine($"{Environment.NewLine}Test calibration modes using device {dev.Info[CameraInfo.Name]}" +
$"{Environment.NewLine}\tSerial number: {dev.Info[CameraInfo.SerialNumber]}" +
$"{Environment.NewLine}\tFirmware version: {dev.Info[CameraInfo.FirmwareVersion]}");
var depthProfiles = dev.QuerySensors()
.SelectMany(s => s.StreamProfiles)
.Where(sp => sp.Stream == Stream.Depth)
.Select(sp => sp.As<VideoStreamProfile>())
.OrderByDescending(p => p.Height)
.OrderByDescending(p => p.Width)
.OrderByDescending(p => p.Framerate)
.OrderByDescending(p => p.Format)
.ToList();
var fileName = Path.Combine(Directory.GetCurrentDirectory(), "ReportTestCalibrationModes.csv");
Console.WriteLine($"Report about testing calibration modes will be written to file:\n\t{fileName}");
var calibrationSetCount = 0;
if (IsLinux)
calibrationSetCount = CountEnumEntries<CalibrationSpeed>() * CountEnumEntries<CalibrationScanParameter>();
else
calibrationSetCount = CountEnumEntries<CalibrationSpeed>() * CountEnumEntries<CalibrationScanParameter>() * CountEnumEntries<CalibrationDataSampling>();
var depthProfileSet = 0;
var depthProfileSetCount = depthProfiles.Count;
using (var file = new StreamWriter(fileName))
{
var headers = GetHeader4FileReport();
file.WriteLine(headers);
file.Flush();
foreach (var depthProfile in depthProfiles)
{
Console.WriteLine($"{Environment.NewLine}Starting pipeline for calibration mode, depth profile set {++depthProfileSet} of {depthProfiles.Count}");
Console.WriteLine($"\t Format:\t {depthProfile.Format}");
Console.WriteLine($"\t Framerate:\t {depthProfile.Framerate}");
Console.WriteLine($"\t Width:\t\t {depthProfile.Width}");
Console.WriteLine($"\t Height:\t {depthProfile.Height}");
using (var pipeline = StartPipeline(depthProfile, dev))
{
var aCalibratedDevice = AutoCalibratedDevice.FromDevice(dev);
//calibratoin config set
var calibrationSet = 0;
foreach (var calibrationSpeed in GetEnumEntries<CalibrationSpeed>())
{
foreach (var calibrationScanParameter in GetEnumEntries<CalibrationScanParameter>())
{
foreach (var calibrationDataSampling in GetEnumEntries<CalibrationDataSampling>())
{
if (calibrationDataSampling == CalibrationDataSampling.Windows && IsLinux)
continue;
Console.WriteLine("");
#if (CalibrationTestAndBurns2FW)
// 1. Calibration table before running on-chip calibration.
Console.WriteLine($"{Environment.NewLine}1. Calibration table before running on-chip calibration.");
var calTableBefore = aCalibratedDevice.CalibrationTable;
Console.WriteLine("\t Step 1 : done");
#endif
// 2. Runs the on-chip self-calibration routine that returns pointer to new calibration table.
Console.WriteLine($"2. Runs the on-chip self-calibration routine for depth profile {depthProfileSet} of {depthProfileSetCount} calibration, set {++calibrationSet} of {calibrationSetCount} ");
Console.WriteLine($"\t Format : {depthProfile.Format}");
Console.WriteLine($"\t Framerate : {depthProfile.Framerate}");
Console.WriteLine($"\t Width : {depthProfile.Width}");
Console.WriteLine($"\t Height : {depthProfile.Height}");
Console.WriteLine($"\t Calibration Speed : {calibrationSpeed}");
Console.WriteLine($"\t Calibration Scan Parameter: {calibrationScanParameter}");
Console.WriteLine($"\t Calibration Data Sampling : {calibrationDataSampling}");
string calibrationConfig = GetCalibrationConfig(calibrationSpeed, calibrationScanParameter, calibrationDataSampling);
//var timeout = _timeoutForCalibrationSpeed[calibrationSpeed];
var timeout = Convert.ToInt32(TimeSpan.FromMinutes(2).TotalMilliseconds);
float health = 888;
var deviceHealth = DeviceHealth.Failed;
byte[] calTableAfter = null;
var sw = new Stopwatch();
try
{
sw.Start();
calTableAfter = aCalibratedDevice.RunOnChipCalibration(calibrationConfig, out health, timeout);
sw.Stop();
deviceHealth = GetDeviceHealth(health);
}
catch (Exception ex)
{
sw.Stop();
//write to report file
file.WriteLine(GetRow4FileReport(depthProfileSet, depthProfile, calibrationSet, calibrationSpeed, calibrationScanParameter, calibrationDataSampling, timeout, sw.Elapsed, "", _deviceHealthDescription[DeviceHealth.Failed], ex.Message.Replace("\n", "\t")));
file.Flush();
Console.WriteLine($"\t Time spend: {sw.Elapsed.ToString(@"mm\:ss\.fff")}");
Console.WriteLine($"\t Calibration failed: {ex.Message.Replace("\n", "\t")}");
Console.WriteLine("\t Step 2: failed");
continue;
}
file.WriteLine(GetRow4FileReport(depthProfileSet, depthProfile, calibrationSet, calibrationSpeed, calibrationScanParameter, calibrationDataSampling, timeout, sw.Elapsed, health.ToString(), _deviceHealthDescription[GetDeviceHealth(health)]));
file.Flush();
Console.WriteLine($"\t Time spend: {sw.Elapsed.ToString(@"mm\:ss\.fff")}");
Console.WriteLine($"\t Device health: {health} ({_deviceHealthDescription[GetDeviceHealth(health)]})");
Console.WriteLine("\t Step 2: done");
#if (CalibrationTestAndBurns2FW)
// 3. Toggle between calibration tables to assess which is better. This is optional.
Console.WriteLine("3. Toggle between calibration tables to assess which is better.");
if (deviceHealth <= DeviceHealth.CouldBePerformed)
{
aCalibratedDevice.CalibrationTable = calTableAfter;
Console.WriteLine("\t Step 3: done");
}
else
Console.WriteLine("\t Step 3: skipped");
// 4. burns the new calibration to FW persistently.
Console.WriteLine("4. Burns the new calibration to FW persistently.");
if (deviceHealth <= DeviceHealth.CouldBePerformed)
{
aCalibratedDevice.WriteCalibration();
Console.WriteLine("\t Step 4: done");
}
else
Console.WriteLine("\t Step 4: skipped");
#endif
}
}
}
Console.WriteLine($"{Environment.NewLine}Stopping calibration pipeline for depth profile set {depthProfileSet} of {depthProfiles.Count}...");
pipeline.Stop();
}
}
}
Console.WriteLine($"Report about testing calibration modes has been written to file:\n\t{fileName}");
Console.WriteLine("Testing calibration modes complete");
}
catch (Exception ex)
{
Console.WriteLine($"{Environment.NewLine} Error during calibration:{Environment.NewLine} {ex.Message}");
}
}
private string GetHeader4FileReport()
{
var row = new List<string>();
row.Add("depthProfileSet");
row.Add("Width");
row.Add("Height");
row.Add("Framerate");
row.Add("Format");
row.Add("calibrationSet");
row.Add("(int)CalibrationSpeed");
row.Add("(int)CalibrationScanParameter");
row.Add("(int)CalibrationDataSampling");
row.Add("CalibrationSpeed");
row.Add("CalibrationScanParameter");
row.Add("CalibrationDataSampling");
//row.Add("timeout (min:sec.millisec)");
row.Add("calibrationTakes (min:sec.millisec)");
row.Add("Device health");
row.Add("Device health description");
row.Add("Comments");
return string.Join(",", row);
}
private string GetRow4FileReport(
int depthProfileSet,
VideoStreamProfile profile,
int calibrationSet,
CalibrationSpeed calibrationSpeed,
CalibrationScanParameter calibrationScanParameter,
CalibrationDataSampling calibrationDataSampling,
int timeout,
TimeSpan calibrationTakes,
string deviceHealth,
string deviceHealthDescription,
string comments = ""
)
{
var row = new List<string>();
row.Add($"{depthProfileSet}");
row.Add($"{profile.Width}");
row.Add($"{profile.Height}");
row.Add($"{profile.Framerate}");
row.Add($"{profile.Format}");
row.Add($"{calibrationSet}");
row.Add($"{(int)calibrationSpeed}");
row.Add($"{(int)calibrationScanParameter}");
row.Add($"{(int)calibrationDataSampling}");
row.Add($"{calibrationSpeed}");
row.Add($"{calibrationScanParameter}");
row.Add($"{calibrationDataSampling}");
//row.Add($@"{timeout:mm\:ss\.fff}");
row.Add($@"{calibrationTakes:mm\:ss\.fff}");
row.Add($"{deviceHealth}");
row.Add($"{deviceHealthDescription}");
row.Add($"{comments}");
return string.Join(",", row);
}
private static bool IsLinux
{
get
{
//Spec https://docs.microsoft.com/en-us/dotnet/api/system.platformid?view=net-5
//MacOSX 6
//Other 7
//Unix 4
//Win32NT 2
//Win32S 0
//Win32Windows 1
//WinCE 3
//Xbox 5
int p = (int)Environment.OSVersion.Platform;
return (p == 4) || (p == 6) || (p == 128);
}
}
private static int CountEnumEntries<T>() where T : struct, IConvertible
{
if (!typeof(T).IsEnum)
throw new ArgumentException("T must be an enumerated type");
return Enum.GetNames(typeof(T)).Length;
}
private static T[] GetEnumEntries<T>() where T : struct, IConvertible
{
if (!typeof(T).IsEnum)
throw new ArgumentException("T must be an enumerated type");
return (T[])Enum.GetValues(typeof(T));
}
#endregion CalibrationModesTest
private Pipeline StartPipeline(VideoStreamProfile depthProfile, Device dev)
{
var cfg = new Config();
cfg.EnableDevice(dev.Info[CameraInfo.SerialNumber]);
cfg.EnableStream(Stream.Depth, depthProfile.Width, depthProfile.Height, depthProfile.Format, depthProfile.Framerate);
var pipeline = new Pipeline();
var pp = pipeline.Start(cfg);
return pipeline;
}
private DeviceHealth GetDeviceHealth(double health)
{
health = System.Math.Abs(health);
if (health < 0.25)
return DeviceHealth.Good;
else if (health < 0.75)
return DeviceHealth.CouldBePerformed;
else
return DeviceHealth.NeedRecalibration;
}
private string GetCalibrationConfig(CalibrationSpeed calibrationSpeed, CalibrationScanParameter calibrationScanParameter, CalibrationDataSampling calibrationDataSampling)
=>
$@"{{
""speed"": {(int)calibrationSpeed},
""scan parameter"": {(int)calibrationScanParameter},
""data sampling"": {(int)calibrationDataSampling}
}}";
public static ConsoleKey ConsoleGetKey(ConsoleKey[] possibleKeys, string prompt)
{
ConsoleKey res = ConsoleKey.Enter;
do
{
Console.WriteLine(prompt);
res = Console.ReadKey(true).Key;
}
while (!possibleKeys.Contains(res));
return res;
}
public static bool IsTheDeviceD400Series(Device dev)
{
bool res = false;
if (null != dev)
{
res = Regex.IsMatch(dev.Info[CameraInfo.Name].ToString(), @"(D4\d\d)");
if (!res)
Console.WriteLine($"{Environment.NewLine}Device is not a D400 series model");
}
return res;
}
private void PrintStartProdures()
{
Console.WriteLine($"{Environment.NewLine}Preparing Procedures:");
Console.WriteLine("\tRead carefully: https://dev.intelrealsense.com/docs/self-calibration-for-depth-cameras");
Console.WriteLine("\tPrint the target: https://dev.intelrealsense.com/docs/self-calibration-for-depth-cameras#section-appendix-a-example-target");
Console.WriteLine("\tPlace statically target around 1-1.5m away from your camera");
}
}
}