public static PXCMPointI32 GetTrackedPoint(PXCMBlobExtractor.BlobData blobData, BlobUtilityClass.TrackingBlobPoint blobPointToTrack)
{
PXCMPointI32 point = new PXCMPointI32();
switch (blobPointToTrack)
{
case TrackingBlobPoint.BottomPoint:
point = blobData.bottomPoint;
break;
case TrackingBlobPoint.TopPoint:
point = blobData.topPoint;
break;
case TrackingBlobPoint.RightPoint:
point = blobData.rightPoint;
break;
case TrackingBlobPoint.LeftPoint:
point = blobData.leftPoint;
break;
case TrackingBlobPoint.CenterPoint:
//converting to int
point.x = (int)blobData.centerPoint.x;
point.y = (int)blobData.centerPoint.y;
break;
case TrackingBlobPoint.ClosestPoint:
point = blobData.closestPoint;
break;
}
return point;
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.VideoDepthStream))
{
trigger.ErrorDetected = true;
Debug.LogError("Blob Analysis Module Not Set");
return(false);
}
if (!(trigger is TrackTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
bool success = false;
// make sure we have valid values
if (RealWorldBoxDimensions.x <= 0)
{
RealWorldBoxDimensions.x = 1;
}
if (RealWorldBoxDimensions.y <= 0)
{
RealWorldBoxDimensions.y = 1;
}
if (RealWorldBoxDimensions.z <= 0)
{
RealWorldBoxDimensions.z = 1;
}
if (SenseToolkitManager.Instance != null && SenseToolkitManager.Instance.Initialized && SenseToolkitManager.Instance.BlobExtractor != null && SenseToolkitManager.Instance.ImageDepthOutput != null)
{
// Setting max distance for this rule and process the image
PXCMBlobExtractor.BlobData _blobData = new PXCMBlobExtractor.BlobData();
SenseToolkitManager.Instance.BlobExtractor.SetMaxDistance(MaxDistance * 10);
var sts = SenseToolkitManager.Instance.BlobExtractor.ProcessImage(SenseToolkitManager.Instance.ImageDepthOutput);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR && SenseToolkitManager.Instance.BlobExtractor.QueryNumberOfBlobs() > 0)
{
if (BlobIndex >= SenseToolkitManager.Instance.BlobExtractor.QueryNumberOfBlobs())
{
return(false);
}
PXCMImage.ImageInfo info = SenseToolkitManager.Instance.ImageDepthOutput.QueryInfo();
info.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
PXCMImage new_image = SenseToolkitManager.Instance.SenseManager.session.CreateImage(info);
// Process Tracking
SenseToolkitManager.Instance.BlobExtractor.QueryBlobData(BlobIndex, new_image, out _blobData);
new_image.Dispose();
TrackTrigger specificTrigger = (TrackTrigger)trigger;
PXCMPointI32 trackedPoint = BlobUtilityClass.GetTrackedPoint(_blobData, BlobPointToTrack);
PXCMImage.ImageData data;
SenseToolkitManager.Instance.ImageDepthOutput.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_F32, out data);
if (_depthArray == null)
{
_depthArray = new float[SenseToolkitManager.Instance.ImageDepthOutput.info.width * SenseToolkitManager.Instance.ImageDepthOutput.info.height];
}
data.ToFloatArray(0, _depthArray);
float depth = _depthArray[(int)trackedPoint.x + (int)trackedPoint.y * SenseToolkitManager.Instance.ImageDepthOutput.info.width];
if (_pos_uvz == null)
{
_pos_uvz = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
_pos_uvz[0].x = trackedPoint.x;
_pos_uvz[0].y = trackedPoint.y;
_pos_uvz[0].z = depth;
if (_pos3d == null)
{
_pos3d = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
SenseToolkitManager.Instance.Projection.ProjectDepthToCamera(_pos_uvz, _pos3d);
Vector3 position = new Vector3();
position.x = -_pos3d[0].x / 10;
position.y = _pos3d[0].y / 10;
position.z = _pos3d[0].z / 10;
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(data);
TrackingUtilityClass.ClampToRealWorldInputBox(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(position.x) && !float.IsNaN(position.y) && !float.IsNaN(position.z))
{
specificTrigger.Position = position;
}
else
{
return(false);
}
success = true;
}
}
else
{
return(false);
}
return(success);
}
public override bool Process(Trigger trigger)
{
trigger.ErrorDetected = false;
if (!SenseToolkitManager.Instance.IsSenseOptionSet(SenseOption.SenseOptionID.VideoDepthStream))
{
trigger.ErrorDetected = true;
Debug.LogError("Depth Stream Not Set");
return(false);
}
if (!(trigger is EventTrigger))
{
trigger.ErrorDetected = true;
return(false);
}
EventTrigger specificTrigger = (EventTrigger)trigger;
specificTrigger.Source = this.name;
bool success = false;
if (SenseToolkitManager.Instance.Initialized && SenseToolkitManager.Instance.BlobExtractor != null && SenseToolkitManager.Instance.ImageDepthOutput != null)
{
// Setting max distance for this rule and process the image
SenseToolkitManager.Instance.BlobExtractor.SetMaxDistance(MaxDistance * 10);
var sts = SenseToolkitManager.Instance.BlobExtractor.ProcessImage(SenseToolkitManager.Instance.ImageDepthOutput);
if (sts == pxcmStatus.PXCM_STATUS_NO_ERROR && SenseToolkitManager.Instance.BlobExtractor.QueryNumberOfBlobs() >= NumberOfBlobs)
{
int numberOfBlobsDetected = SenseToolkitManager.Instance.BlobExtractor.QueryNumberOfBlobs();
int blobsRightlyDetected = 0;
PXCMBlobExtractor.BlobData blobData = new PXCMBlobExtractor.BlobData();
// For each detected blob, project the tracked point to the real world and
// check that it is in our real world box
for (int i = 0; i < numberOfBlobsDetected; i++)
{
PXCMImage.ImageInfo info = SenseToolkitManager.Instance.ImageDepthOutput.QueryInfo();
info.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
PXCMImage new_image = SenseToolkitManager.Instance.SenseManager.session.CreateImage(info);
// Process Tracking
SenseToolkitManager.Instance.BlobExtractor.QueryBlobData(i, new_image, out blobData);
new_image.Dispose();
PXCMPointI32 trackedPoint = BlobUtilityClass.GetTrackedPoint(blobData, BlobPointToTrack);
PXCMImage.ImageData data;
SenseToolkitManager.Instance.ImageDepthOutput.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_F32, out data);
if (_depthArray == null)
{
_depthArray = new float[SenseToolkitManager.Instance.ImageDepthOutput.info.width * SenseToolkitManager.Instance.ImageDepthOutput.info.height];
}
data.ToFloatArray(0, _depthArray);
float depth = _depthArray[(int)trackedPoint.x + (int)trackedPoint.y * SenseToolkitManager.Instance.ImageDepthOutput.info.width];
if (_pos_uvz == null)
{
_pos_uvz = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
_pos_uvz[0].x = trackedPoint.x;
_pos_uvz[0].y = trackedPoint.y;
_pos_uvz[0].z = depth;
if (_pos3d == null)
{
_pos3d = new PXCMPoint3DF32[1] {
new PXCMPoint3DF32()
};
}
SenseToolkitManager.Instance.Projection.ProjectDepthToCamera(_pos_uvz, _pos3d);
Vector3 position = new Vector3();
position.x = -_pos3d[0].x / 10;
position.y = _pos3d[0].y / 10;
position.z = _pos3d[0].z / 10;
SenseToolkitManager.Instance.ImageDepthOutput.ReleaseAccess(data);
TrackingUtilityClass.ClampToRealWorldInputBox(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
TrackingUtilityClass.Normalize(ref position, RealWorldBoxCenter, RealWorldBoxDimensions);
if (!float.IsNaN(position.x) && !float.IsNaN(position.y) && !float.IsNaN(position.z))
{
if (position.x > 0 && position.x < 1 &&
position.y > 0 && position.y < 1 &&
position.z > 0 && position.z < 1)
{
blobsRightlyDetected++;
if (blobsRightlyDetected == NumberOfBlobs)
{
break;
}
}
}
else
{
_lastFrameDetected = false;
return(false);
}
}
if (blobsRightlyDetected >= NumberOfBlobs)
{
if (!_lastFrameDetected)
{
success = true;
}
_lastFrameDetected = true;
}
else
{
_lastFrameDetected = false;
}
}
else
{
_lastFrameDetected = false;
}
}
else
{
return(false);
}
return(success);
}
