From 8077b046fadd14f5b7decedef56a4b60c667fd65 Mon Sep 17 00:00:00 2001 From: Yu Meng Date: Fri, 4 Dec 2020 10:08:06 +0800 Subject: [PATCH] On-chip calibration support in csharp wrapper --- .../Devices/AutoCalibratedDevice.cs | 70 +++++++++++++++++ .../Intel.RealSense/Devices/CMakeLists.txt | 2 + .../Devices/CalibratedDevice.cs | 31 ++++++++ .../csharp/Intel.RealSense/NativeMethods.cs | 77 ++++++++++--------- 4 files changed, 145 insertions(+), 35 deletions(-) create mode 100644 wrappers/csharp/Intel.RealSense/Devices/AutoCalibratedDevice.cs create mode 100644 wrappers/csharp/Intel.RealSense/Devices/CalibratedDevice.cs diff --git a/wrappers/csharp/Intel.RealSense/Devices/AutoCalibratedDevice.cs b/wrappers/csharp/Intel.RealSense/Devices/AutoCalibratedDevice.cs new file mode 100644 index 0000000000..b3a8744d9f --- /dev/null +++ b/wrappers/csharp/Intel.RealSense/Devices/AutoCalibratedDevice.cs @@ -0,0 +1,70 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +namespace Intel.RealSense +{ + using System; + using System.Runtime.InteropServices; + + public class AutoCalibratedDevice : CalibratedDevice + { + internal AutoCalibratedDevice(IntPtr dev) + : base(dev) + { } + public static AutoCalibratedDevice FromDevice(Device dev) + { + object error; + if (NativeMethods.rs2_is_device_extendable_to(dev.Handle, Extension.AutoCalibratedDevice, out error) == 0) + { + throw new ArgumentException($"Device does not support {nameof(Extension.AutoCalibratedDevice)}"); + } + + return Device.Create(dev.Handle); + } + + public byte[] RunOnChipCalibration(string json, out float health, int timeout_ms) + { + object error; + IntPtr rawDataBuffer = NativeMethods.rs2_run_on_chip_calibration(Handle, json, json.Length, out health, null, IntPtr.Zero, timeout_ms, out error); + return GetByteArrayFromRawDataObject(rawDataBuffer); + } + + public byte[] RunTareCalibration(float ground_truth_mm, string json, int timeout_ms) + { + object error; + IntPtr rawDataBuffer = NativeMethods.rs2_run_tare_calibration(Handle, ground_truth_mm, json, json.Length, null, IntPtr.Zero, timeout_ms, out error); + return GetByteArrayFromRawDataObject(rawDataBuffer); + } + + public byte[] CalibrationTable + { + get + { + object error; + IntPtr rawDataBuffer = NativeMethods.rs2_get_calibration_table(Handle, out error); + return GetByteArrayFromRawDataObject(rawDataBuffer); + } + set + { + IntPtr rawCalibrationTable = Marshal.AllocHGlobal(value.Length); + Marshal.Copy(value, 0, rawCalibrationTable, value.Length); + object error; + NativeMethods.rs2_set_calibration_table(Handle, rawCalibrationTable, value.Length, out error); + Marshal.FreeHGlobal(rawCalibrationTable); + } + } + + byte[] GetByteArrayFromRawDataObject(IntPtr raw) + { + object error; + var start = NativeMethods.rs2_get_raw_data(raw, out error); + var size = NativeMethods.rs2_get_raw_data_size(raw, out error); + + byte[] managedBytes = new byte[size]; + Marshal.Copy(start, managedBytes, 0, size); + + NativeMethods.rs2_delete_raw_data(raw); + return managedBytes; + } + } +} diff --git a/wrappers/csharp/Intel.RealSense/Devices/CMakeLists.txt b/wrappers/csharp/Intel.RealSense/Devices/CMakeLists.txt index 9693738a26..03ad837e23 100644 --- a/wrappers/csharp/Intel.RealSense/Devices/CMakeLists.txt +++ b/wrappers/csharp/Intel.RealSense/Devices/CMakeLists.txt @@ -1,6 +1,8 @@ target_sources(${LRS_DOTNET_TARGET} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/AdvancedDevice.cs" + "${CMAKE_CURRENT_LIST_DIR}/AutoCalibratedDevice.cs" + "${CMAKE_CURRENT_LIST_DIR}/CalibratedDevice.cs" "${CMAKE_CURRENT_LIST_DIR}/DebugDevice.cs" "${CMAKE_CURRENT_LIST_DIR}/Device.cs" "${CMAKE_CURRENT_LIST_DIR}/DeviceList.cs" diff --git a/wrappers/csharp/Intel.RealSense/Devices/CalibratedDevice.cs b/wrappers/csharp/Intel.RealSense/Devices/CalibratedDevice.cs new file mode 100644 index 0000000000..f04bb43e57 --- /dev/null +++ b/wrappers/csharp/Intel.RealSense/Devices/CalibratedDevice.cs @@ -0,0 +1,31 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2017 Intel Corporation. All Rights Reserved. + +namespace Intel.RealSense +{ + using System; + + public class CalibratedDevice : Device + { + internal CalibratedDevice(IntPtr dev) + : base(dev) + { } + + public static CalibratedDevice FromDevice(Device dev) + { + return Device.Create(dev.Handle); ; + } + + public void WriteCalibration() + { + object error; + NativeMethods.rs2_write_calibration(Handle, out error); + } + + public void reset_to_factory_calibration() + { + object error; + NativeMethods.rs2_reset_to_factory_calibration(Handle, out error); + } +} +} diff --git a/wrappers/csharp/Intel.RealSense/NativeMethods.cs b/wrappers/csharp/Intel.RealSense/NativeMethods.cs index a35be66cf0..252a65980c 100644 --- a/wrappers/csharp/Intel.RealSense/NativeMethods.cs +++ b/wrappers/csharp/Intel.RealSense/NativeMethods.cs @@ -53,7 +53,7 @@ internal static MemCpyDelegate GetMethod() [DllImport("msvcrt.dll", EntryPoint = "memcpy", CallingConvention = CallingConvention.Cdecl, SetLastError = false)] internal static extern IntPtr win_memcpy(IntPtr dest, IntPtr src, int count); -#region rs_record_playback + #region rs_record_playback [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_create_record_device(IntPtr device, [MarshalAs(UnmanagedType.LPStr)] string file, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); @@ -104,9 +104,8 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern void rs2_playback_device_stop(IntPtr device, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - -#endregion -#region rs_processing + #endregion + #region rs_processing [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_create_colorizer([MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); @@ -193,7 +192,6 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_create_sequence_id_filter([MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - #endregion #region rs_option [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] @@ -228,9 +226,8 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_get_option_value_description(IntPtr options, Option option, float value, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - -#endregion -#region rs_frame + #endregion + #region rs_frame [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern long rs2_get_frame_metadata(IntPtr frame, FrameMetadataValue frame_metadata, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); @@ -301,10 +298,10 @@ internal static MemCpyDelegate GetMethod() internal static extern IntPtr rs2_allocate_synthetic_motion_frame(IntPtr source, IntPtr new_stream, IntPtr original, Extension frame_type, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] - internal static extern IntPtr rs2_allocate_composite_frame(IntPtr source, [In]IntPtr[] frames, int count, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + internal static extern IntPtr rs2_allocate_composite_frame(IntPtr source, [In] IntPtr[] frames, int count, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] - internal static extern IntPtr rs2_allocate_composite_frame(IntPtr source, [In]IntPtr frames, int count, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + internal static extern IntPtr rs2_allocate_composite_frame(IntPtr source, [In] IntPtr frames, int count, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_extract_frame(IntPtr composite, int index, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); @@ -317,9 +314,8 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern void rs2_pose_frame_get_pose_data(IntPtr frame, [In, Out] Pose pose, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - -#endregion -#region rs_sensor + #endregion + #region rs_sensor [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern void rs2_delete_sensor_list(IntPtr info_list); @@ -357,7 +353,7 @@ internal static MemCpyDelegate GetMethod() internal static extern void rs2_open(IntPtr device, IntPtr profile, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] - internal static extern void rs2_open_multiple(IntPtr device, [In]IntPtr[] profiles, int count, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + internal static extern void rs2_open_multiple(IntPtr device, [In] IntPtr[] profiles, int count, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern void rs2_close(IntPtr sensor, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); @@ -475,11 +471,9 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern float rs2_get_max_usable_depth_range(IntPtr sensor, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - - - #endregion - #region rs_device - [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] + #endregion + #region rs_device + [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern int rs2_get_device_count(IntPtr info_list, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] @@ -520,7 +514,6 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_create_flash_backup(IntPtr device, rs2_update_progress_callback callback, IntPtr user_data, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - #endregion #region rs_context [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] @@ -558,8 +551,8 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern void rs2_context_add_software_device(IntPtr ctx, IntPtr device, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); -#endregion -#region rs + #endregion + #region rs [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern int rs2_get_raw_data_size(IntPtr buffer, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); @@ -592,9 +585,8 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern double rs2_get_time([MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - -#endregion -#region rs_advanced_mode + #endregion + #region rs_advanced_mode [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern void rs2_toggle_advanced_mode(IntPtr dev, int enable, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); @@ -684,9 +676,28 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_serialize_json(IntPtr dev, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + #endregion + #region rs_calibrated_device + [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] + internal static extern void rs2_write_calibration(IntPtr dev, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); -#endregion -#region rs_internal + [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] + internal static extern void rs2_reset_to_factory_calibration(IntPtr dev, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + #endregion + #region rs_auto_calibrated_device + [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] + internal static extern IntPtr rs2_run_on_chip_calibration(IntPtr dev, [MarshalAs(UnmanagedType.LPStr)] string json_content, int content_size, out float health, [MarshalAs(UnmanagedType.FunctionPtr)] rs2_update_progress_callback callback, IntPtr client_data, int timeout_ms, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + + [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] + internal static extern IntPtr rs2_run_tare_calibration(IntPtr dev, float ground_truth_mm, [MarshalAs(UnmanagedType.LPStr)] string json_content, int content_size, [MarshalAs(UnmanagedType.FunctionPtr)] rs2_update_progress_callback callback, IntPtr client_data, int timeout_ms, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + + [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] + internal static extern IntPtr rs2_get_calibration_table(IntPtr dev, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + + [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] + internal static extern void rs2_set_calibration_table(IntPtr dev, IntPtr calibration, int content_size, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + #endregion + #region rs_internal [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_create_recording_context(int api_version, [MarshalAs(UnmanagedType.LPStr)] string filename, [MarshalAs(UnmanagedType.LPStr)] string section, RecordingMode mode, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); @@ -731,7 +742,6 @@ internal static MemCpyDelegate GetMethod() internal static extern void rs2_software_sensor_update_read_only_option(IntPtr sensor, Option option, float val, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); #endregion #region fw_logs - [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_create_fw_log_message(IntPtr device, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); @@ -788,10 +798,9 @@ internal static MemCpyDelegate GetMethod() [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern uint rs2_get_fw_log_parsed_timestamp(IntPtr fw_parsed_log, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - + [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern uint rs2_get_fw_log_parsed_sequence_id(IntPtr fw_parsed_log, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - #endregion #region terminal_parser [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] @@ -808,7 +817,6 @@ internal static extern IntPtr rs2_terminal_parse_command(IntPtr terminal_parser, internal static extern IntPtr rs2_terminal_parse_response(IntPtr terminal_parser, [MarshalAs(UnmanagedType.LPStr)] string command, uint size_of_command, IntPtr response_bytes, uint size_of_response, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); - #endregion #endregion #region rs_pipeline @@ -892,10 +900,9 @@ internal static extern IntPtr rs2_terminal_parse_response(IntPtr terminal_parser [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern int rs2_config_can_resolve(IntPtr config, IntPtr pipe, [MarshalAs(UnmanagedType.CustomMarshaler, MarshalTypeRef = typeof(ErrorMarshaler))] out object error); + #endregion -#endregion - -#region Error Handling + #region Error Handling [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_get_failed_function(IntPtr error); @@ -910,7 +917,7 @@ internal static extern IntPtr rs2_terminal_parse_response(IntPtr terminal_parser [DllImport(dllName, CallingConvention = CallingConvention.Cdecl)] internal static extern IntPtr rs2_free_error(IntPtr error); -#endregion + #endregion } }