//RealSenseメソッド-------------------------------------------------------------------
/// <summary> 機能の初期化 </summary>
private bool InitializeRealSense()
{
try
{
//SenseManagerを生成
senseManager = PXCMSenseManager.CreateInstance();
//カラーストリームの有効
var sts = senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, COLOR_WIDTH, COLOR_HEIGHT, COLOR_FPS);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("Colorストリームの有効化に失敗しました");
}
// Depthストリームを有効にする
sts = senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH,
DEPTH_WIDTH, DEPTH_HEIGHT, DEPTH_FPS);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("Depthストリームの有効化に失敗しました");
}
// 手の検出を有効にする
sts = senseManager.EnableHand();
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("手の検出の有効化に失敗しました");
}
//パイプラインを初期化する
//(インスタンスはInit()が正常終了した後作成されるので，機能に対する各種設定はInit()呼び出し後となる)
sts = senseManager.Init();
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("パイプラインの初期化に失敗しました");
}
//ミラー表示にする
senseManager.QueryCaptureManager().QueryDevice().SetMirrorMode(
PXCMCapture.Device.MirrorMode.MIRROR_MODE_HORIZONTAL);
//デバイスを取得する
device = senseManager.captureManager.device;
//座標変換オブジェクトを作成
projection = device.CreateProjection();
// 手の検出の初期化
InitializeHandTracking();
return(true);
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
return(false);
}
}
public static DS4CalibrationRecord Calibrate(PXCMCapture.Device device)
{
PXCMProjection projection = device.CreateProjection();
/* Get a calibration instance */
PXCMCalibration calib = projection.QueryInstance <PXCMCalibration>();
PXCMCalibration.StreamCalibration calibration;
PXCMCalibration.StreamTransform transformation;
calib.QueryStreamProjectionParameters(PXCMCapture.StreamType.STREAM_TYPE_COLOR, out calibration, out transformation);
float[] translation = transformation.translation;
DS4CalibrationRecord record = new DS4CalibrationRecord
{
DeviceCapture = new DS4CalibrationRecordInternal
{
ColorFocalLength = device.QueryColorFocalLength().toFloatArray(),
DepthFocalLength = device.QueryDepthFocalLength().toFloatArray(),
ColorFOV = device.QueryColorFieldOfView().toFloatArray(),
DepthFOV = device.QueryDepthFieldOfView().toFloatArray(),
ColorPrincipalPoint = device.QueryColorPrincipalPoint().toFloatArray(),
DepthPrincipalPoint = device.QueryDepthPrincipalPoint().toFloatArray(),
ColorWidth = Frame.COLOR_WIDTH,
ColorHeight = Frame.COLOR_HEIGHT,
DepthWidth = Frame.DEPTH_WIDTH,
DepthHeight = Frame.DEPTH_HEIGHT,
DepthStride = Frame.DEPTH_STRIDE,
LowConfValue = device.QueryDepthLowConfidenceValue(),
Extrinsics = new float[] { 1.0f,
0.0f,
0.0f,
translation[0],
0.0f,
1.0f,
0.0f,
translation[1],
0.0f,
0.0f,
1.0f,
translation[2],
0.0f,
0.0f,
0.0f,
1.0f }
},
API = "RSSDK"
};
return(record);
}
public Projection(PXCMSession session, PXCMCapture.Device device, PXCMImage.ImageInfo dinfo)
{
/* retrieve the invalid depth pixel values */
invalid_value = device.QueryDepthLowConfidenceValue();
/* Create the projection instance */
projection = device.CreateProjection();
uvmap = new PXCMPointF32[dinfo.width * dinfo.height];
}
public Projection(PXCMSession session, PXCMCapture.Device device, PXCMImage.ImageInfo dinfo)
{
//: start ros serial node:
// rosPublisher.start("192.168.0.10");
/* Create the projection instance */
projection = device.CreateProjection();
height = dinfo.height;
width = dinfo.width;
numOfPixels = dinfo.width * dinfo.height;
UInt16 invalid_value = device.QueryDepthLowConfidenceValue();
obj_detector = new managed_obj_detector.ObjDetector(dinfo.width, dinfo.height, invalid_value);
coords = new PXCMPoint3DF32[numOfPixels];
rgb_ir_d_xyz_points = new managed_obj_detector.RgbIrDXyzPoint[numOfPixels];
}
public double[,] GetDepthData()
{
Depth = null;
PXCMCapture.Sample Image = sm.QuerySample();
PXCMImage depth = Image.depth;
if (depth != null)
{
var DepthWidth = depth.info.width;
var DepthHeight = depth.info.height;
Device = sm.captureManager.QueryDevice();
Device.SetMirrorMode(PXCMCapture.Device.MirrorMode.MIRROR_MODE_HORIZONTAL);
Projection = Device.CreateProjection();
PXCMPoint3DF32[] Vertices = new PXCMPoint3DF32[DepthHeight * DepthWidth];
Status = Projection.QueryVertices(depth, Vertices);
Depth = new double[6, (DepthWidth * DepthHeight) / 100];
int j = 0;
for (int i = 0; i < DepthWidth * DepthHeight; i += 10)
{
Depth[0, j] = -Vertices[i].x / 10;
Depth[1, j] = Vertices[i].y / 10;
Depth[2, j] = Vertices[i].z / 10;
j++;
if (i % 640 == 0)
{
i += 6400;
}
}
Projection.Dispose();
}
sm.ReleaseFrame();
return(Depth);
}