// sets up depth2color calibration data for Astra & Astra-Pro
public void SetupCalibrationData(bool bAstraPro)
{
instance = this;
if (!bAstraPro)
{
// Astra
this.lCamInt1 = new Vector3(288.126f, 0f, 156.578f);
this.lCamInt2 = new Vector3(0f, 288.780f, 124.968f);
this.lCamInt3 = new Vector3(0f, 0f, 1f);
this.rCamInt1 = new Vector3(256.204f, 0f, 163.978f);
this.rCamInt2 = new Vector3(0f, 256.450f, 118.382f);
this.rCamInt3 = new Vector3(0f, 0f, 1f);
this.r2lRot1 = new Vector3(0.999983f, 0.00264383f, -0.0052673f);
this.r2lRot2 = new Vector3(-0.00264698f, 0.999996f, -0.000589696f);
this.r2lRot3 = new Vector3(0.00526572f, 0.000603628f, 0.999986f);
this.r2lTrans = new Vector3(-24.2641f, -0.439535f, -0.577864f);
this.vMatAdjustX = new Vector4(0f, 0f, 32.53856f, 0f);
this.vMatAdjustY = new Vector4(0f, 0f, 5.3296f, 0f);
// this.vMatAdjustX = new Vector4(0.05353134f, 0f, 0f, 2400f);
// this.vMatAdjustY = new Vector4(0f, 0f, 0f, 9000f);
}
else
{
// Astra Pro
this.lCamInt1 = new Vector3(574.679f, 0f, 318.731f);
this.lCamInt2 = new Vector3(0f, 536.747f, 244.835f);
this.lCamInt3 = new Vector3(0f, 0f, 1f);
this.rCamInt1 = new Vector3(590.402f, 0f, 324.859f);
this.rCamInt2 = new Vector3(0f, 551.584f, 231.235f);
this.rCamInt3 = new Vector3(0f, 0f, 1f);
this.r2lRot1 = new Vector3(0.999874f, 0.0158587f, -0.000310587f);
this.r2lRot2 = new Vector3(-0.015858f, 0.999872f, 0.00199448f);
this.r2lRot3 = new Vector3(0.000342177f, -0.00198931f, 0.999998f);
this.r2lTrans = new Vector3(-24.9939f, -0.087019f, -0.091892f);
this.vMatAdjustX = new Vector4(0f, 0f, 0f, 19370.21f); //3.
this.vMatAdjustY = new Vector4(0f, 0f, 0f, 0f);
// this.vMatAdjustX = new Vector4(0f, 0f, 0f, 9880f);
// this.vMatAdjustY = new Vector4(0f, 0f, 0f, 20000f);
}
SetupMatricesRow();
}
public void CloseSensor(KinectInterop.SensorData sensorData)
{
if (colorWebCam)
{
colorWebCam.Stop();
colorWebCam = null;
}
if (coordMapper != null)
{
coordMapper.CleanUp();
coordMapper = null;
}
ShutdownAstraInterface();
// try to unload the library, to prevent the crash
//KinectInterop.UnloadNativeLib("OrbbecAstraInterface.dll");
Debug.Log("OrbbecAstra sensor closed");
}
public void CloseSensor(KinectInterop.SensorData sensorData)
{
if (colorWebCam)
{
colorWebCam.Stop();
colorWebCam = null;
}
if (coordMapper != null)
{
coordMapper.CleanUp();
coordMapper = null;
}
if (colorSensor != null)
{
colorSensor.OnUpdateEvent -= HandleOnColorSensorUpdateEvent;
colorSensor = null;
colorFrame = null;
}
if (depthSensor != null)
{
depthSensor.OnUpdateEvent -= HandleOnDepthSensorUpdateEvent;
depthSensor = null;
depthFrame = null;
}
if (userTracker != null)
{
userTracker.OnUpdateEvent -= HandleOnUserTrackerUpdateEvent;
userTracker = null;
userFrame = null;
}
if (skeletonTracker != null)
{
skeletonTracker.OnSkeletonUpdateEvent -= HandleOnSkeletonUpdateEvent;
skeletonTracker = null;
skeletonData = null;
}
if (handTracker != null)
{
handTracker.OnUpdateEvent -= HandleOnHandsUpdateEvent;
handTracker = null;
handTrackerData = null;
}
if (gestureRecognizer != null)
{
gestureRecognizer.OnNewGesturesEvent -= OnNewGestures;
gestureRecognizer = null;
}
if (bNuitrackInited)
{
bNuitrackInited = false;
nuitrack.Nuitrack.onIssueUpdateEvent -= OnIssuesUpdate;
NuitrackTerm();
}
Debug.Log("Nuitrack sensor closed");
}
public KinectInterop.SensorData OpenDefaultSensor(KinectInterop.FrameSource dwFlags, float sensorAngle, bool bUseMultiSource)
{
// init interface
if (!bNuitrackInited)
{
bNuitrackInited = NuitrackInit();
if (!bNuitrackInited)
{
return(null);
}
}
sensorFlags = dwFlags;
bMultiSource = bUseMultiSource;
KinectInterop.SensorData sensorData = new KinectInterop.SensorData();
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
for (int i = 0; i < WebCamTexture.devices.Length; i++)
{
if (WebCamTexture.devices[i].name.IndexOf("astra", StringComparison.CurrentCultureIgnoreCase) >= 0)
{
Debug.Log(" " + WebCamTexture.devices[i].name + "- AstraPro detected.");
colorWebCam = new WebCamTexture(WebCamTexture.devices[i].name, 640, 480, 30);
break;
}
}
if (!colorWebCam)
{
colorSensor = nuitrack.ColorSensor.Create();
colorSensor.OnUpdateEvent += HandleOnColorSensorUpdateEvent;
}
else
{
bWebColorStream = true;
colorWebCam.Play();
sensorData.colorImageTexture = colorWebCam;
}
}
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
depthSensor = nuitrack.DepthSensor.Create();
// depthSensor.SetMirror (true);
depthSensor.OnUpdateEvent += HandleOnDepthSensorUpdateEvent;
}
if ((dwFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0)
{
userTracker = nuitrack.UserTracker.Create();
userTracker.OnUpdateEvent += HandleOnUserTrackerUpdateEvent;
}
if ((dwFlags & KinectInterop.FrameSource.TypeBody) != 0)
{
skeletonTracker = nuitrack.SkeletonTracker.Create();
skeletonTracker.OnSkeletonUpdateEvent += HandleOnSkeletonUpdateEvent;
handTracker = nuitrack.HandTracker.Create();
handTracker.OnUpdateEvent += HandleOnHandsUpdateEvent;
gestureRecognizer = nuitrack.GestureRecognizer.Create();
gestureRecognizer.OnNewGesturesEvent += OnNewGestures;
}
// if((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
// {
// }
nuitrack.Nuitrack.onIssueUpdateEvent += OnIssuesUpdate;
nuitrack.Nuitrack.Run();
sensorData.bodyCount = Constants.SkeletonCount;
sensorData.jointCount = Constants.JointCount;
sensorData.depthCameraOffset = 0f;
sensorData.faceOverlayOffset = 0f;
if (!bWebColorStream)
{
// // wait for color frame
// if (colorSensor != null)
// {
// colorFrame = colorSensor.GetColorFrame();
// float waitTillTime = Time.realtimeSinceStartup + 2.5f;
//
// while (colorFrame == null && Time.realtimeSinceStartup <= waitTillTime)
// {
// nuitrack.Nuitrack.Update();
// System.Threading.Thread.Sleep(50);
// colorFrame = colorSensor.GetColorFrame();
// }
// }
sensorData.colorImageWidth = colorSensor != null?colorSensor.GetOutputMode().XRes : 640;
sensorData.colorImageHeight = colorSensor != null?colorSensor.GetOutputMode().YRes : 480;
// flip color image vertically
sensorData.colorImageScale = new Vector3(1f, -1f, 1f);
}
else
{
sensorData.colorImageWidth = colorWebCam.width;
sensorData.colorImageHeight = colorWebCam.height;
// flip color image horizontally
sensorData.colorImageScale = new Vector3(-1f, 1f, 1f);
}
Debug.Log(" Color sensor: " + (colorSensor != null ? colorSensor.ToString() : "-") +
", width: " + sensorData.colorImageWidth + ", height: " + sensorData.colorImageHeight);
// // wait for depth frame
// if (depthSensor != null)
// {
// depthFrame = depthSensor.GetDepthFrame();
// float waitTillTime = Time.realtimeSinceStartup + 2.5f;
//
// while (depthFrame == null && Time.realtimeSinceStartup <= waitTillTime)
// {
// nuitrack.Nuitrack.Update();
// System.Threading.Thread.Sleep(50);
// depthFrame = depthSensor.GetDepthFrame();
// }
// }
sensorData.depthImageWidth = depthSensor != null?depthSensor.GetOutputMode().XRes : 640;
sensorData.depthImageHeight = depthSensor != null?depthSensor.GetOutputMode().YRes : 480;
Debug.Log(" Depth sensor: " + (depthSensor != null ? depthSensor.ToString() : "-") +
", width: " + sensorData.depthImageWidth + ", height: " + sensorData.depthImageHeight);
// color & depth FOV
float hfovr = colorSensor != null?colorSensor.GetOutputMode().HFOV : 0f;
float vfovr = 2f * Mathf.Atan(Mathf.Tan(hfovr / 2f) * sensorData.depthImageHeight / sensorData.depthImageWidth);
sensorData.colorCameraFOV = vfovr * Mathf.Rad2Deg;
hfovr = depthSensor != null?depthSensor.GetOutputMode().HFOV : 0f;
vfovr = 2f * Mathf.Atan(Mathf.Tan(hfovr / 2f) * sensorData.depthImageHeight / sensorData.depthImageWidth);
sensorData.depthCameraFOV = vfovr * Mathf.Rad2Deg;
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
int colorImageSize = !colorWebCam ? (sensorData.colorImageWidth * sensorData.colorImageHeight * 3) : 0;
sensorData.colorImage = new byte[colorImageSize];
if (!colorWebCam)
{
sensorData.colorImageTexture2D = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGB24, false);
}
}
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
int depthImageSize = sensorData.depthImageWidth * sensorData.depthImageHeight;
sensorData.depthImage = new ushort[depthImageSize];
}
if ((dwFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0)
{
int bodyIndexImageSize = sensorData.depthImageWidth * sensorData.depthImageHeight;
sensorData.bodyIndexImage = new byte[bodyIndexImageSize];
}
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
{
int depthImageSize = sensorData.depthImageWidth * sensorData.depthImageHeight;
sensorData.infraredImage = new ushort[depthImageSize];
}
// setup coordinate mapper
coordMapper = new OrbbecAstraMapper();
coordMapper.SetupSpaceMapping(sensorData.depthImageWidth, sensorData.depthImageHeight, hfovr, vfovr);
// d2c-calibration data is valid for Orbbec-Astra only (sensor id & calib.data not provided by Nuitrack)
coordMapper.SetupCalibrationData(bWebColorStream);
// set lost-user time tolerance equal to KM
if (KinectManager.Instance != null)
{
waitTimeBeforeRemove = KinectManager.Instance.waitTimeBeforeRemove;
}
Debug.Log("Nuitrack sensor opened");
return(sensorData);
}
public KinectInterop.SensorData OpenDefaultSensor(KinectInterop.FrameSource dwFlags, float sensorAngle, bool bUseMultiSource)
{
// init interface
int hr = InitAstraInterface();
if (hr != 0)
{
return(null);
}
sensorFlags = dwFlags;
bMultiSource = bUseMultiSource;
KinectInterop.SensorData sensorData = new KinectInterop.SensorData();
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
hr = OpenAstraColorStream();
// try to get a color frame
hr = PollColorFrame(500);
if (hr == 0)
{
ReleaseColorFrame();
Debug.Log("Astra-sensor detected");
}
if (hr != 0)
{
bWebColorStream = true;
isAstraPro = true;
Debug.Log("AstraPro camera detected.");
for (int i = 0; i < WebCamTexture.devices.Length; i++)
{
Debug.Log(WebCamTexture.devices [i].name);
if (WebCamTexture.devices[i].name.IndexOf("astra", StringComparison.CurrentCultureIgnoreCase) >= 0)
{
colorWebCam = new WebCamTexture(WebCamTexture.devices[i].name, 640, 480, 30);
break;
}
}
if (colorWebCam)
{
colorWebCam.Play();
sensorData.colorImageWidth = colorWebCam.width;
sensorData.colorImageHeight = colorWebCam.height;
sensorData.colorImageTexture = colorWebCam;
//Debug.Log("Webcam - vMirrored: " + colorWebCam.videoVerticallyMirrored + ", rotAngle: " + colorWebCam.videoRotationAngle);
}
}
}
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
hr = OpenAstraDepthStream();
// try to get a depth frame
hr = PollDepthFrame(500);
if (hr == 0)
{
ReleaseDepthFrame();
}
}
if (((dwFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0) ||
((dwFlags & KinectInterop.FrameSource.TypeBody) != 0))
{
hr = OpenAstraBodyStream();
if (hr == 0)
{
obtBody = new ObtBody();
obtBody.joints = new ObtJoint[OBT_MAX_JOINTS];
obtBodyInited = true;
}
}
// if((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
// {
// }
sensorData.bodyCount = Constants.SkeletonCount;
sensorData.jointCount = Constants.JointCount;
sensorData.depthCameraFOV = 45.64f;
sensorData.colorCameraFOV = 45.64f;
sensorData.depthCameraOffset = 0f;
sensorData.faceOverlayOffset = 0f;
if (!bWebColorStream)
{
sensorData.colorImageWidth = GetColorWidth();
sensorData.colorImageHeight = GetColorHeight();
// flip color image vertically
sensorData.colorImageScale = new Vector3(1f, -1f, 1f);
}
else
{
// flip color image horizontally
sensorData.colorImageScale = new Vector3(-1f, 1f, 1f);
}
sensorData.depthImageWidth = GetDepthWidth();
sensorData.depthImageHeight = GetDepthHeight();
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
int colorImageSize = !colorWebCam ? (GetColorDataSize() * 4 / 3) : 0;
sensorData.colorImage = new byte[colorImageSize];
}
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
int depthImageSize = GetDepthDataSize() / sizeof(ushort);
sensorData.depthImage = new ushort[depthImageSize];
}
if ((dwFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0)
{
int bodyIndexImageSize = GetBodyIndexDataSize();
sensorData.bodyIndexImage = new byte[bodyIndexImageSize];
}
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
{
int depthImageSize = GetDepthDataSize() / sizeof(ushort);
sensorData.infraredImage = new ushort[depthImageSize];
}
// setup coordinate mapper
coordMapper = new OrbbecAstraMapper();
coordMapper.SetupSpaceMapping(sensorData.depthImageWidth, sensorData.depthImageHeight, 1.0226f, 0.7966157f); // hfov, vfov in rad
coordMapper.SetupCalibrationData(isAstraPro);
// set lost-user time tolerance equal to KM
if (KinectManager.Instance != null)
{
waitTimeBeforeRemove = KinectManager.Instance.waitTimeBeforeRemove;
}
// enable depth-to-color sync, if needed
if (bMultiSource && ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0) && ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0))
{
//hr = EnableDepthColorSync(1);
}
Debug.Log("OrbbecAstra sensor opened");
return(sensorData);
}
