static void Main(string[] args)
{
Console.WriteLine("test start\n");
try
{
SDK.TYInitLib();
TY_VERSION_INFO info = new TY_VERSION_INFO();
SDK.TYLibVersion(info);
Console.WriteLine(string.Format("LIB VERSION :{0} {1} {2}", info.major, info.minor, info.patch));
var dev_info = SimpleDeviceSelect();
if (dev_info == null)
{
return;
}
IntPtr dev_handle = new IntPtr();
IntPtr iface_handle = new IntPtr();
SDK.TYOpenInterface(dev_info.iface.id, ref iface_handle);
SDK.TYOpenDevice(iface_handle, dev_info.id, ref dev_handle);
FetchFrameLoop(dev_handle);
SDK.TYCloseDevice(dev_handle);
SDK.TYCloseInterface(iface_handle);
}
catch (System.ComponentModel.Win32Exception ex)
{
Console.WriteLine(ex.Message);
}
finally
{
SDK.TYDeinitLib();
}
Console.WriteLine("done");
Console.ReadKey();
}
static TY_DEVICE_BASE_INFO SimpleDeviceSelect()
{
DeviceInfoVector devs = new DeviceInfoVector();
SDK.selectDevice(SDK.TY_INTERFACE_ALL, "", "", 10, devs);
int sz = devs.Count();
if (sz == 0)
{
return(null);
}
Console.WriteLine("found follow devices:");
for (int idx = 0; idx < sz; idx++)
{
var item = devs[idx];
Console.WriteLine("{0} -- {1} {2}", idx, item.id, item.modelName);
}
Console.WriteLine("select one:");
int selected_idx = int.Parse(Console.ReadLine());
return(devs[selected_idx]);
}
static void ShowDeviceList()
{
SDK.TYUpdateInterfaceList();
uint num = 0;
SDK.TYGetInterfaceNumber(out num);
if (num == 0)
{
return;
}
TY_INTERFACE_INFO_ARRAY arr = new TY_INTERFACE_INFO_ARRAY((int)num);
uint filled;
SDK.TYGetInterfaceList(arr, num, out filled);
for (int idx = 0; idx < filled; idx++)
{
TY_INTERFACE_INFO ifaceinfo = arr[idx];
Console.WriteLine(string.Format("Interface: {0} {1}", ifaceinfo.id, ifaceinfo.type));
IntPtr iface_handle = new IntPtr();
SDK.TYOpenInterface(ifaceinfo.id, ref iface_handle);
SDK.TYUpdateDeviceList(iface_handle);
uint dev_num, dev_arr_sz;
SDK.TYGetDeviceNumber(iface_handle, out dev_num);
if (dev_num == 0)
{
continue;
}
TY_DEVICE_BASE_INFO_ARRAY dev_arr = new TY_DEVICE_BASE_INFO_ARRAY((int)dev_num);
SDK.TYGetDeviceList(iface_handle, dev_arr, dev_num, out dev_arr_sz);
for (int didx = 0; didx < dev_arr_sz; didx++)
{
var dev_info = dev_arr[didx];
Console.WriteLine(string.Format("\t{0} {1}", dev_info.id, dev_info.modelName));
}
}
}
static int __TYDetectOldVer21ColorCam(IntPtr handle)
{
TY_DEVICE_BASE_INFO info = new TY_DEVICE_BASE_INFO();
int ret = SDK.TYGetDeviceInfo(handle, info);
if (ret < 0)
{
return(-1); //ERROR
}
if (info.iface.type == SDK.TY_INTERFACE_USB)
{
return(1);
}
ret = __TYCompareFirmwareVersion(info, 2, 2);
if (((info.iface.type == SDK.TY_INTERFACE_ETHERNET) ||
(info.iface.type == SDK.TY_INTERFACE_RAW)) &&
(ret < 0))
{
return(1);
}
return(0);
}
static void FetchFrameLoop(IntPtr handle)
{
IntPtr color_isp_handle = new IntPtr();
uint cal_size = calib_inf.CSize();
SDK.TYGetStruct(handle, SDK.TY_COMPONENT_DEPTH_CAM, SDK.TY_STRUCT_CAM_CALIB_DATA, calib_inf.getCPtr(), cal_size);
Console.WriteLine(string.Format("Depth calib inf width:{0} height:{1}", calib_inf.intrinsicWidth, calib_inf.intrinsicHeight));
Console.WriteLine(string.Format("Depth intrinsic:{0} {1} {2} {3} {4} {5} {6} {7} {8}",
calib_inf.intrinsic.data[0], calib_inf.intrinsic.data[1], calib_inf.intrinsic.data[2],
calib_inf.intrinsic.data[3], calib_inf.intrinsic.data[4], calib_inf.intrinsic.data[5],
calib_inf.intrinsic.data[6], calib_inf.intrinsic.data[7], calib_inf.intrinsic.data[8]));
SDK.TYEnableComponents(handle, SDK.TY_COMPONENT_DEPTH_CAM);
SDK.TYEnableComponents(handle, SDK.TY_COMPONENT_RGB_CAM);
//set depth cam resolution
SDK.TYSetEnum(handle, SDK.TY_COMPONENT_DEPTH_CAM, SDK.TY_ENUM_IMAGE_MODE, (int)(SDK.TY_RESOLUTION_MODE_640x480 | SDK.TY_PIXEL_FORMAT_DEPTH16));
SDK.TYISPCreate(ref color_isp_handle);
SDK_ISP.ColorIspInitSetting(color_isp_handle, handle);
uint buff_sz;
SDK.TYGetFrameBufferSize(handle, out buff_sz);
int width, height;
SDK.TYGetInt(handle, SDK.TY_COMPONENT_RGB_CAM, SDK.TY_INT_WIDTH, out width);
SDK.TYGetInt(handle, SDK.TY_COMPONENT_RGB_CAM, SDK.TY_INT_HEIGHT, out height);
Console.WriteLine(string.Format("RGB Image size:{0} {1}", width, height));
int color_size = width * height * 3;
buffer[0] = new uint8_t_ARRAY((int)buff_sz);
buffer[1] = new uint8_t_ARRAY((int)buff_sz);
color_data = new uint8_t_ARRAY(color_size);
SDK.TYEnqueueBuffer(handle, buffer[0].VoidPtr(), buff_sz);
SDK.TYEnqueueBuffer(handle, buffer[1].VoidPtr(), buff_sz);
SDK.TYStartCapture(handle);
int img_index = 0;
while (true)
{
TY_FRAME_DATA frame = new TY_FRAME_DATA();
try
{
SDK.TYFetchFrame(handle, frame, -1);
Console.WriteLine(string.Format("capture {0} ", img_index));
var images = frame.image;
for (int idx = 0; idx < frame.validCount; idx++)
{
var img = images[idx];
if (img.componentID == SDK.TY_COMPONENT_DEPTH_CAM)
{
var pixel_arr = uint16_t_ARRAY.FromVoidPtr(img.buffer);
IntPtr pt = pixel_arr.VoidPtr2();
int offset = img.width * img.height / 2 + img.width / 2;
ushort distance = pixel_arr[offset];
TY_PIXEL_DESC pix = new TY_PIXEL_DESC();
TY_VECT_3F p3d = new TY_VECT_3F();
pix.x = (short)(img.width / 2);
pix.y = (short)(img.height / 2);
pix.depth = distance;
SDK.TYMapDepthToPoint3d(calib_inf, (uint)img.width, (uint)img.height, pix, 1, p3d);
Console.WriteLine(string.Format("Depth Image Center Pixel Distance:{0}", distance));
Console.WriteLine(string.Format("Point Cloud Center Data:(x:{0} y:{1} z:{2})", p3d.x, p3d.y, p3d.z));
}
else if (img.componentID == SDK.TY_COMPONENT_RGB_CAM)
{
if (img.pixelFormat == SDK.TY_PIXEL_FORMAT_YVYU)
{
var pixel_arr = uint8_t_ARRAY.FromVoidPtr(img.buffer);
SDK_ISP.ConvertYVYU2RGB(pixel_arr, color_data, img.width, img.height);
int offset = 3 * (img.width * img.height / 2 + img.width / 2);
byte b = color_data[offset];
byte g = color_data[offset + 1];
byte r = color_data[offset + 2];
Console.WriteLine(string.Format("Color Image Center Pixel value(YVYU):{0} {1} {2}", r, g, b));
}
else if (img.pixelFormat == SDK.TY_PIXEL_FORMAT_YUYV)
{
var pixel_arr = uint8_t_ARRAY.FromVoidPtr(img.buffer);
SDK_ISP.ConvertYUYV2RGB(pixel_arr, color_data, img.width, img.height);
int offset = 3 * (img.width * img.height / 2 + img.width / 2);
byte b = color_data[offset];
byte g = color_data[offset + 1];
byte r = color_data[offset + 2];
Console.WriteLine(string.Format("Color Image Center Pixel value(YUYV):{0} {1} {2}", r, g, b));
}
else if (img.pixelFormat == SDK.TY_PIXEL_FORMAT_BAYER8GB)
{
var pixel_arr = uint8_t_ARRAY.FromVoidPtr(img.buffer);
SWIGTYPE_p_void pointer = (SWIGTYPE_p_void)color_data.VoidPtr();
TY_IMAGE_DATA out_buff = SDK.TYInitImageData((uint)color_size, pointer, (uint)(img.width), (uint)(img.height));
out_buff.pixelFormat = (int)SDK.TY_PIXEL_FORMAT_BGR;
SDK.TYISPProcessImage(color_isp_handle, img, out_buff);
SDK.TYISPUpdateDevice(color_isp_handle);
var color_pixel_arr = uint8_t_ARRAY.FromVoidPtr(out_buff.buffer);
int offset = 3 * (img.width * img.height / 2 + img.width / 2);
byte b = color_pixel_arr[offset];
byte g = color_pixel_arr[offset + 1];
byte r = color_pixel_arr[offset + 2];
Console.WriteLine(string.Format("Color Image Center Pixel value(Bayer):{0} {1} {2}", r, g, b));
}
else
{
Console.WriteLine(string.Format("Color Image Type:{0}", img.pixelFormat));
}
}
}
SDK.TYEnqueueBuffer(handle, frame.userBuffer, (uint)frame.bufferSize);
img_index++;
}
catch (System.ComponentModel.Win32Exception ex)
{
Console.WriteLine(ex.ToString());
}
}
}
static void FetchFrameLoop(IntPtr handle)
{
uint cal_size = calib_inf.CSize();
SDK.TYGetStruct(handle, SDK.TY_COMPONENT_DEPTH_CAM, SDK.TY_STRUCT_CAM_CALIB_DATA, calib_inf.getCPtr(), cal_size);
Console.WriteLine(string.Format("Depth calib inf width:{0} height:{1}", calib_inf.intrinsicWidth, calib_inf.intrinsicHeight));
Console.WriteLine(string.Format("Depth intrinsic:{0} {1} {2} {3} {4} {5} {6} {7} {8}",
calib_inf.intrinsic.data[0], calib_inf.intrinsic.data[1], calib_inf.intrinsic.data[2],
calib_inf.intrinsic.data[3], calib_inf.intrinsic.data[4], calib_inf.intrinsic.data[5],
calib_inf.intrinsic.data[6], calib_inf.intrinsic.data[7], calib_inf.intrinsic.data[8]));
SDK.TYEnableComponents(handle, SDK.TY_COMPONENT_DEPTH_CAM);
//set depth cam resolution
SDK.TYSetEnum(handle, SDK.TY_COMPONENT_DEPTH_CAM, SDK.TY_ENUM_IMAGE_MODE, (int)(SDK.TY_RESOLUTION_MODE_640x480 | SDK.TY_PIXEL_FORMAT_DEPTH16));
uint buff_sz;
SDK.TYGetFrameBufferSize(handle, out buff_sz);
buffer[0] = new uint8_t_ARRAY((int)buff_sz);
buffer[1] = new uint8_t_ARRAY((int)buff_sz);
SDK.TYEnqueueBuffer(handle, buffer[0].VoidPtr(), buff_sz);
SDK.TYEnqueueBuffer(handle, buffer[1].VoidPtr(), buff_sz);
TY_PIXEL_DESC_ARRAY pixArray = new TY_PIXEL_DESC_ARRAY(640 * 480);
TY_VECT_3F_ARRAY p3dArray = new TY_VECT_3F_ARRAY(640 * 480);
TY_PIXEL_DESC temp = new TY_PIXEL_DESC();
//trigger mode
TY_TRIGGER_PARAM param = new TY_TRIGGER_PARAM();
param.mode = SDK.TY_TRIGGER_MODE_OFF;
SDK.TYSetStruct(handle, SDK.TY_COMPONENT_DEVICE, SDK.TY_STRUCT_TRIGGER_PARAM, param.getCPtr(), param.CSize());
SDK.TYStartCapture(handle);
int img_index = 0;
while (true)
{
TY_FRAME_DATA frame = new TY_FRAME_DATA();
try
{
//send soft trigger sig
//SDK.TYSendSoftTrigger(handle);
SDK.TYFetchFrame(handle, frame, 20000);
Console.WriteLine(string.Format("capture {0} ", img_index));
var images = frame.image;
for (int idx = 0; idx < frame.validCount; idx++)
{
var img = images[idx];
if (img.componentID == SDK.TY_COMPONENT_DEPTH_CAM)
{
var pixel_arr = uint16_t_ARRAY.FromVoidPtr(img.buffer);
SDK.TYMapDepthImageToPoint3d(calib_inf, (uint)img.width, (uint)img.height, pixel_arr.cast(), p3dArray.cast());
IntPtr ptP3D = p3dArray.VoidPtr2();
int offset = img.width * img.height / 2 + img.width / 2;
float p3d_fx = p3dArray.getitem(offset).x;
float p3d_fy = p3dArray.getitem(offset).y;
float p3d_fz = p3dArray.getitem(offset).z;
Console.WriteLine(string.Format("Point Cloud Center Data:(x:{0} y:{1} z:{2})", p3d_fx, p3d_fy, p3d_fz));
}
}
SDK.TYEnqueueBuffer(handle, frame.userBuffer, (uint)frame.bufferSize);
img_index++;
}
catch (System.ComponentModel.Win32Exception ex)
{
Console.WriteLine(ex.ToString());
}
}
}
public static void ColorIspInitSetting(IntPtr isp_handle, IntPtr handle)
{
int is_v21_color_device = __TYDetectOldVer21ColorCam(handle);
if (is_v21_color_device < 0)
{
Console.WriteLine("__TYDetectOldVer21ColorCam error.");
return;
}
if (is_v21_color_device > 0)
{
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_BLACK_LEVEL, 11);
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_BLACK_LEVEL_GAIN, 256.0f / (256 - 11));//float
}
else
{
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_BLACK_LEVEL, 0);
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_BLACK_LEVEL_GAIN, 1.0f);//float
bool b;
SDK.TYHasFeature(handle, SDK.TY_COMPONENT_RGB_CAM, SDK.TY_INT_ANALOG_GAIN, out b);
if (b)
{
SDK.TYSetInt(handle, SDK.TY_COMPONENT_RGB_CAM, SDK.TY_INT_ANALOG_GAIN, 1);
}
}
SWIGTYPE_p_unsigned_char pointer;
float [] fShading = new float[9];
fShading[0] = 0.30890417098999026f; fShading[1] = 10.63355541229248f; fShading[2] = -6.433426856994629f;
fShading[3] = 0.24413758516311646f; fShading[4] = 11.739893913269043f; fShading[5] = -8.148622512817383f;
fShading[6] = 0.1255662441253662f; fShading[7] = 11.88359546661377f; fShading[8] = -7.865192413330078f;
pointer = FloatArrayToSWIGTYPE(fShading, 9);
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_SHADING, pointer, 9 * sizeof(float));
int[] m_shading_center = new int[2];
m_shading_center[0] = 640;
m_shading_center[1] = 480;
pointer = IntArrayToSWIGTYPE(m_shading_center, 2);
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_SHADING_CENTER, pointer, 2 * sizeof(int));
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_CCM_ENABLE, 0);
pointer = IntPtrToSWIGTYPE(handle);
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_CAM_DEV_HANDLE, pointer, sizeof(int));
Int32 t = (int)SDK.TY_COMPONENT_RGB_CAM;
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_CAM_DEV_COMPONENT, t);
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_GAMMA, 1.0f); //float
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_AUTOBRIGHT, 1);
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_ENABLE_AUTO_EXPOSURE_GAIN, 0);
int current_image_width = 1280;
SDK.TYGetInt(handle, SDK.TY_COMPONENT_RGB_CAM, SDK.TY_INT_WIDTH, out current_image_width);
int [] image_size = new int[2];
image_size[0] = 1280;
image_size[1] = 960;// image size for current parameters
pointer = IntArrayToSWIGTYPE(image_size, 2);
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_IMAGE_SIZE, pointer, 2 * sizeof(int));
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_INPUT_RESAMPLE_SCALE, image_size[0] / current_image_width);
SDK.TYISPSetFeature(isp_handle, SDK.TY_ISP_FEATURE_ENABLE_AUTO_WHITEBALANCE, 1); //eanble auto white balance
uint comp_all;
SDK.TYGetComponentIDs(handle, out comp_all);
if (0 != (comp_all & (int)SDK.TY_COMPONENT_STORAGE))
{
return;
}
bool has_isp_block = false;
SDK.TYHasFeature(handle, SDK.TY_COMPONENT_STORAGE, SDK.TY_BYTEARRAY_ISP_BLOCK, out has_isp_block);
if (!has_isp_block)
{
return;
}
UInt32 sz = 0;
SDK.TYGetByteArraySize(handle, SDK.TY_COMPONENT_STORAGE, SDK.TY_BYTEARRAY_ISP_BLOCK, out sz);
if (sz <= 0)
{
return;
}
uint8_t_ARRAY buff = new uint8_t_ARRAY((int)sz);
SDK.TYGetByteArray(handle, SDK.TY_COMPONENT_STORAGE, SDK.TY_BYTEARRAY_ISP_BLOCK, buff.cast(), sz);
SDK.TYISPLoadConfig(isp_handle, buff.cast(), sz);
}
static void FetchFrameLoop(IntPtr handle)
{
uint cal_size1 = depth_calib.CSize();
SDK.TYGetStruct(handle, SDK.TY_COMPONENT_DEPTH_CAM, SDK.TY_STRUCT_CAM_CALIB_DATA, depth_calib.getCPtr(), cal_size1);
uint cal_size2 = color_calib.CSize();
SDK.TYGetStruct(handle, SDK.TY_COMPONENT_RGB_CAM, SDK.TY_STRUCT_CAM_CALIB_DATA, color_calib.getCPtr(), cal_size2);
IntPtr color_isp_handle = new IntPtr();
SDK.TYEnableComponents(handle, SDK.TY_COMPONENT_DEPTH_CAM);
SDK.TYEnableComponents(handle, SDK.TY_COMPONENT_RGB_CAM);
SDK.TYISPCreate(ref color_isp_handle);
SDK_ISP.ColorIspInitSetting(color_isp_handle, handle);
uint buff_sz;
SDK.TYGetFrameBufferSize(handle, out buff_sz);
int depth_width, depth_height;
SDK.TYGetInt(handle, SDK.TY_COMPONENT_DEPTH_CAM, SDK.TY_INT_WIDTH, out depth_width);
SDK.TYGetInt(handle, SDK.TY_COMPONENT_DEPTH_CAM, SDK.TY_INT_HEIGHT, out depth_height);
Console.WriteLine(string.Format("Depth Image size:{0} {1}", depth_width, depth_height));
int color_width, color_height;
SDK.TYGetInt(handle, SDK.TY_COMPONENT_RGB_CAM, SDK.TY_INT_WIDTH, out color_width);
SDK.TYGetInt(handle, SDK.TY_COMPONENT_RGB_CAM, SDK.TY_INT_HEIGHT, out color_height);
Console.WriteLine(string.Format("RGB Image size:{0} {1}", color_width, color_height));
int color_size = color_width * color_height * 3;
buffer[0] = new uint8_t_ARRAY((int)buff_sz);
buffer[1] = new uint8_t_ARRAY((int)buff_sz);
color_data = new uint8_t_ARRAY(color_size);
undistort_color_data = new uint8_t_ARRAY(color_size);
registration_depth_data = new uint16_t_ARRAY(color_size);
src.width = color_width;
src.height = color_height;
src.size = 3 * color_width * color_height;
src.pixelFormat = (int)SDK.TY_PIXEL_FORMAT_RGB;
src.buffer = color_data.VoidPtr();
dst.width = color_width;
dst.height = color_height;
dst.size = 3 * color_width * color_height;
dst.pixelFormat = (int)SDK.TY_PIXEL_FORMAT_RGB;
dst.buffer = undistort_color_data.VoidPtr();
SDK.TYEnqueueBuffer(handle, buffer[0].VoidPtr(), buff_sz);
SDK.TYEnqueueBuffer(handle, buffer[1].VoidPtr(), buff_sz);
SDK.TYStartCapture(handle);
int img_index = 0;
while (true)
{
TY_FRAME_DATA frame = new TY_FRAME_DATA();
try
{
SDK.TYFetchFrame(handle, frame, -1);
Console.WriteLine(string.Format("capture {0} ", img_index));
bool depth_enable = false;
bool color_enable = false;
uint16_t_ARRAY depth_pixel_arr = null;
var images = frame.image;
for (int idx = 0; idx < frame.validCount; idx++)
{
var img = images[idx];
if (img.componentID == SDK.TY_COMPONENT_DEPTH_CAM)
{
depth_pixel_arr = uint16_t_ARRAY.FromVoidPtr(img.buffer);
depth_enable = true;
}
else if (img.componentID == SDK.TY_COMPONENT_RGB_CAM)
{
if (img.pixelFormat == SDK.TY_PIXEL_FORMAT_YVYU)
{
var pixel_arr = uint8_t_ARRAY.FromVoidPtr(img.buffer);
SDK_ISP.ConvertYVYU2RGB(pixel_arr, color_data, img.width, img.height);
color_enable = true;
}
else if (img.pixelFormat == SDK.TY_PIXEL_FORMAT_YUYV)
{
var pixel_arr = uint8_t_ARRAY.FromVoidPtr(img.buffer);
SDK_ISP.ConvertYUYV2RGB(pixel_arr, color_data, img.width, img.height);
color_enable = true;
}
else if (img.pixelFormat == SDK.TY_PIXEL_FORMAT_BAYER8GB)
{
var pixel_arr = uint8_t_ARRAY.FromVoidPtr(img.buffer);
SWIGTYPE_p_void pointer = (SWIGTYPE_p_void)color_data.VoidPtr();
TY_IMAGE_DATA out_buff = SDK.TYInitImageData((uint)color_size, pointer, (uint)(img.width), (uint)(img.height));
out_buff.pixelFormat = (int)SDK.TY_PIXEL_FORMAT_BGR;
SDK.TYISPProcessImage(color_isp_handle, img, out_buff);
SDK.TYISPUpdateDevice(color_isp_handle);
var color_pixel_arr = uint8_t_ARRAY.FromVoidPtr(out_buff.buffer);
color_enable = true;
}
else
{
Console.WriteLine(string.Format("Color Image Type:{0}", img.pixelFormat));
}
}
}
if ((depth_enable) && (color_enable))
{
SDK.TYUndistortImage(color_calib, src, color_calib.intrinsic, dst);
int offset = color_width * color_height / 2 + color_width / 2;
byte b = undistort_color_data[3 * offset];
byte g = undistort_color_data[3 * offset + 1];
byte r = undistort_color_data[3 * offset + 2];
SDK.TYMapDepthImageToColorCoordinate(depth_calib, (uint)depth_width, (uint)depth_height, depth_pixel_arr.cast(), color_calib,
(uint)color_width, (uint)color_height, registration_depth_data.cast());
ushort distance = registration_depth_data[offset];
Console.WriteLine(string.Format("The rgbd value of the center position of the image :R.{0} G.{1} B.{2} D.{3}", r, g, b, distance));
}
SDK.TYEnqueueBuffer(handle, frame.userBuffer, (uint)frame.bufferSize);
img_index++;
}
catch (System.ComponentModel.Win32Exception ex)
{
Console.WriteLine(ex.ToString());
}
}
}
static void ShowDeviceFeatures(IntPtr handle)
{
uint compIDs;
SDK.TYGetComponentIDs(handle, out compIDs);
Dictionary <uint, string> comp_list = new Dictionary <uint, string> {
{ SDK.TY_COMPONENT_DEPTH_CAM, "TY_COMPONENT_DEPTH_CAM" },
{ SDK.TY_COMPONENT_RGB_CAM, "TY_COMPONENT_RGB_CAM" },
{ SDK.TY_COMPONENT_IR_CAM_LEFT, "TY_COMPONENT_IR_CAM_LEFT" }
};
Dictionary <uint, string> feature_list = new Dictionary <uint, string> {
{ SDK.TY_INT_WIDTH, "TY_INT_WIDTH" },
{ SDK.TY_INT_HEIGHT, "TY_INT_HEIGHT" },
{ SDK.TY_ENUM_IMAGE_MODE, "TY_ENUM_IMAGE_MODE" },
{ SDK.TY_STRUCT_CAM_INTRINSIC, "TY_STRUCT_CAM_INTRINSIC" },
{ SDK.TY_STRUCT_CAM_DISTORTION, "TY_STRUCT_CAM_DISTORTION" },
{ SDK.TY_STRUCT_EXTRINSIC_TO_LEFT_IR, "TY_STRUCT_EXTRINSIC_TO_LEFT_IR" },
};
foreach (var comp in comp_list)
{
if ((comp.Key & compIDs) == 0)
{
continue;
}
Console.WriteLine(string.Format("== {0}", comp.Value));
foreach (var feat in feature_list)
{
bool has_feat;
SDK.TYHasFeature(handle, comp.Key, feat.Key, out has_feat);
if (!has_feat)
{
continue;
}
uint feat_type = SDK.TYFeatureType(feat.Key);
if (feat_type == SDK.TY_FEATURE_INT)
{
int val;
SDK.TYGetInt(handle, comp.Key, feat.Key, out val);
Console.WriteLine(string.Format("\t{0}: {1}", feat.Value, val));
}
if (feat_type == SDK.TY_FEATURE_ENUM)
{
Console.WriteLine(string.Format("\t{0}:", feat.Value));
uint entryCount, filledCount;
SDK.TYGetEnumEntryCount(handle, comp.Key, feat.Key, out entryCount);
TY_ENUM_ENTRY_ARRAY arr = new TY_ENUM_ENTRY_ARRAY((int)entryCount);
SDK.TYGetEnumEntryInfo(handle, comp.Key, feat.Key, arr, entryCount, out filledCount);
for (int idx = 0; idx < filledCount; idx++)
{
var entry = arr[idx];
Console.WriteLine(string.Format("\t\t {0} : {1}", entry.description, entry.value));
}
}
if (feat_type == SDK.TY_FEATURE_STRUCT)
{
Console.WriteLine(string.Format("\t{0}:", feat.Value));
if (SDK.TY_STRUCT_CAM_INTRINSIC == feat.Key)
{
TY_CAMERA_INTRINSIC intri = new TY_CAMERA_INTRINSIC();
SDK.TYGetStruct(handle, comp.Key, feat.Key, intri.getCPtr(), intri.CSize());
var arr = intri.data;
DisplayArray(arr, 9);
}
if (SDK.TY_STRUCT_EXTRINSIC_TO_LEFT_IR == feat.Key)
{
TY_CAMERA_EXTRINSIC intri = new TY_CAMERA_EXTRINSIC();
SDK.TYGetStruct(handle, comp.Key, feat.Key, intri.getCPtr(), intri.CSize());
var arr = intri.data;
DisplayArray(arr, 16);
}
if (SDK.TY_STRUCT_CAM_DISTORTION == feat.Key)
{
TY_CAMERA_DISTORTION intri = new TY_CAMERA_DISTORTION();
SDK.TYGetStruct(handle, comp.Key, feat.Key, intri.getCPtr(), intri.CSize());
var arr = intri.data;
DisplayArray(arr, 9);
}
}
}
}
}
