private int weightsNum = 15; //smoothing factor
// Use this for initialization
void Start () {
hands = new HandsModel ();
MaxHands = hands.MaxHands;
MaxJoints = hands.MaxJoints;
handList = new Hashtable ();
senseManager = PXCMSenseManager.CreateInstance ();
if (senseManager == null)
Debug.LogError ("SenseManager Initialization Failed");
/* Enable hand tracking and retrieve an hand module instance to configure */
status = senseManager.EnableHand ();
handAnalyzer = senseManager.QueryHand ();
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError ("PXCSenseManager.EnableHand: " + status);
/* Initialize the execution pipeline */
status = senseManager.Init ();
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError ("PXCSenseManager.Init: " + status);
/* Retrieve the the DataSmoothing instance */
senseManager.QuerySession ().CreateImpl<PXCMDataSmoothing> (out dataSmoothing);
smoother3D = new PXCMDataSmoothing.Smoother3D[MaxHands][];
/* Configure a hand - Enable Gestures and Alerts */
PXCMHandConfiguration hcfg = handAnalyzer.CreateActiveConfiguration ();
hcfg.EnableAllGestures ();
hcfg.EnableAlert (PXCMHandData.AlertType.ALERT_HAND_NOT_DETECTED);
hcfg.ApplyChanges ();
hcfg.Dispose ();
InitObject ();
}
// Use this for initialization
void Start()
{
// Creates an instance of the sense manager to be called later
session = PXCMSenseManager.CreateInstance();
//Output an error if there is no instance of the sense manager
if (session == null)
{
Debug.LogError("SenseManager Init Failed!");
}
// Enables hand tracking
sts = session.EnableHand();
handAnalyzer = session.QueryHand();
sts2 = session.EnableFace();
faceAnalyzer = session.QueryFace();
if (sts != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError("PXCSenseManager.EnableHand: " + sts);
if (sts2 != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError("PXCSenseManager.EnableFace: " + sts2);
// Creates the session
sts = session.Init();
sts2 = session.Init();
if (sts != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError("PXCSenseManager.Init: " + sts);
if (sts2 != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError("PXCSenseManager.Init: " + sts2);
// Creates a hand config for future data
PXCMHandConfiguration handconfig = handAnalyzer.CreateActiveConfiguration();
PXCMFaceConfiguration faceconfig = faceAnalyzer.CreateActiveConfiguration();
//If there is handconfig instance
if (handconfig != null)
{
handconfig.EnableAllAlerts();
handconfig.ApplyChanges();
handconfig.Dispose();
}
if (faceconfig != null)
{
faceconfig.EnableAllAlerts();
faceconfig.ApplyChanges();
faceconfig.Dispose();
}
}
bool SetGrammarFromFile(String GrammarFilename)
{
Int32 grammar = 1;
pxcmStatus sts = sr.BuildGrammarFromFile(grammar, PXCMSpeechRecognition.GrammarFileType.GFT_NONE, GrammarFilename);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
form.PrintStatus("Grammar Compile Errors:");
form.PrintStatus(sr.GetGrammarCompileErrors(grammar));
return(false);
}
sts = sr.SetGrammar(grammar);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return(false);
}
return(true);
}
/**
* @brief Lock to access the internal storage of a specified format. The function will perform format conversion if unmatched.
* @param[in] access The access mode.
* @param[in] format The requested smaple format.
* @param[in] options The option flags.
* @param[out] data The sample data storage, to be returned.
* @return PXCM_STATUS_NO_ERROR Successful execution.
*/
public pxcmStatus AcquireAccess(Access access, PixelFormat format, Option options, out ImageData data)
{
ImageDataNative dataNative = new ImageDataNative();
dataNative.native = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(ImageDataNative)));
data = dataNative;
pxcmStatus sts = PXCMImage_AcquireAccess(instance, access, format, options, dataNative.native);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
IntPtr native = dataNative.native;
Marshal.PtrToStructure(dataNative.native, dataNative);
dataNative.native = native;
}
else
{
Marshal.FreeHGlobal(dataNative.native);
dataNative.native = IntPtr.Zero;
}
return(sts);
}
internal static pxcmStatus QueryAllTrackingValuesINT(IntPtr instance, out TrackingValues[] trackingValues)
{
Int32 nvalues = PXCMTracker_QueryNumberTrackingValues(instance);
IntPtr tv2 = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(TrackingValues)) * nvalues);
pxcmStatus sts = PXCMTracker_QueryAllTrackingValues(instance, tv2);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
trackingValues = new TrackingValues[nvalues];
for (int i = 0; i < nvalues; i++)
{
trackingValues[i] = new TrackingValues();
Marshal.PtrToStructure(new IntPtr(tv2.ToInt64() + Marshal.SizeOf(typeof(TrackingValues)) * i), trackingValues[i]);
}
}
else
{
trackingValues = null;
}
Marshal.FreeHGlobal(tv2);
return(sts);
}
internal static pxcmStatus QueryAllEmotionDataINT(IntPtr emotion, Int32 fid, out EmotionData[] data)
{
Int32 data_size = PXCMEmotion_QueryEmotionSize(emotion);
IntPtr data2 = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(EmotionData)) * data_size);
pxcmStatus sts = PXCMEmotion_QueryAllEmotionData(emotion, fid, data2);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
data = new EmotionData[data_size];
for (int i = 0; i < data.Length; i++)
{
data[i] = new EmotionData();
Marshal.PtrToStructure(new IntPtr(data2.ToInt64() + i * Marshal.SizeOf(typeof(EmotionData))), data[i]);
}
}
else
{
data = null;
}
Marshal.FreeHGlobal(data2);
return(sts);
}
void CompositionTarget_Rendering(object sender, EventArgs e)
{
try {
// フレームを取得する
pxcmStatus ret = senseManager.AcquireFrame(false);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// フレームデータを取得する
PXCMCapture.Sample sample = senseManager.QuerySample();
UpdateColorImage(sample.color);
// フレームを解放する
senseManager.ReleaseFrame();
}
catch (Exception ex) {
MessageBox.Show(ex.Message);
Close();
}
}
void RSSetup()
{
//_____________________________REALSENSE_________________________
/* Initialize a PXCMSenseManager instance */
psm = PXCMSenseManager.CreateInstance();
if (psm == null)
{
Debug.LogError("SenseManager Initialization Failed");
return;
}
/* Enable the depth stream of size 640x480 and color stream of size 640x480 */
psm.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 640, 480);
/* Initialize the execution pipeline */
sts = psm.Init();
if (sts != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Debug.LogError("PXCMSenseManager.Init Failed");
OnDisable(); // Clean-up
return;
}
//_____________________________END_________________________
}
void CompositionTarget_Rendering(object sender, EventArgs e)
{
try {
// フレームを取得する
pxcmStatus ret = senseManager.AcquireFrame(false);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// セグメンテーションデータを取得する
var image = segmentation.AcquireSegmentedImage();
UpdateSegmentationImage(image);
// フレームを解放する
senseManager.ReleaseFrame();
}
catch (Exception ex) {
MessageBox.Show(ex.Message);
Close();
}
}
protected override void UpdateFrame()
{
pxcmStatus ret = this.senseManager.AcquireFrame(false);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
PXCMCapture.Sample sample = this.senseManager.QuerySample();
if (sample != null)
{
this.image = sample.color;
}
if (this.image != null)
{
this.invalidate = true;
}
senseManager.ReleaseFrame();
}
protected override byte[] GetImageBuffer()
{
if (this.image == null)
{
return(null);
}
// データを取得する
PXCMImage.ImageData data;
if (FInMode[0] == Mode.Mask)
{
return(this.imageBuffer);
}
else
{
pxcmStatus ret = this.image.AcquireAccess(
PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data
);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("画像の取得に失敗しました");
}
// バイト配列に変換する
var info = this.image.QueryInfo();
var length = data.pitches[0] * info.height;
var buffer = data.ToByteArray(0, length);
this.image.ReleaseAccess(data);
return(buffer);
}
}
public void StartStream()
{
SenseManager.captureManager.FilterByDeviceInfo(DeviceInfo);
// Initializing the SenseManager
pxcmStatus initSenseManagerStatus = SenseManager.Init();
// Initializing the SenseManager
if (initSenseManagerStatus != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new InvalidRealSenseStatusException(initSenseManagerStatus, string.Format("Failed to initialize the SenseManager. Return code: {0}", initSenseManagerStatus));
}
if (SenseManager == null)
{
throw new NullReferenceException("The SenseManager isn't initialized. Please check if you already called the InitializeStream method.");
}
IsRunning = true;
CaptureThread = new Thread(() => {
while (IsRunning)
{
if (SenseManager == null || !SenseManager.IsConnected())
{
throw new Exception("The SenseManager is not ready to stream.");
}
// Acquiring a frame with ifall=true to wait for both samples to be ready (aligned samples)
pxcmStatus acquireFrameStatus = SenseManager.AcquireFrame(true);
if (acquireFrameStatus != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new InvalidRealSenseStatusException(acquireFrameStatus, string.Format("Failed to acquire a frame. Return code: {0}", acquireFrameStatus));
}
SenseManager.ReleaseFrame();
}
});
CaptureThread.Start();
}
private bool ConfigureSpeechRecognition()
{
/* Create the AudioSource instance */
source = session.CreateAudioSource();
/* Set audio volume to 0.2 */
source.SetVolume(0.2f);
/* Set Audio Source */
source.SetDevice(sourceDeviceInfo[_activeSource]);
pxcmStatus sts = session.CreateImpl <PXCMSpeechRecognition>(out sr);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
/* Configure */
PXCMSpeechRecognition.ProfileInfo pinfo;
sr.QueryProfile(_activeLanguage, out pinfo);
sr.SetProfile(pinfo);
/* Set Command/Control or Dictation */
string[] cmds = new String[4] {
"Create", "Save", "Load", "Run"
};
if (cmds != null && cmds.GetLength(0) != 0)
{
// voice commands available, use them
sr.BuildGrammarFromStringList(1, cmds, null);
sr.SetGrammar(1);
}
}
else
{
Debug.Log("VoiceThreadError - InitFailed - CreateImpl!");
return(false);
}
return(true);
}
protected override bool Initialize()
{
this.width = 320;
this.height = 240;
this.GetSessionAndSenseManager();
pxcmStatus sts = this.senseManager.Enable3DScan();
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("3Dスキャンの有効化に失敗しました");
}
this.InitSenseManager();
this.Initialize3DScan();
this.initializing = false;
this.initialized = true;
return(true);
}
public void SetConfiguration(PXCMTouchlessController.ProfileInfo.Configuration configuration)
{
PXCMTouchlessController.ProfileInfo pInfo;
pxcmStatus res = m_touchlessController.ClearAllGestureActionMappings();
if (res != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new RealSenseEngineException("Failed to Clear All Gesture Action Mappings. {0}", res);
}
res = m_touchlessController.QueryProfile(out pInfo);
if (res != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new RealSenseEngineException("Failed to Query Profile. {0}", res);
}
pInfo.config = configuration;
res = m_touchlessController.SetProfile(pInfo);
if (res != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new RealSenseEngineException("Failed to Set Profile. {0}", res);
}
}
void UpdateFrame()
{
// フレームを取得する
pxcmStatus ret = senseManager.AcquireFrame(true);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// フレームデータを取得する
PXCMCapture.Sample sample = senseManager.QuerySample();
if (sample != null)
{
// 各データを表示する
UpdateDepthImage(sample.depth);
UpdateDepthData(sample.depth);
ShowSelectedDepth();
}
// フレームを解放する
senseManager.ReleaseFrame();
}
private void button1_Click(object sender, RoutedEventArgs e)
{
PXCMSession session = PXCMSession.CreateInstance();
PXCMSession.ImplVersion version = session.QueryVersion();
textBox1.Text = version.major.ToString() + "." + version.minor.ToString();
PXCMSenseManager sm = session.CreateSenseManager();
sm.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 0, 0);
sm.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 0, 0);
sm.Init();
pxcmStatus status = sm.AcquireFrame(true);
PXCMCapture.Sample sample = sm.QuerySample();
PXCMImage image = sample.color;
PXCMImage dimage = sample.depth;
PXCMImage.ImageData data;
PXCMImage.ImageData data2;
image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
dimage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_RAW, out data2);
WriteableBitmap wbm = data.ToWritableBitmap(0, image.info.width, image.info.height, 96.0, 96.0);
WriteableBitmap wbm2 = data2.ToWritableBitmap(0, dimage.info.width, dimage.info.height, 96.0, 96.0);
image1.Source = wbm;
image2.Source = wbm2;
image.ReleaseAccess(data);
dimage.ReleaseAccess(data2);
sm.ReleaseFrame();
sm.Close();
session.Dispose();
}
// Use this for initialization
void Start()
{
// Set up the reference to the aeroplane controller.
m_Aeroplane = GetComponent <AeroplaneController>();
/* Initialize a PXCMSenseManager instance */
sm = PXCMSenseManager.CreateInstance();
if (sm != null)
{
/* Enable hand tracking and configure the hand module */
pxcmStatus sts = sm.EnableHand();
if (sts == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
/*init hand data structure*/
handData = new PXCMHandData.IHand[2];
/* Hand module interface instance */
hand = sm.QueryHand();
/* Hand data interface instance */
hand_data = hand.CreateOutput();
// Create hand configuration instance and configure
hcfg = hand.CreateActiveConfiguration();
hcfg.EnableAllAlerts();
hcfg.SubscribeAlert(OnFiredAlert);
hcfg.EnableNormalizedJoints(true);
hcfg.ApplyChanges();
hcfg.Dispose();
/* Initialize the execution pipeline */
if (sm.Init() != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
OnDisable();
}
}
}
}
public void DepthToCameraCoordinates(PXCMImage depth, PXCMPoint3DF32[] cameraSpacePts)
{
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
bool isdepth = (depth.info.format == PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH);
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 dpitch = ddata.pitches[0] / sizeof(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
depth.ReleaseAccess(ddata);
/* Projection Calculation */
PXCMPoint3DF32[] dcords = new PXCMPoint3DF32[dwidth * dheight];
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
dcords[k].x = x;
dcords[k].y = y;
dcords[k].z = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
}
}
pxcmStatus status = projection.ProjectDepthToCamera(dcords, cameraSpacePts);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new InvalidOperationException("Projection depth to camera failed");
}
}
}
void InitializeHandModule()
{
/* Enable the hand tracking module*/
sts = SenseToolkitManager.Instance.SenseManager.EnableHand();
if (sts != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Debug.LogError("PXCSenseManager.EnableHand: " + sts);
}
/* Retrieve an instance of hand to configure */
handAnalyzer = SenseToolkitManager.Instance.SenseManager.QueryHand();
if (handAnalyzer == null)
{
Debug.LogError("PXCSenseManager.QueryHand");
}
/* Initialize the execution pipeline */
sts = SenseToolkitManager.Instance.SenseManager.Init();
if (sts != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Debug.LogError("PXCMSenseManager.Init Failed");
OnDisable();
// Clean-up
return;
}
}
public override bool OnNewFrame()
{
faceAnalysis = QueryFace();
faceAnalysis.QueryFace(fidx, out faceId, out timeStamp);
//Get face location
faceLocation = (PXCMFaceAnalysis.Detection)faceAnalysis.DynamicCast(PXCMFaceAnalysis.Detection.CUID);
locationStatus = faceLocation.QueryData(faceId, out faceLocationData);
detectionConfidence = faceLocationData.confidence.ToString();
//Get face attributes (smile, age group, gender, eye blink, etc)
faceAttributes = (PXCMFaceAnalysis.Attribute)faceAnalysis.DynamicCast(PXCMFaceAnalysis.Attribute.CUID);
faceAttributes.QueryProfile(PXCMFaceAnalysis.Attribute.Label.LABEL_EYE_CLOSED, 0, out attributeProfile);
attributeProfile.threshold = 50; //Must be here!
faceAttributes.SetProfile(PXCMFaceAnalysis.Attribute.Label.LABEL_EYE_CLOSED, ref attributeProfile);
attributeStatus = faceAttributes.QueryData(PXCMFaceAnalysis.Attribute.Label.LABEL_EYE_CLOSED, faceId, out blink);
//Get face landmarks (eye, mouth, nose position)
faceLandmark = (PXCMFaceAnalysis.Landmark)faceAnalysis.DynamicCast(PXCMFaceAnalysis.Landmark.CUID);
faceLandmark.QueryProfile(1, out landmarkProfile);
faceLandmark.SetProfile(ref landmarkProfile);
landmarkStatus = faceLandmark.QueryLandmarkData(faceId, PXCMFaceAnalysis.Landmark.Label.LABEL_7POINTS, faceLandmarkData);
ShowAttributesOnForm();
//Do the application events
try
{
Application.DoEvents(); //TODO: This should be avoided using a different thread, but how?
}
catch (AccessViolationException e)
{
//TODO: Handle exception!
}
return(true);
}
private void UpdateColorImage(PXCMImage colorFrame)
{
// データを取得する
PXCMImage.ImageData data;
PXCMImage.ImageInfo info = colorFrame.QueryInfo();
Width = info.width;
Height = info.height;
pxcmStatus ret = colorFrame.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB24, out data);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// Bitmapに変換する
var buffer = data.ToByteArray(0, info.width * info.height * 3);
ImageColor.Source = BitmapSource.Create(info.width, info.height, 96, 96, PixelFormats.Bgr24, null, buffer, info.width * 3);
// データを解放する
colorFrame.ReleaseAccess(data);
}
// Depth(距離)データを更新する
private void UpdateDepthData(PXCMImage depthFrame)
{
if (depthFrame == null)
{
return;
}
// データを取得する
PXCMImage.ImageData data;
pxcmStatus ret = depthFrame.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out data);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("Depth画像の取得に失敗");
}
// Depthデータを取得する
var info = depthFrame.QueryInfo();
depthBuffer = data.ToShortArray(0, info.width * info.height);
// データを解放する
depthFrame.ReleaseAccess(data);
}
public RealSenseEngine()
{
m_session = PXCMSession.CreateInstance();
if (m_session == null)
{
throw new RealSenseEngineException("Failed to create a Session");
}
m_senseManager = m_session.CreateSenseManager();
if (m_senseManager == null)
{
throw new RealSenseEngineException("Failed to create a SenseManager");
}
pxcmStatus res = m_senseManager.EnableTouchlessController();
if (res != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new RealSenseEngineException("Failed to Enable Touchless Controller");
}
var handler = new PXCMSenseManager.Handler();
res = m_senseManager.Init(handler);
if (res != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new RealSenseEngineException("Failed to Init Handler");
}
// getting touchless controller
m_touchlessController = m_senseManager.QueryTouchlessController();
m_touchlessController.SetPointerSensitivity(PXCMTouchlessController.PointerSensitivity.PointerSensitivity_Smoothed);
if (m_touchlessController == null)
{
throw new RealSenseEngineException("Failed to Query Touchless Controller");
}
}
/// <summary>
/// Sets the texture for the associated game object with the given image
/// </summary>
/// <param name='image'>
/// Image.
/// </param>
private void SetTexture(PXCMImage image)
{
if (image == null)
{
return;
}
if (_texture == null)
{
/* Save size and preallocate the Texture2D */
size.width = image.info.width;
size.height = image.info.height;
_texture = new Texture2D((int)size.width, (int)size.height, TextureFormat.ARGB32, false);
/* Associate the texture to the game object */
//renderer.sharedMaterial.mainTexture = _texture;
// Modified here. Added the texture to the canvas renderer
GetComponent <CanvasRenderer>().SetMaterial(GetComponent <CanvasRenderer>().GetMaterial(), _texture);
}
/* Retrieve the image data */
PXCMImage.ImageData data;
pxcmStatus sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
data.ToTexture2D(0, _texture);
image.ReleaseAccess(data);
/* and display it */
_texture.Apply();
}
}
public void SetManualMode()
{
// REALSENSE
/* Initialize a PXCMSenseManager instance */
sm = PXCMSenseManager.CreateInstance();
if (sm == null)
Debug.LogError("SenseManager Initialization Failed");
/* Enable hand tracking and retrieve an hand module instance to configure */
sts = sm.EnableHand();
handAnalyzer = sm.QueryHand();
if (sts != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError("PXCSenseManager.EnableHand: " + sts);
/* Initialize the execution pipeline */
sts = sm.Init();
if (sts != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError("PXCSenseManager.Init: " + sts);
/* Retrieve the the DataSmoothing instance */
sm.QuerySession().CreateImpl<PXCMSmoother>(out smoother);
/* Create a 3D Weighted algorithm */
smoother3D = new PXCMSmoother.Smoother3D[MaxHands][];
/* Configure a hand - Enable Gestures and Alerts */
PXCMHandConfiguration hcfg = handAnalyzer.CreateActiveConfiguration();
if (hcfg != null)
{
hcfg.EnableAllGestures();
hcfg.EnableAlert(PXCMHandData.AlertType.ALERT_HAND_NOT_DETECTED);
hcfg.SubscribeGesture(OnFiredGesture);
hcfg.ApplyChanges();
hcfg.Dispose();
}
// handAnalyzer = sm.QueryHand();
//////
curMode = userMode = DroneMode.Manual;
targetBehavior = targetDirectBehavior;
EnterFreeMovementState();
}
private void FaceTrackingPipeline()
{
IsDispose = false;
OnStart?.Invoke(this, null);
#region Manager Init
realSenseManager = RealSenseObjects.Session.CreateSenseManager();
if (realSenseManager == null)
{
MessageBox.Show(
"PXCMSenseManager初始化失敗。",
"初始化失敗",
MessageBoxButtons.OK,
MessageBoxIcon.Error);
OnStop?.Invoke(this, null);
return;
}
PXCMCaptureManager captureManager = realSenseManager.captureManager;
if (captureManager == null)
{
MessageBox.Show(
"PXCMCaptureManager初始化失敗。",
"初始化失敗",
MessageBoxButtons.OK,
MessageBoxIcon.Error);
OnStop?.Invoke(this, null);
return;
}
#endregion
#region 基本設定
//設定裝置
captureManager.FilterByDeviceInfo(Form.SelectedDevice);
//設定串流類型
captureManager.FilterByStreamProfiles(Form.SelectedDeviceStreamProfile);
//啟用臉部追蹤模組
realSenseManager.EnableFace();
PXCMFaceModule faceModule = realSenseManager.QueryFace();
if (faceModule == null)
{
MessageBox.Show(
"取得PXCMFaceModule失敗。",
"初始化失敗",
MessageBoxButtons.OK,
MessageBoxIcon.Error);
OnStop?.Invoke(this, null);
return;
}
//建立臉部追蹤模組設定
moduleConfiguration = faceModule.CreateActiveConfiguration();
if (moduleConfiguration == null)
{
MessageBox.Show(
"建立PXCMFaceConfiguration失敗。",
"初始化失敗",
MessageBoxButtons.OK,
MessageBoxIcon.Error);
OnStop?.Invoke(this, null);
return;
}
//追蹤模式設定
moduleConfiguration.SetTrackingMode(Form.ModeType);
moduleConfiguration.strategy = PXCMFaceConfiguration.TrackingStrategyType.STRATEGY_RIGHT_TO_LEFT;
moduleConfiguration.detection.isEnabled = true;
moduleConfiguration.detection.maxTrackedFaces = 4;//最大追蹤4個臉
moduleConfiguration.landmarks.isEnabled = false;
moduleConfiguration.pose.isEnabled = false;
recognitionConfig =
moduleConfiguration.QueryRecognition();
if (recognitionConfig == null)
{
MessageBox.Show(
"建立RecognitionConfiguration失敗。",
"初始化失敗",
MessageBoxButtons.OK,
MessageBoxIcon.Error);
OnStop?.Invoke(this, null);
return;
}
recognitionConfig.Enable();
#endregion
#region 讀取資料庫數據
if (Form.FaceData != null)
{
recognitionConfig.SetDatabase(Form.FaceData);
moduleConfiguration.ApplyChanges();
}
#endregion
#region 預備啟動
moduleConfiguration.EnableAllAlerts();
//moduleConfiguration.SubscribeAlert(FaceAlertHandler);
pxcmStatus applyChangesStatus = moduleConfiguration.ApplyChanges();
Form.SetStatus("RealSenseManager初始化中");
if (applyChangesStatus.IsError() || realSenseManager.Init().IsError())
{
MessageBox.Show(
"RealSenseManager初始化失敗，請檢查設定正確。",
"初始化失敗",
MessageBoxButtons.OK,
MessageBoxIcon.Error);
OnStop?.Invoke(this, null);
return;
}
#endregion
using (moduleOutput = faceModule.CreateOutput()) {
PXCMCapture.Device.StreamProfileSet profiles;
PXCMCapture.Device device = captureManager.QueryDevice();
if (device == null)
{
MessageBox.Show(
"取得設備失敗。",
"初始化失敗",
MessageBoxButtons.OK,
MessageBoxIcon.Error);
OnStop?.Invoke(this, null);
return;
}
device.QueryStreamProfileSet(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 0, out profiles);
#region Loop
while (!_Stop)
{
while (_Paush)
{
Application.DoEvents();
}
if (realSenseManager.AcquireFrame(true).IsError())
{
break;
}
var isConnected = realSenseManager.IsConnected();
if (isConnected)
{
var sample = realSenseManager.QueryFaceSample();
if (sample == null)
{
realSenseManager.ReleaseFrame();
continue;
}
#region 畫面取出
PXCMImage image = null;
if (Form.ModeType == PXCMFaceConfiguration.TrackingModeType.FACE_MODE_IR)
{
image = sample.ir;
}
else
{
image = sample.color;
}
#endregion
moduleOutput.Update();//更新辨識
PXCMFaceConfiguration.RecognitionConfiguration recognition = moduleConfiguration.QueryRecognition();
if (recognition == null)
{
realSenseManager.ReleaseFrame();
continue;
}
#region 繪圖與事件
OnFrame?.Invoke(this, new FaceRecognitionEventArgs()
{
Image = ToBitmap(image)
});
FindFace(moduleOutput);
#endregion
}
//發布框
realSenseManager.ReleaseFrame();
}
#endregion
//更新資料庫緩衝區
//Buffer = moduleOutput.QueryRecognitionModule().GetDatabaseBuffer();
}
#region 釋放資源
moduleConfiguration.Dispose();
realSenseManager.Close();
realSenseManager.Dispose();
#endregion
IsDispose = true;
OnStop?.Invoke(this, null);
}
public void SimplePipeline()
{
PXCMSenseManager pp = m_form.Session.CreateSenseManager();
if (pp == null)
{
throw new Exception("PXCMSenseManager null");
}
// Set Source & Landmark Profile Index
PXCMCapture.DeviceInfo info;
if (m_form.GetRecordState())
{
pp.captureManager.SetFileName(m_form.GetFileName(), true);
if (m_form.Devices.TryGetValue(m_form.GetCheckedDevice(), out info))
{
pp.captureManager.FilterByDeviceInfo(info);
}
}
else if (m_form.GetPlaybackState())
{
pp.captureManager.SetFileName(m_form.GetFileName(), false);
PXCMCaptureManager cmanager = pp.QueryCaptureManager();
if (cmanager == null)
{
throw new Exception("PXCMCaptureManager null");
}
cmanager.SetRealtime(false);
}
else
{
if (m_form.Devices.TryGetValue(m_form.GetCheckedDevice(), out info))
{
pp.captureManager.FilterByDeviceInfo(info);
Tuple <PXCMImage.ImageInfo, PXCMRangeF32> selectedRes = m_form.GetCheckedColorResolution();
var set = new PXCMCapture.Device.StreamProfileSet();
set.color.frameRate = selectedRes.Item2;
set.color.imageInfo.format = selectedRes.Item1.format;
set.color.imageInfo.width = selectedRes.Item1.width;
set.color.imageInfo.height = selectedRes.Item1.height;
pp.captureManager.FilterByStreamProfiles(set);
}
}
// Set Module
pp.EnableFace();
PXCMFaceModule faceModule = pp.QueryFace();
if (faceModule == null)
{
Debug.Assert(faceModule != null);
return;
}
PXCMFaceConfiguration moduleConfiguration = faceModule.CreateActiveConfiguration();
if (moduleConfiguration == null)
{
Debug.Assert(moduleConfiguration != null);
return;
}
PXCMFaceConfiguration.TrackingModeType mode = m_form.GetCheckedProfile().Contains("3D")
? PXCMFaceConfiguration.TrackingModeType.FACE_MODE_COLOR_PLUS_DEPTH
: PXCMFaceConfiguration.TrackingModeType.FACE_MODE_COLOR;
moduleConfiguration.SetTrackingMode(mode);
moduleConfiguration.strategy = PXCMFaceConfiguration.TrackingStrategyType.STRATEGY_RIGHT_TO_LEFT;
moduleConfiguration.detection.maxTrackedFaces = m_form.NumDetection;
moduleConfiguration.landmarks.maxTrackedFaces = m_form.NumLandmarks;
moduleConfiguration.pose.maxTrackedFaces = m_form.NumPose;
PXCMFaceConfiguration.ExpressionsConfiguration econfiguration = moduleConfiguration.QueryExpressions();
if (econfiguration == null)
{
throw new Exception("ExpressionsConfiguration null");
}
econfiguration.properties.maxTrackedFaces = m_form.NumExpressions;
econfiguration.EnableAllExpressions();
moduleConfiguration.detection.isEnabled = m_form.IsDetectionEnabled();
moduleConfiguration.landmarks.isEnabled = m_form.IsLandmarksEnabled();
moduleConfiguration.pose.isEnabled = m_form.IsPoseEnabled();
if (m_form.IsExpressionsEnabled())
{
econfiguration.Enable();
}
PXCMFaceConfiguration.RecognitionConfiguration qrecognition = moduleConfiguration.QueryRecognition();
if (qrecognition == null)
{
throw new Exception("PXCMFaceConfiguration.RecognitionConfiguration null");
}
if (m_form.IsRecognitionChecked())
{
qrecognition.Enable();
}
moduleConfiguration.EnableAllAlerts();
moduleConfiguration.SubscribeAlert(FaceAlertHandler);
pxcmStatus applyChangesStatus = moduleConfiguration.ApplyChanges();
m_form.UpdateStatus("Init Started", MainForm.Label.StatusLabel);
if (applyChangesStatus < pxcmStatus.PXCM_STATUS_NO_ERROR || pp.Init() < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
m_form.UpdateStatus("Init Failed", MainForm.Label.StatusLabel);
}
else
{
using (PXCMFaceData moduleOutput = faceModule.CreateOutput())
{
Debug.Assert(moduleOutput != null);
PXCMCapture.Device.StreamProfileSet profiles;
PXCMCaptureManager cmanager = pp.QueryCaptureManager();
if (cmanager == null)
{
throw new Exception("capture manager null");
}
PXCMCapture.Device device = cmanager.QueryDevice();
if (device == null)
{
throw new Exception("device null");
}
device.QueryStreamProfileSet(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 0, out profiles);
CheckForDepthStream(profiles, faceModule);
ushort threshold = device.QueryDepthConfidenceThreshold();
int filter_option = device.QueryIVCAMFilterOption();
int range_tradeoff = device.QueryIVCAMMotionRangeTradeOff();
device.SetDepthConfidenceThreshold(1);
device.SetIVCAMFilterOption(6);
device.SetIVCAMMotionRangeTradeOff(21);
if (m_form.IsMirrored())
{
device.SetMirrorMode(PXCMCapture.Device.MirrorMode.MIRROR_MODE_HORIZONTAL);
}
else
{
device.SetMirrorMode(PXCMCapture.Device.MirrorMode.MIRROR_MODE_DISABLED);
}
m_form.UpdateStatus("Streaming", MainForm.Label.StatusLabel);
m_timer = new FPSTimer(m_form);
while (!m_form.Stopped)
{
if (pp.AcquireFrame(true) < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
break;
}
bool isConnected = pp.IsConnected();
DisplayDeviceConnection(isConnected);
if (isConnected)
{
PXCMCapture.Sample sample = pp.QueryFaceSample();
if (sample == null)
{
pp.ReleaseFrame();
continue;
}
DisplayPicture(sample.color);
moduleOutput.Update();
if (moduleConfiguration.QueryRecognition().properties.isEnabled)
{
UpdateRecognition(moduleOutput);
}
m_form.DrawGraphics(moduleOutput);
m_form.UpdatePanel();
}
pp.ReleaseFrame();
}
device.SetDepthConfidenceThreshold(threshold);
device.SetIVCAMFilterOption(filter_option);
device.SetIVCAMMotionRangeTradeOff(range_tradeoff);
}
moduleConfiguration.UnsubscribeAlert(FaceAlertHandler);
moduleConfiguration.ApplyChanges();
m_form.UpdateStatus("Stopped", MainForm.Label.StatusLabel);
}
moduleConfiguration.Dispose();
pp.Close();
pp.Dispose();
}
private bool SaveMesh(PXCM3DScan scan)
{
form.Invoke(new Action(() => form.HideTrackingAlert()));
form.Invoke(new Action(() => form.HideRangeAlerts()));
bool sts = true;
form.UpdateStatus("Saving...");
// wait for filename to be filled
string filename = null;
do
{
filename = form.GetMeshFileName();
System.Threading.Thread.Sleep(100);
} while (filename == null);
if (filename != "c")
{
form.UpdateStatus("Saving " + filename + "...");
string mesh_format = filename.Substring(filename.Length - 3);
pxcmStatus res = pxcmStatus.PXCM_STATUS_NO_ERROR;
if (mesh_format.ToLower() == "obj")
{
res = scan.Reconstruct(PXCM3DScan.FileFormat.OBJ, filename);
}
else if (mesh_format.ToLower() == "ply")
{
res = scan.Reconstruct(PXCM3DScan.FileFormat.PLY, filename);
}
else if (mesh_format.ToLower() == "stl")
{
res = scan.Reconstruct(PXCM3DScan.FileFormat.STL, filename);
}
if (res < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (res == pxcmStatus.PXCM_STATUS_FILE_WRITE_FAILED)
{
form.UpdateStatus("Error: Write failed to " + filename + " Exiting...");
}
else
{
form.UpdateStatus("Reconstruct() returned " + res);
}
sts = false;
}
else
{
form.UpdateStatus(filename + " saved.");
sts = false;
if (mesh_format.ToLower() == "obj") // Viewer only supports obj format in this release
{
if (!form.isTextureSelected()) // Viewer only supports non-textured meshes in this release
{
// Generate an html version of the obj file
String htmlFile = filename + ".html";
if (!GenerateHtmlFromObj(filename, htmlFile))
{
// Load the resulting html file
GC.Collect(); // Free up as much system RAM as possible before loading the html file.
System.Diagnostics.Process process = new System.Diagnostics.Process();
process.StartInfo.WindowStyle = System.Diagnostics.ProcessWindowStyle.Hidden;
process.StartInfo.FileName = (string)htmlFile;
process.Start();
process.Dispose();
}
}
}
}
}
else
{
form.UpdateStatus("Canceled");
}
form.UseWaitCursor = false;
form.reconstruct_requested = false;
form.ReleaseMeshFileName();
return(sts);
}
public void StreamColorDepth(String scanType) /* Stream Color and Depth Synchronously or Asynchronously */
{
bool sts = true;
PXCM3DScan.Configuration scan_config = new PXCM3DScan.Configuration();
String statusString;
/* Create an instance of the PXCSenseManager interface */
PXCMSenseManager pp = PXCMSenseManager.CreateInstance();
if (pp == null)
{
form.UpdateStatus("Failed to create sense manager");
return;
}
if (pp.captureManager == null)
{
form.UpdateStatus("Capture manager does not exist");
return;
}
if (!form.IsModeLive())
{
pp.captureManager.SetFileName(form.GetFileName(), form.IsModeRecord());
}
/* Set Input Source */
PXCMCapture.DeviceInfo dinfo2 = form.GetCheckedDevice();
if (form.IsModeLive() || form.IsModeRecord())
{
pp.captureManager.FilterByDeviceInfo(dinfo2);
}
if (form.IsModeRecord())
{
// Delay recording frames until the scan starts
pp.captureManager.SetPause(true);
}
else if (!form.IsModeLive())
{
// Disable real-time mode if we are playing back a file
// to ensure that frames are not skipped.
pp.captureManager.SetRealtime(false);
}
/* Set Color & Depth Resolution */
PXCMCapture.Device.StreamProfile cinfo = form.GetColorConfiguration();
if (cinfo.imageInfo.format != 0)
{
Single cfps = cinfo.frameRate.max;
pp.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, cinfo.imageInfo.width, cinfo.imageInfo.height, cfps);
}
PXCMCapture.Device.StreamProfile dinfo = form.GetDepthConfiguration();
if (dinfo.imageInfo.format != 0)
{
Single dfps = dinfo.frameRate.max;
pp.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, dinfo.imageInfo.width, dinfo.imageInfo.height, dfps);
}
/* Initialization */
FPSTimer timer = new FPSTimer(form);
if (form.IsModeLive())
{
form.UpdateStatus("Initializing...");
}
/* Enable the 3D Scan video module */
pxcmStatus result = pp.Enable3DScan();
if (result != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
pp.Close();
pp.Dispose();
form.UpdateStatus("Enable3DScan() returned " + result);
return;
}
/* Initialize the camera system */
result = pp.Init();
form.UpdateStatus("");
device = pp.captureManager.device;
if (result >= pxcmStatus.PXCM_STATUS_NO_ERROR && device != null)
{
bool bAutoExpAndWBChanged = false;
bool bAutoExposureEnabled = true;
bool bAutoWhiteBalanceEnabled = true;
/* Setup the scanning configuration */
if (scanType == "Object")
{
scan_config.mode = PXCM3DScan.ScanningMode.OBJECT_ON_PLANAR_SURFACE_DETECTION;
}
else if (scanType == "Face")
{
scan_config.mode = PXCM3DScan.ScanningMode.FACE;
}
else if (scanType == "Body")
{
scan_config.mode = PXCM3DScan.ScanningMode.BODY;
}
else if (scanType == "Head")
{
scan_config.mode = PXCM3DScan.ScanningMode.HEAD;
}
else if (scanType == "Full")
{
scan_config.mode = PXCM3DScan.ScanningMode.VARIABLE;
}
/* Select the Targeting Options */
scan_config.options = PXCM3DScan.ReconstructionOption.NONE;
if (form.isSolidificationSelected())
{
scan_config.options
|= (PXCM3DScan.ReconstructionOption.SOLIDIFICATION);
}
if (form.isTextureSelected())
{
scan_config.options
|= (PXCM3DScan.ReconstructionOption.TEXTURE);
}
if (form.isLandmarksSelected())
{
scan_config.options
|= (PXCM3DScan.ReconstructionOption.LANDMARKS);
}
//scan_config.useMarker = form.isUseMarkerChecked();
scan_config.flopPreviewImage = form.isFlopPreviewImageSelected();
/* Try to initialize the scanning system */
PXCM3DScan scan = pp.Query3DScan();
sts = false;
if (scan == null)
{
form.UpdateStatus("3DScan module not found.");
}
else
{
result = scan.SetConfiguration(scan_config);
if (result < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
scan.Dispose();
// Show the configuration related error code
switch (result)
{
case pxcmStatus.PXCM_STATUS_FEATURE_UNSUPPORTED:
form.UpdateStatus("Configuration not supported.");
break;
case pxcmStatus.PXCM_STATUS_ITEM_UNAVAILABLE:
form.UpdateStatus("Face module not found.");
break;
default:
form.UpdateStatus("SetConfiguration returned an error.");
break;
}
}
else
{
sts = true;
}
}
// Conditionally finish the initialization and enter the main loop
if (sts == true)
{
// Subscribe to recieve range and tracking alerts
scan.Subscribe(OnAlert);
Projection projection = new Projection(
pp.session, device, dinfo.imageInfo);
Boolean bScanning = false;
Boolean bPlaybackStarted = false;
form.Invoke(new Action(() => form.SetButtonState(sample3dscan.cs.MainForm.ButtonState.Ce_SSd)));
while (form.reconstruct_requested || !form.GetStopState())
{
if (form.GetScanRequested()) /* one time latch */
{
form.Invoke(new Action(() => form.SetScanRequested(false)));
// Delay recording frames until the start of the scan is requested
if (form.IsModeRecord())
{
pp.captureManager.SetPause(false);
}
// If the max tri/vert controls are enabled,
// use the set values. Otherwise, disabled decimation
// by setting the values to zero.
/*
* scan_config.maxTriangles = form.getMaxTrianglesEnabledChecked()
* ? form.getMaxTriangles() : 0;
* scan_config.maxVertices = form.getMaxVerticesEnabledChecked()
* ? form.getMaxVertices() : 0;
*/
// Request that the scan starts as soon as possible
scan_config.startScan = true;
scan.SetConfiguration(scan_config);
/* Update the status bar to help users understand what the detector is looking for */
if (form.IsModeLive())
{
if (scan_config.mode == PXCM3DScan.ScanningMode.OBJECT_ON_PLANAR_SURFACE_DETECTION)
{
form.UpdateStatus("Object not detected. Place object on flat surface in center of view.");
}
}
}
else if (form.reconstruct_requested)
{
sts = SaveMesh(scan);
}
/* Get preview image from the 3D Scan video module */
if (!form.GetStopState())
{
/* Wait until a frame is ready: Synchronized or Asynchronous */
if (pp.AcquireFrame() < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
projection.Dispose();
if (!form.IsModeLive())
{
form.Invoke(new Action(() => form.EndScan()));
sts = SaveMesh(scan);
}
break;
}
/* Get preview image from the 3D Scan video module */
PXCMImage preview_image = scan.AcquirePreviewImage();
pp.ReleaseFrame();
/* Display Image and Status */
if (preview_image != null)
{
form.SetBitmap(preview_image);
if (scan.IsScanning())
{
statusString = "Scanning";
timer.Tick(statusString + " ");
if (bScanning == false) // Lazy initializer
{
bScanning = true; // One way latch
// Once the scanning process starts, we want to enable the Reconstruct button
form.Invoke(new Action(() => form.SetButtonState(sample3dscan.cs.MainForm.ButtonState.Ce_ESe)));
// Object, head and body scanning with a rear facing camera involves walking
// around the target, which effectivly exposes the camera to the full
// environment, similar to a panorama. To avoid undesirable color
// inconsistencies (realted to the response of the auto-exposure/wb changes),
// it usually works best to disable them.
// Note that these property changes are restored (below).
if (device.deviceInfo.orientation
== PXCMCapture.DeviceOrientation.DEVICE_ORIENTATION_REAR_FACING &&
scan_config.mode != PXCM3DScan.ScanningMode.FACE &&
form.IsModeLive())
{
bAutoExpAndWBChanged = true;
bAutoExposureEnabled = device.QueryColorAutoExposure();
device.SetColorAutoExposure(false);
bAutoWhiteBalanceEnabled = device.QueryColorAutoWhiteBalance();
device.SetColorAutoWhiteBalance(false);
}
}
}
else
{
if (!form.IsModeLive() && !form.IsModeRecord()) // In playback mode, automatically request the scan
{
if (bPlaybackStarted == false) // Lazy initializer
{
bPlaybackStarted = true; // One way latch
form.scan_requested = true;
form.Invoke(new Action(() => form.StartScanning(false)));
form.Invoke(new Action(() => form.SetButtonState(sample3dscan.cs.MainForm.ButtonState.Ce_ESe)));
}
}
else
{
if (!form.GetStopState())
{
if (isScanReady(form.landmarksChecked()))
{
form.Invoke(new Action(() => form.EnableReconstruction(true)));
}
else
{
form.Invoke(new Action(() => form.EnableReconstruction(false)));
}
}
}
}
preview_image.Dispose();
}
}
}
projection.Dispose();
scan.Dispose();
}
// Restore the default camera properties
if (bAutoExpAndWBChanged)
{
device.SetColorAutoExposure(bAutoExposureEnabled);
device.SetColorAutoWhiteBalance(bAutoWhiteBalanceEnabled);
}
device.Dispose();
device = null;
}
else
{
try { form.UpdateStatus(result + ""); }
catch { }
sts = false;
}
if (sts)
{
try { form.UpdateStatus(""); }
catch { }
}
pp.Close();
pp.Dispose();
try { form.Invoke(new Action(() => form.ResetStop())); }
catch { }
}
// Use this for initialization
void Start()
{
handList = new Hashtable ();
/* Initialize a PXCMSenseManager instance */
sm = PXCMSenseManager.CreateInstance ();
if (sm == null)
Debug.LogError ("SenseManager Initialization Failed");
/* Enable hand tracking and retrieve an hand module instance to configure */
sts = sm.EnableHand ();
handAnalyzer = sm.QueryHand ();
if (sts != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError ("PXCSenseManager.EnableHand: " + sts);
/* Initialize the execution pipeline */
sts = sm.Init ();
if (sts != pxcmStatus.PXCM_STATUS_NO_ERROR)
Debug.LogError ("PXCSenseManager.Init: " + sts);
/* Retrieve the the DataSmoothing instance */
sm.QuerySession ().CreateImpl<PXCMDataSmoothing> (out ds);
/* Create a 3D Weighted algorithm */
smoother3D = new PXCMDataSmoothing.Smoother3D[MaxHands][];
/* Configure a hand - Enable Gestures and Alerts */
PXCMHandConfiguration hcfg = handAnalyzer.CreateActiveConfiguration ();
hcfg.EnableAllGestures ();
hcfg.EnableAlert (PXCMHandData.AlertType.ALERT_HAND_NOT_DETECTED);
hcfg.ApplyChanges ();
hcfg.Dispose ();
InitializeGameobjects ();
transform.RotateAround (transform.position, Vector3.up, 200 * Time.deltaTime);
// Vector3 _RotateAround = new Vector3 (1, 1f, 180f);
// transform.RotateAround (_RotateAround, Vector3.up, 20);
}
public unsafe override void GetNextFrame()
{
lock (this)
{
if (capture == null) return;
PXCMImage.ImageData refEquals;
sts = capture.ReadStreamAsync(images, out sps[0]);
PXCMScheduler.SyncPoint.SynchronizeEx(sps);
images[1].AcquireAccess(PXCMImage.Access.ACCESS_READ, out refEquals);
short* depth = (short*)refEquals.buffer.planes[0];
ushort* confidence = (ushort*)refEquals.buffer.planes[1];
float* uv = (float*)refEquals.buffer.planes[2];
int width = (int)images[1].imageInfo.width;
int height = (int)images[1].imageInfo.height;
if (depthFrame == null) this.depthFrame = new DepthCameraFrame(width, height);
if (textureFrame == null) this.textureFrame = new TextureMapFrame(width, height);
for (int i = 0; i < width * height; i++)
{
this.textureFrame.data[2 * i] = uv[2 * i];
this.textureFrame.data[2 * i + 1] = uv[2 * i + 1];
this.depthFrame.data[4 * i] = depth[3 * i];
this.depthFrame.data[4 * i + 1] = depth[3 * i + 1];
this.depthFrame.data[4 * i + 2] = depth[3 * i + 2];
this.depthFrame.data[4 * i + 3] = confidence[i];
}
images[1].ReleaseAccess(ref refEquals);
((CLCalc.Program.Variable)depthFrame.GetMemoryObject()).WriteToDevice(depthFrame.data);
((CLCalc.Program.Variable)textureFrame.GetMemoryObject()).WriteToDevice(textureFrame.data);
images[0].AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out refEquals);
byte* rgb = (byte*)refEquals.buffer.planes[0];
width = (int)images[0].imageInfo.width;
height = (int)images[0].imageInfo.height;
if (colorFrame == null) this.colorFrame = new ColorCameraFrame(width, height);
int offset = 0;
for (int i = 0; i < width * height; i++)
{
this.colorFrame.data[offset] = rgb[offset + 2];
this.colorFrame.data[offset + 1] = rgb[offset + 1];
this.colorFrame.data[offset + 2] = rgb[offset];
this.colorFrame.data[offset + 3] = rgb[offset + 3];
offset += 4;
}
images[0].ReleaseAccess(ref refEquals);
((CLCalc.Program.Image2D)colorFrame.GetMemoryObject()).WriteToDevice(colorFrame.data);
foreach (PXCMScheduler.SyncPoint s in sps) if (s != null) s.Dispose();
foreach (PXCMImage i in images) if (i != null) i.Dispose();
}
}
private void OnStatus(Int32 mid, pxcmStatus sts)
{
handler.onStatus(mid, sts);
}
public InvalidRealSenseStatusException(pxcmStatus status, string message)
: base(message)
{
InvalidStatus = status;
}
void OnEnable()
{
Initialized = false;
/* Create a SenseManager instance */
SenseManager = PXCMSenseManager.CreateInstance();
if (SenseManager == null)
{
print("Unable to create the pipeline instance");
return;
}
if (_speechCommandsRef.Count != 0)
{
SetSenseOption(SenseOption.SenseOptionID.Speech);
}
int numberOfEnabledModalities = 0;
//Set mode according to RunMode - play from file / record / live stream
if (RunMode == MCTTypes.RunModes.PlayFromFile)
{
//CHECK IF FILE EXISTS
if (!System.IO.File.Exists(FilePath))
{
Debug.LogWarning("No Filepath Set Or File Doesn't Exist, Run Mode Will Be Changed to Live Stream");
RunMode = MCTTypes.RunModes.LiveStream;
}
else
{
PXCMCaptureManager cManager = SenseManager.QueryCaptureManager();
cManager.SetFileName(FilePath, false);
Debug.Log("SenseToolkitManager: Playing from file: " + FilePath);
}
}
if (RunMode == MCTTypes.RunModes.RecordToFile)
{
//CHECK IF PATH
string PathOnly = FilePath;
while (!PathOnly[PathOnly.Length - 1].Equals('\\'))
{
PathOnly = PathOnly.Remove(PathOnly.Length - 1, 1);
}
if (!System.IO.Directory.Exists(PathOnly))
{
Debug.LogWarning("No Filepath Set Or Path Doesn't Exist, Run Mode Will Be Changed to Live Stream");
RunMode = MCTTypes.RunModes.LiveStream;
}
else
{
PXCMCaptureManager cManager = SenseManager.QueryCaptureManager();
cManager.SetFileName(FilePath, true);
Debug.Log("SenseToolkitManager: Recording to file: " + FilePath);
}
}
/* Enable modalities according to the set options*/
if (IsSenseOptionSet(SenseOption.SenseOptionID.Face, true))
{
SenseManager.EnableFace();
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Face).Initialized = true;
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Face).Enabled = true;
SetSenseOption(SenseOption.SenseOptionID.VideoColorStream);
numberOfEnabledModalities++;
}
if (IsSenseOptionSet(SenseOption.SenseOptionID.Hand, true))
{
_sts = SenseManager.EnableHand();
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Hand).Initialized = true;
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Hand).Enabled = true;
numberOfEnabledModalities++;
}
if (IsSenseOptionSet(SenseOption.SenseOptionID.Object, true))
{
_sts = SenseManager.EnableTracker();
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Object).Initialized = true;
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Object).Enabled = true;
numberOfEnabledModalities++;
}
if (IsSenseOptionSet(SenseOption.SenseOptionID.Speech, true))
{
if (!SpeechManager.IsInitialized)
{
if (SpeechManager.InitalizeSpeech())
{
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Speech).Initialized = true;
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Speech).Enabled = true;
numberOfEnabledModalities++;
}
else
{
UnsetSenseOption(SenseOption.SenseOptionID.Speech);
}
}
else
{
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Speech).Initialized = true;
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Speech).Enabled = true;
numberOfEnabledModalities++;
}
}
if (IsSenseOptionSet(SenseOption.SenseOptionID.VideoDepthStream, true) ||
IsSenseOptionSet(SenseOption.SenseOptionID.PointCloud, true))
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 0, 0, 0);
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.VideoDepthStream).Initialized = true;
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.VideoDepthStream).Enabled = true;
numberOfEnabledModalities++;
}
if (IsSenseOptionSet(SenseOption.SenseOptionID.VideoIRStream, true))
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_IR, 0, 0, 0);
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.VideoIRStream).Initialized = true;
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.VideoIRStream).Enabled = true;
numberOfEnabledModalities++;
}
if (IsSenseOptionSet(SenseOption.SenseOptionID.VideoColorStream, true))
{
if (ColorImageQuality == MCTTypes.RGBQuality.FullHD)
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 1920, 1080, 0);
}
else if (ColorImageQuality == MCTTypes.RGBQuality.HD)
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 1280, 720, 0);
}
else if (ColorImageQuality == MCTTypes.RGBQuality.HalfHD)
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 960, 540, 0);
}
else
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 640, 480, 0);
}
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.VideoColorStream).Initialized = true;
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.VideoColorStream).Enabled = true;
numberOfEnabledModalities++;
}
if (IsSenseOptionSet(SenseOption.SenseOptionID.VideoSegmentation, true))
{
if (ColorImageQuality == MCTTypes.RGBQuality.FullHD)
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 1920, 1080, 0);
}
else if (ColorImageQuality == MCTTypes.RGBQuality.HD)
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 1280, 720, 0);
}
else if (ColorImageQuality == MCTTypes.RGBQuality.HalfHD)
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 960, 540, 0);
}
else
{
SenseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 640, 480, 0);
}
SenseManager.Enable3DSeg();
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.VideoSegmentation).Initialized = true;
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.VideoSegmentation).Enabled = true;
numberOfEnabledModalities++;
}
/* Initialize the execution */
_sts = SenseManager.Init();
if (_sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (numberOfEnabledModalities > 0)
{
Debug.LogError("Unable to initialize all modalities");
}
return;
}
//Set different configurations:
// Face
if (IsSenseOptionSet(SenseOption.SenseOptionID.Face, true))
{
var faceModule = SenseManager.QueryFace();
var faceConfiguration = faceModule.CreateActiveConfiguration();
if (faceConfiguration == null) throw new UnityException("CreateActiveConfiguration returned null");
faceConfiguration.Update();
faceConfiguration.detection.isEnabled = true;
faceConfiguration.detection.smoothingLevel = PXCMFaceConfiguration.SmoothingLevelType.SMOOTHING_DISABLED;
faceConfiguration.landmarks.isEnabled = true;
faceConfiguration.landmarks.smoothingLevel = PXCMFaceConfiguration.SmoothingLevelType.SMOOTHING_DISABLED;
faceConfiguration.pose.isEnabled = true;
faceConfiguration.pose.smoothingLevel = PXCMFaceConfiguration.SmoothingLevelType.SMOOTHING_DISABLED;
faceConfiguration.DisableAllAlerts();
faceConfiguration.strategy = PXCMFaceConfiguration.TrackingStrategyType.STRATEGY_APPEARANCE_TIME;
if ((NumberOfDetectedFaces < 1) || (NumberOfDetectedFaces > 15))
{
Debug.Log("Ilegal value for Number Of Detected Faces, value is set to 1");
NumberOfDetectedFaces = 1;
}
faceConfiguration.detection.maxTrackedFaces = NumberOfDetectedFaces;
faceConfiguration.landmarks.maxTrackedFaces = NumberOfDetectedFaces;
faceConfiguration.pose.maxTrackedFaces = NumberOfDetectedFaces;
PXCMFaceConfiguration.ExpressionsConfiguration expressionConfig = faceConfiguration.QueryExpressions();
expressionConfig.Enable();
expressionConfig.EnableAllExpressions();
faceConfiguration.ApplyChanges();
faceConfiguration.Dispose();
FaceModuleOutput = faceModule.CreateOutput();
UnsetSenseOption(SenseOption.SenseOptionID.VideoColorStream);
}
// Hand
if (IsSenseOptionSet(SenseOption.SenseOptionID.Hand, true))
{
PXCMHandModule handAnalysis = SenseManager.QueryHand();
PXCMHandConfiguration handConfiguration = handAnalysis.CreateActiveConfiguration();
if (handConfiguration == null) throw new UnityException("CreateActiveConfiguration returned null");
handConfiguration.Update();
handConfiguration.EnableAllGestures();
handConfiguration.EnableStabilizer(true);
handConfiguration.DisableAllAlerts();
handConfiguration.EnableSegmentationImage(false);
handConfiguration.ApplyChanges();
handConfiguration.Dispose();
HandDataOutput = handAnalysis.CreateOutput();
}
if (IsSenseOptionSet(SenseOption.SenseOptionID.Object, true))
{
if (_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Object).Enabled != true)
{
_senseOptions.Find( i => i.ID == SenseOption.SenseOptionID.Object).Enabled = true;
OnDisable();
OnEnable();
}
}
if (IsSenseOptionSet(SenseOption.SenseOptionID.Speech, true))
{
UpdateSpeechCommands();
SpeechManager.Start();
}
// Create an instance for the projection & blob extractor
if (Projection == null)
{
Projection = SenseManager.QueryCaptureManager().QueryDevice().CreateProjection();
}
if (BlobExtractor == null)
{
SenseManager.session.CreateImpl<PXCMBlobExtractor>(out BlobExtractor);
}
// Set initialization flag
Initialized = true;
}
void Update()
{
//Dynamically Pause/Enable Modules
int numberOfEnabledModules = 0;
foreach (var option in _senseOptions)
{
if (option.RefCounter == 0 && option.Enabled )
{
if (option.ModuleCUID > 0)
{
SenseManager.PauseModule(option.ModuleCUID, true);
}
option.Enabled = false;
}
else if (option.RefCounter > 0 && !option.Enabled)
{
if (!option.Initialized)
{
OnDisable();
OnEnable();
Start();
}
if (option.ModuleCUID > 0)
{
SenseManager.PauseModule(option.ModuleCUID, false);
}
option.Enabled = true;
}
if (option.Enabled)
{
numberOfEnabledModules++;
}
}
//Update Speech commands if changed
if (_speechCommandsChanged)
{
UpdateSpeechCommands();
SpeechManager.Reset();
}
// Every frame update all the data
if (Initialized && numberOfEnabledModules > 0)
{
_sts = SenseManager.AcquireFrame(true, 100);
if (_sts == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoColorStream).Enabled)
{
if (ImageRgbOutput != null)
{
ImageRgbOutput.Dispose();
}
if (_captureSample == null)
{
_captureSample = SenseManager.QuerySample();
}
if (_captureSample.color != null)
{
ImageRgbOutput = _captureSample.color;
ImageRgbOutput.QueryInstance<PXCMAddRef>().AddRef();
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoDepthStream).Enabled ||
_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.PointCloud).Enabled)
{
if (ImageDepthOutput != null)
{
ImageDepthOutput.Dispose();
}
if (_captureSample == null)
{
_captureSample = SenseManager.QuerySample();
}
if (_captureSample.depth != null)
{
ImageDepthOutput = _captureSample.depth;
ImageDepthOutput.QueryInstance<PXCMAddRef>().AddRef();
if (!_isInitBlob)
{
PXCMImage.ImageInfo info = ImageDepthOutput.QueryInfo();
BlobExtractor.Init(info);
BlobExtractor.SetMaxBlobs(MaxBlobsToDetect);
_isInitBlob = true;
}
if (PointCloud == null)
{
PointCloud = new PXCMPoint3DF32[ImageDepthOutput.info.width * ImageDepthOutput.info.height];
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.PointCloud).Enabled)
{
if (PointCloud == null)
{
PointCloud = new PXCMPoint3DF32[ImageDepthOutput.info.width * ImageDepthOutput.info.height];
}
_sts = Projection.QueryVertices(ImageDepthOutput, PointCloud);
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.UVMap).Enabled)
{
if (UvMap == null)
{
UvMap = new PXCMPointF32[ImageDepthOutput.info.width * ImageDepthOutput.info.height];
}
Projection.QueryUVMap(ImageDepthOutput, UvMap);
}
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoIRStream).Enabled)
{
if (ImageIROutput != null)
{
ImageIROutput.Dispose();
}
if (_captureSample == null)
{
_captureSample = SenseManager.QuerySample();
}
if (_captureSample.ir != null)
{
ImageIROutput = _captureSample.ir;
ImageIROutput.QueryInstance<PXCMAddRef>().AddRef();
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoSegmentation).Enabled)
{
if (Image3DSegmentationOutput != null)
{
Image3DSegmentationOutput.Dispose();
}
PXCM3DSeg seg = SenseManager.Query3DSeg();
if (seg != null)
{
Image3DSegmentationOutput = seg.AcquireSegmentedImage();
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.Face).Enabled)
{
FaceModuleOutput.Update();
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.Hand).Enabled)
{
HandDataOutput.Update();
}
_captureSample = null;
SenseManager.ReleaseFrame();
}
//Speech
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.Speech).Enabled)
{
SpeechManager.QueryRecognizedCommands(out SpeechOutput);
}
}
}
public override bool OnNewFrame()
{
faceAnalysis = QueryFace();
faceAnalysis.QueryFace(fidx, out faceId, out timeStamp);
//Get face location
faceLocation = (PXCMFaceAnalysis.Detection)faceAnalysis.DynamicCast(PXCMFaceAnalysis.Detection.CUID);
locationStatus = faceLocation.QueryData(faceId, out faceLocationData);
detectionConfidence = faceLocationData.confidence.ToString();
//Get face attributes (smile, age group, gender, eye blink, etc)
faceAttributes = (PXCMFaceAnalysis.Attribute)faceAnalysis.DynamicCast(PXCMFaceAnalysis.Attribute.CUID);
faceAttributes.QueryProfile(PXCMFaceAnalysis.Attribute.Label.LABEL_EYE_CLOSED, 0, out attributeProfile);
attributeProfile.threshold = 50; //Must be here!
faceAttributes.SetProfile(PXCMFaceAnalysis.Attribute.Label.LABEL_EYE_CLOSED, ref attributeProfile);
attributeStatus = faceAttributes.QueryData(PXCMFaceAnalysis.Attribute.Label.LABEL_EYE_CLOSED, faceId, out blink);
//Get face landmarks (eye, mouth, nose position)
faceLandmark = (PXCMFaceAnalysis.Landmark)faceAnalysis.DynamicCast(PXCMFaceAnalysis.Landmark.CUID);
faceLandmark.QueryProfile(1, out landmarkProfile);
faceLandmark.SetProfile(ref landmarkProfile);
landmarkStatus = faceLandmark.QueryLandmarkData(faceId, PXCMFaceAnalysis.Landmark.Label.LABEL_7POINTS, faceLandmarkData);
ShowAttributesOnForm();
//Do the application events
try
{
Application.DoEvents(); //TODO: This should be avoided using a different thread, but how?
}
catch (AccessViolationException e)
{
//TODO: Handle exception!
}
return true;
}
/// <summary>
/// Device-specific implementation of Connect.
/// Connects the camera.
/// </summary>
/// <remarks>This method is implicitely called by <see cref="Camera.Connect"/> inside a camera lock.</remarks>
/// <seealso cref="Camera.Connect"/>
protected override void ConnectImpl()
{
log.EnterMethod();
if (deviceInfo.Count == 0)
{
ScanForCameras();
}
if (deviceInfo.Count == 0)
{
log.Error(Name + ": No devices found.");
ExceptionBuilder.Throw(typeof(ConnectionFailedException), this, "No devices found.");
}
int deviceIdx;
for (deviceIdx = 0; deviceIdx < deviceInfo.Count; deviceIdx++)
{
if (deviceInfo[deviceIdx].name != "Intel(R) RealSense(TM) 3D Camera R200")
{
continue;
}
log.Debug("RealSense R200 found");
break;
}
if (deviceIdx >= deviceInfo.Count)
{
log.Error("Failed to find Intel Real Sense R200 camera!");
ExceptionBuilder.Throw(typeof(ConnectionFailedException), this, "No R200 found!");
}
ScanForProfiles(deviceIdx);
// Create an instance of the PXCSenseManager interface
pp = PXCMSenseManager.CreateInstance();
if (pp == null)
{
log.Error(Name + ": Failed to create an SDK pipeline object");
return;
}
pp.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, widthColor, heightColor, fpsColor);
pp.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, widthZImage, heightZImage, fpsZImage);
pxcmStatus retStat = pp.Init();
if (retStat < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
log.Error(Name + ": Init() failed.");
ExceptionBuilder.Throw(typeof(ConnectionFailedException), this, "Failed to Initialize Real Sense SDK");
}
pp.captureManager.device.ResetProperties(PXCMCapture.StreamType.STREAM_TYPE_ANY);
// Find calibration data
PXCMProjection projection = pp.captureManager.device.CreateProjection();
PXCMCalibration calib = projection.QueryInstance <PXCMCalibration>();
retStat = calib.QueryStreamProjectionParameters(PXCMCapture.StreamType.STREAM_TYPE_COLOR, out calibDataColor, out calibTransColor);
if (retStat < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
log.WarnFormat("Could not get calibration for color channel.");
}
retStat = calib.QueryStreamProjectionParameters(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, out calibDataDepth, out calibTransDepth);
if (retStat < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
log.WarnFormat("Could not get calibration for depth channel.");
}
projection.Dispose();
log.InfoFormat("Color calibration: f=({0}; {1}) c=({2}; {3}) k=({4}; {5}; {6}) p=({7}; {8})",
calibDataColor.focalLength.x, calibDataColor.focalLength.y,
calibDataColor.principalPoint.x, calibDataColor.principalPoint.y,
calibDataColor.radialDistortion[0], calibDataColor.radialDistortion[1], calibDataColor.radialDistortion[2],
calibDataColor.tangentialDistortion[0], calibDataColor.tangentialDistortion[1]
);
log.InfoFormat("Color translation: t=({0}; {1}; {2})",
calibTransColor.translation[0], calibTransColor.translation[1], calibTransColor.translation[2]
);
log.InfoFormat("Depth calibration: f=({0}; {1}) c=({2}; {3}) k=({4}; {5}; {6}) p=({7}; {8})",
calibDataDepth.focalLength.x, calibDataDepth.focalLength.y,
calibDataDepth.principalPoint.x, calibDataDepth.principalPoint.y,
calibDataDepth.radialDistortion[0], calibDataDepth.radialDistortion[1], calibDataDepth.radialDistortion[2],
calibDataDepth.tangentialDistortion[0], calibDataDepth.tangentialDistortion[1]
);
log.InfoFormat("Depth translation: t=({0}; {1}; {2})",
calibTransDepth.translation[0], calibTransDepth.translation[1], calibTransDepth.translation[2]
);
ActivateChannel(ChannelNames.ZImage);
ActivateChannel(ChannelNames.Color);
SelectChannel(ChannelNames.ZImage);
log.LeaveMethod();
}
public override bool Initialize()
{
sts = PXCMSession.CreateInstance(out session);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Console.WriteLine("Failed to create the SDK session");
return false;
}
PXCMSession.ImplDesc desc;
session.QueryImpl(0, out desc);
capture = new UtilMCapture(session);
PXCMCapture.VideoStream.DataDesc dataDesc = new PXCMCapture.VideoStream.DataDesc();
PXCMBase tmp1;
sts = session.CreateImpl(PXCMFaceAnalysis.CUID, out tmp1);
PXCMCapture.VideoStream.DataDesc.StreamDesc stream;
stream = new PXCMCapture.VideoStream.DataDesc.StreamDesc();
stream.format = PXCMImage.ColorFormat.COLOR_FORMAT_NV12;
dataDesc.streams[0] = stream;
stream = new PXCMCapture.VideoStream.DataDesc.StreamDesc();
stream.format = PXCMImage.ColorFormat.COLOR_FORMAT_VERTICES;
stream.sizeMin.width = 320;
stream.sizeMin.height = 240;
stream.sizeMax.width = 320;
stream.sizeMax.height = 240;
dataDesc.streams[1] = stream;
sts = capture.LocateStreams(ref dataDesc);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Console.WriteLine("Failed to locate a capture module");
capture.Dispose();
session.Dispose();
return false;
}
return true;
}
public void OnStatus(Int32 mid, pxcmStatus status)
{
//PXCMBase m = this.sm.QueryModule(mid);
//Console.WriteLine("RSSDK(Status): " + mid.ToString() + " - " + status.ToString());
}
protected override void UpdateFrame()
{
pxcmStatus ret = this.senseManager.AcquireFrame(true);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (ret == pxcmStatus.PXCM_STATUS_EXEC_ABORTED)
{
// do noting
}
else
{
throw new Exception("Could not acquire frame. " + ret.ToString());
}
}
PXCMCapture.Sample sample = this.senseManager.QuerySample();
if (sample != null)
{
// update image
this.image = sample.depth;
}
this.senseManager.ReleaseFrame();
if (this.image == null)
{
return;
}
this.invalidate = true;
// get distance
PXCMImage.ImageData data;
ret = this.image.AcquireAccess(
PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out data
);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("Could not acquire Depth image.");
}
var info = this.image.QueryInfo();
var length = info.width * info.height;
var buffer = data.ToShortArray(0, length);
this.image.ReleaseAccess(data);
for (int i = 0; i < FInPosition.SliceCount; i++)
{
int index = (int)((FInPosition[i].y * this.width) + FInPosition[i].x);
int max = this.width * this.height;
float dst = 0.0f;
if (0 <= index && index <= max)
{
dst = (float)buffer[index];
}
FOutDistance.SliceCount = i + 1;
FOutDistance[i] = dst;
}
}
public void AdvancedPipeline()
{
PXCMSession session;
pxcmStatus sts = PXCMSession.CreateInstance(out session);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
form.UpdateStatus("Failed to create an SDK session");
return;
}
/* Set Module */
PXCMSession.ImplDesc desc = new PXCMSession.ImplDesc();
desc.friendlyName.set(form.GetCheckedModule());
PXCMEmotion emotionDet;
sts = session.CreateImpl <PXCMEmotion>(ref desc, PXCMEmotion.CUID, out emotionDet);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
form.UpdateStatus("Failed to create the emotionDet module");
session.Dispose();
return;
}
UtilMCapture capture = null;
if (form.GetRecordState())
{
capture = new UtilMCaptureFile(session, form.GetFileName(), true);
capture.SetFilter(form.GetCheckedDevice());
}
else if (form.GetPlaybackState())
{
capture = new UtilMCaptureFile(session, form.GetFileName(), false);
}
else
{
capture = new UtilMCapture(session);
capture.SetFilter(form.GetCheckedDevice());
}
form.UpdateStatus("Pair moudle with I/O");
for (uint i = 0; ; i++)
{
PXCMEmotion.ProfileInfo pinfo;
sts = emotionDet.QueryProfile(i, out pinfo);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
break;
}
sts = capture.LocateStreams(ref pinfo.inputs);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
sts = emotionDet.SetProfile(ref pinfo);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
break;
}
}
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
form.UpdateStatus("Failed to pair the emotionDet module with I/O");
capture.Dispose();
emotionDet.Dispose();
session.Dispose();
return;
}
form.UpdateStatus("Streaming");
PXCMImage[] images = new PXCMImage[PXCMCapture.VideoStream.STREAM_LIMIT];
PXCMScheduler.SyncPoint[] sps = new PXCMScheduler.SyncPoint[2];
while (!form.stop)
{
PXCMImage.Dispose(images);
PXCMScheduler.SyncPoint.Dispose(sps);
sts = capture.ReadStreamAsync(images, out sps[0]);
if (DisplayDeviceConnection(sts == pxcmStatus.PXCM_STATUS_DEVICE_LOST))
{
continue;
}
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
break;
}
sts = emotionDet.ProcessImageAsync(images, out sps[1]);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
break;
}
PXCMScheduler.SyncPoint.SynchronizeEx(sps);
sts = sps[0].Synchronize();
if (DisplayDeviceConnection(sts == pxcmStatus.PXCM_STATUS_DEVICE_LOST))
{
continue;
}
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
break;
}
/* Display Results */
DisplayPicture(capture.QueryImage(images, PXCMImage.ImageType.IMAGE_TYPE_COLOR));
DisplayLocation(emotionDet);
form.UpdatePanel();
}
PXCMImage.Dispose(images);
PXCMScheduler.SyncPoint.Dispose(sps);
capture.Dispose();
emotionDet.Dispose();
session.Dispose();
form.UpdateStatus("Stopped");
}
public RealSenseException(string message, pxcmStatus status)
: this(message)
{
this.status = status;
}
