void Update()
{
if (sm.AcquireFrame(false, 0) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
/* Query and return available image samples*/
PXCMCapture.Sample sample = sm.QuerySample();
if (sample != null && sample.depth != null)
{
/* Query depth image properties*/
PXCMImage.ImageInfo info = sample.depth.QueryInfo();
if (isInitBlob == false)
{
/* Initialize the blob extraction algorithm*/
m_blob.Init(info);
isInitBlob = true;
}
if (isInitContour == false)
{
/* Initialize the contour extraction algorithm*/
m_contour.Init(info);
isInitContour = true;
}
ProcessImage(sample.depth);
}
sm.ReleaseFrame();
}
}
private void DisplayPicture(PXCMImage depth, PXCMGesture gesture)
{
PXCMImage image = depth;
bool dispose = false;
if (form.GetLabelmapState())
{
if (gesture.QueryBlobImage(PXCMGesture.Blob.Label.LABEL_SCENE, 0, out image) < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
dispose = true;
}
PXCMImage.ImageData data;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
PXCMImage.ImageInfo imageInfo = image.imageInfo;
form.DisplayBitmap(data.ToBitmap(imageInfo.width, imageInfo.height));
image.ReleaseAccess(ref data);
}
if (dispose)
{
image.Dispose();
}
}
void ProcessImage(PXCMImage depth)
{
if (depth == null)
{
return;
}
/* Returns the image properties. */
info = depth.QueryInfo();
/* Set the pixel format*/
info.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
/* Perform blob extraction on the specified depth image.*/
m_blob.ProcessImage(depth);
/* Create an instance of the PXC[M]Image interface to manage image buffer access. */
new_image = mask_utils.PipelineManager.session.CreateImage(info);
results = m_blob.QueryBlobData(0, new_image, out blobData[0]);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
/* Get all blob points */
blobPointsPos = new Vector2[6];
blobPointsPos[0] = new Vector2(blobData[0].centerPoint.x * -1, blobData[0].centerPoint.y * -1);
blobPointsPos[1] = new Vector2(blobData[0].topPoint.x * -1, blobData[0].topPoint.y * -1);
blobPointsPos[2] = new Vector2(blobData[0].bottomPoint.x * -1, blobData[0].bottomPoint.y * -1);
blobPointsPos[3] = new Vector2(blobData[0].leftPoint.x * -1, blobData[0].leftPoint.y * -1);
blobPointsPos[4] = new Vector2(blobData[0].rightPoint.x * -1, blobData[0].rightPoint.y * -1);
blobPointsPos[5] = new Vector2(blobData[0].closestPoint.x * -1, blobData[0].closestPoint.y * -1);
/* Sert blob points colors*/
blobPointColors = new Color[6];
blobPointColors[0] = Color.blue;
blobPointColors[1] = Color.yellow;
blobPointColors[2] = Color.yellow;
blobPointColors[3] = Color.yellow;
blobPointColors[4] = Color.yellow;
blobPointColors[5] = Color.green;
DisplayPoints();
results = m_contour.ProcessImage(new_image);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR && m_contour.QueryNumberOfContours() > 0)
{
/* Retrieve the detected contour points.*/
results = m_contour.QueryContourData(0, out pointOuter[0]);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (pointOuter[0] != null && pointOuter[0].Length > 0)
{
DisplayContour(pointOuter[0], 0);
}
}
}
}
new_image.Dispose();
}
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];
}
void ProcessImage(PXCMImage depth)
{
if (depth == null)
{
return;
}
/* Returns the image properties. */
info = depth.QueryInfo();
/* Set the pixel format*/
info.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
/* Perform blob extraction on the specified depth image.*/
m_blob.ProcessImage(depth);
/* Create an instance of the PXC[M]Image interface to manage image buffer access. */
new_image = mask_utils.PipelineManager.session.CreateImage(info);
int blobCount = m_blob.QueryNumberOfBlobs();
/* To store all blob points */
blobPointsPos = new List <Vector2>();
/* To store all contour points */
countourPoints = new List <PXCMPointI32>();
for (int i = 0; i < blobCount; i++)
{
results = m_blob.QueryBlobData(i, new_image, out blobData[i]);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
blobPointsPos.Add(new Vector2(blobData[i].centerPoint.x * -1, blobData[i].centerPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].topPoint.x * -1, blobData[i].topPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].bottomPoint.x * -1, blobData[i].bottomPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].leftPoint.x * -1, blobData[i].leftPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].rightPoint.x * -1, blobData[i].rightPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].closestPoint.x * -1, blobData[i].closestPoint.y * -1));
DisplayPoints();
results = m_contour.ProcessImage(new_image);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR && m_contour.QueryNumberOfContours() > 0)
{
results = m_contour.QueryContourData(0, out pointOuter[i]);
DisplayContour(pointOuter[i], i);
}
}
}
new_image.Dispose();
}
private void DisplayPicture(PXCMImage image)
{
PXCMImage.ImageData data;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
PXCMImage.ImageInfo imageInfo = image.imageInfo;
var bmp = data.ToBitmap(imageInfo.width, imageInfo.height);
if (ImageAvailable != null)
{
ImageAvailable(bmp);
}
image.ReleaseAccess(ref data);
}
}
public CaptureHand(CaptureForm form) : base()
{
pp = new UtilMPipeline();
pp.EnableGesture();
pp.EnableImage(PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH);
if (pp.Init())
{
pp.QueryCapture().SetFilter(PXCMCapture.Device.Property.PROPERTY_DEPTH_SMOOTHING, 100);
do
{
if (!pp.AcquireFrame(true))
{
MessageBox.Show("Failed to aquire a frame.", "Kwi-S", MessageBoxButtons.OK, MessageBoxIcon.Asterisk);
break;
}
gesture = pp.QueryGesture();
gesture.QueryBlobData(PXCMGesture.Blob.Label.LABEL_SCENE, 0, out bdata);
gesture.QueryBlobImage(PXCMGesture.Blob.Label.LABEL_SCENE, 0, out bimage);
bimage.AcquireAccess(PXCMImage.Access.ACCESS_READ, out data);
info = bimage.imageInfo;
color = (int)bdata.labelLeftHand;
form.setTextBox(color.ToString());
pp.ReleaseFrame();
} while (color == -1);
Coord[] locations = handLocation(data, (int)(info.width * info.height), (int)info.width);
bitmap = createBitmap(locations, (int)info.width, (int)info.height);
form.setImage(bitmap);
form.bitmap = bitmap;
}
}
/// <summary>
/// Grab the blob data every frame
/// </summary>
void Update()
{
if (instance.AcquireFrame(true) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
/* To store all blob points */
blobPointsPos = new List <Vector2>();
PXCMCapture.Sample sample = instance.QuerySample();
if (sample != null && sample.depth != null)
{
PXCMImage.ImageInfo info = sample.depth.QueryInfo();
if (blobData != null)
{
blobData.Update();
int numblobs = blobData.QueryNumberOfBlobs();
for (int i = 0; i <= numblobs; i++)
{
PXCMBlobData.IBlob pBlob;
if (blobData.QueryBlobByAccessOrder(i, PXCMBlobData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR, out pBlob) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Vector3 centerPoint = pBlob.QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_CENTER);
Vector3 topPoint = pBlob.QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_TOP_MOST);
Vector3 bottomPoint = pBlob.QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_BOTTOM_MOST);
Vector3 leftPoint = pBlob.QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_LEFT_MOST);
Vector3 rightPoint = pBlob.QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_RIGHT_MOST);
Vector3 closestPoint = pBlob.QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_CLOSEST);
blobPointsPos.Add(new Vector2(centerPoint.x * -1, centerPoint.y * -1));
blobPointsPos.Add(new Vector2(topPoint.x * -1, topPoint.y * -1));
blobPointsPos.Add(new Vector2(bottomPoint.x * -1, bottomPoint.y * -1));
blobPointsPos.Add(new Vector2(leftPoint.x * -1, leftPoint.y * -1));
blobPointsPos.Add(new Vector2(rightPoint.x * -1, rightPoint.y * -1));
blobPointsPos.Add(new Vector2(closestPoint.x * -1, closestPoint.y * -1));
DisplayPoints();
if (pBlob.QueryContourPoints(0, out pointOuter[i]) == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
DisplayContour(pointOuter[i], i, numblobs);
}
}
}
}
}
instance.ReleaseFrame();
}
}
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 CaptureHand(CaptureForm form)
: base()
{
pp = new UtilMPipeline();
pp.EnableGesture();
pp.EnableImage(PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH);
if (pp.Init())
{
pp.QueryCapture().SetFilter(PXCMCapture.Device.Property.PROPERTY_DEPTH_SMOOTHING, 100);
do
{
if (!pp.AcquireFrame(true))
{
MessageBox.Show("Failed to aquire a frame.", "Kwi-S", MessageBoxButtons.OK, MessageBoxIcon.Asterisk);
break;
}
gesture = pp.QueryGesture();
gesture.QueryBlobData(PXCMGesture.Blob.Label.LABEL_SCENE, 0, out bdata);
gesture.QueryBlobImage(PXCMGesture.Blob.Label.LABEL_SCENE, 0, out bimage);
bimage.AcquireAccess(PXCMImage.Access.ACCESS_READ,out data);
info = bimage.imageInfo;
color =(int) bdata.labelLeftHand;
form.setTextBox(color.ToString());
pp.ReleaseFrame();
} while (color == -1);
Coord[] locations = handLocation(data, (int)(info.width * info.height),(int) info.width);
bitmap = createBitmap(locations, (int)info.width, (int)info.height);
form.setImage(bitmap);
form.bitmap = bitmap;
}
}
public void Loop(LoopObjects loopObjects)
{
var segmentation = _camera.Manager.Query3DSeg();
if (segmentation == null)
{
return;
}
PXCMImage image = segmentation.AcquireSegmentedImage();
PXCMImage.ImageData imageData;
image.AcquireAccess(PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32,
out imageData);
PXCMImage.ImageInfo imageInfo = image.QueryInfo();
using (var bitmap = new Bitmap(imageData.ToBitmap(0, imageInfo.width, imageInfo.height))) {
using (var ms = new MemoryStream()) {
bitmap.Save(ms, ImageFormat.Bmp);
_camera.SegmentationStream.CurrentBitmapImage = ms.ToArray();
image.ReleaseAccess(imageData);
}
}
}
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);
}
private void RunLoop()
{
/* IEnumerable<PXCMCapture.DeviceInfo> devices = this.GetDevices();
if (devices.Count() == 0)
{
throw new Smithers.Reading.FrameData.ScannerNotFoundException("No devices found");
}
PXCMCapture.DeviceInfo device = devices.First();
if (devices.Count() > 1)
{
Console.WriteLine(String.Format("More than one device, using this one: {0}", device.name));
}
_senseManager.captureManager.FilterByDeviceInfo(device);*/
pxcmStatus status;
status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, Frame.COLOR_WIDTH, Frame.COLOR_HEIGHT, Frame.COLOR_RATE);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
_logger.info("Unable to enable color stream:" + status.ToString());
throw new Smithers.Reading.FrameData.ScannerNotFoundException("Unable to enable color stream");
}
_logger.info("Color Stream Enabled:" + status.ToString());
//status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 0, 0, Frame.DEPTH_RATE);
status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, Frame.DEPTH_WIDTH, Frame.DEPTH_HEIGHT, Frame.DEPTH_RATE);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
_logger.info("Unable to enable depth stream:" + status.ToString());
throw new Smithers.Reading.FrameData.ScannerNotFoundException("Unable to enable depth stream");
}
_logger.info("Depth Stream Enabled:" + status.ToString());
//status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_LEFT, Frame.DEPTH_WIDTH, Frame.DEPTH_HEIGHT, Frame.DEPTH_RATE);
//if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
//{
// throw new Smithers.Reading.FrameData.ScannerNotFoundException("Unable to enable depth stream");
//}
//status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_RIGHT, Frame.DEPTH_WIDTH, Frame.DEPTH_HEIGHT, Frame.DEPTH_RATE);
//if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
//{
// throw new Smithers.Reading.FrameData.ScannerNotFoundException("Unable to enable depth stream");
//}
status = _senseManager.EnableScenePerception();
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
_logger.info("Unable to enable scene perception:" + status.ToString());
throw new Smithers.Reading.FrameData.ScannerNotFoundException("Scene Perception failed");
}
_logger.info("Scene Perception Enabled:" + status.ToString());
_perceptionHandle = _senseManager.QueryScenePerception();
status = _senseManager.Init();
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
_logger.info("Unable to open sensor in the above mode:" + status.ToString());
throw new Smithers.Reading.FrameData.ScannerNotFoundException("Init failed");
}
_logger.info("Sensor Initialized Successfully:" + status.ToString());
PXCMImage.ImageInfo depthInfo = new PXCMImage.ImageInfo();
PXCMImage.ImageInfo colorInfo = new PXCMImage.ImageInfo();
depthInfo.height = Frame.DEPTH_HEIGHT;
depthInfo.width = Frame.DEPTH_WIDTH;
colorInfo.height = Frame.COLOR_HEIGHT;
colorInfo.width = Frame.COLOR_WIDTH;
/* For UV Mapping & Projection only: Save certain properties */
Projection projection = new Projection(_senseManager.session, _senseManager.captureManager.device, depthInfo, colorInfo);
AccelerometerReading accelerometerReading;
AccelerometerStats acceleroStats = new AccelerometerStats();
//bool firstFrameRecord = true;
// bool firstFrameNotRecord = false;
//pxcmStatus stsInitRecord = pxcmStatus.PXCM_STATUS_PROCESS_FAILED;
while (!_stop)
{
//PXCMCapture.Device.MirrorMode mirrorMode = this.Mirrored ? PXCMCapture.Device.MirrorMode.MIRROR_MODE_HORIZONTAL : PXCMCapture.Device.MirrorMode.MIRROR_MODE_DISABLED;
//{
// _senseManager.captureManager.device.SetMirrorMode(mirrorMode);
//}
/* Wait until a frame is ready: Synchronized or Asynchronous */
//if ((Record || Playback) && firstFrameRecord)
//{
// stsInitRecord = InitRecordPlay();
// if (this.StartCapture != null)
// {
// EventArgs eventArgs = new EventArgs();
// this.StartCapture(this, eventArgs);
// }
// firstFrameRecord = false;
// firstFrameNotRecord = true;
//}
//if (!Record && !Playback && firstFrameNotRecord)
//{
// InitRecordPlay();
// if (this.ShotSavedSuccess != null)
// {
// EventArgs eventArgs = new EventArgs();
// this.ShotSavedSuccess(this, eventArgs);
// }
// firstFrameRecord = true;
// firstFrameNotRecord = false;
//}
//-------
status = _senseManager.AcquireFrame(this.Synced);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
MemoryFrame frame = _pool.ObjectFromPool();
///* Display images */
PXCMCapture.Sample sample;
sample = _senseManager.QueryScenePerceptionSample();
if (sample == null)
sample = _senseManager.QuerySample();
//if (_perceptionHandle != null && sample !=null)
//{
// _perceptionHandle.GetCameraPose(frame.CameraPose);
// frame.TrackingAccuracy = _perceptionHandle.QueryTrackingAccuracy().ToString();
// frame.VoxelResolution = _perceptionHandle.QueryVoxelResolution().ToString();
// float sceneQuality = _perceptionHandle.CheckSceneQuality(sample);
// if (sceneQuality > 0.3f && _perceptionPaused && !_capturing)
// {
// _perceptionPaused = false;
// _senseManager.PauseScenePerception(false);
// _perceptionHandle.Reset(_initPose);
// }
// else if (!(frame.TrackingAccuracy == "HIGH" || frame.TrackingAccuracy == "MID") && !_capturing)
// {
// ResetScenePerception();
// }
//}
//if (this.Accelerometer != null)
//{
// accelerometerReading = Accelerometer.GetCurrentReading();
// acceleroStats.accX = accelerometerReading.AccelerationX;
// acceleroStats.accY = accelerometerReading.AccelerationY;
// acceleroStats.accZ = accelerometerReading.AccelerationZ;
// acceleroStats.timestamp = accelerometerReading.Timestamp;
// frame.AcceleroStats = acceleroStats;
//}
if (sample.color != null)
{
sample.color.CopyToByteArray(frame.BufferColor);
frame.ColorTimeStamp = sample.color.QueryTimeStamp();
}
if (sample.depth != null)
{
sample.depth.CopyToByteArray(frame.BufferDepth);
sample.depth.CopyToByteArray(frame.BufferDepthPreview, true);
frame.DepthTimeStamp = sample.depth.timeStamp;
}
#if DEBUG
if (sample.left != null)
{
sample.left.CopyToByteArray(frame.BufferIRLeft);
}
if (sample.right != null)
{
sample.right.CopyToByteArray(frame.BufferIRRight);
}
// Get mapping.
if (sample.depth != null)
{
projection.DepthToCameraCoordinates(sample.depth, frame.DepthToCameraMapping);
}
#endif
//frame.LowConfidenceDepthValue = projection.InvalidDepthValue;
using (Handle<MemoryFrame> frameHandle = new Handle<MemoryFrame>(frame, _pool))
{
FrameArrivedEventArgs eventArgs = new FrameArrivedEventArgs() { FrameHandle = frameHandle };
if (this.FrameArrived != null)
{
this.FrameArrived(this, eventArgs);
}
}
_senseManager.ReleaseFrame();
//int bitmap_width, bitmap_height;
//byte[] bitmap_data;
//PXCMCapture.StreamType[] streams = form.QueryStreams();
//bool streamState = false;
//for (int s = 0; s < PXCMCapture.STREAM_LIMIT; s++)
//{
// streamState = streamState || form.GetStreamState(PXCMCapture.StreamTypeFromIndex(s));
//}
//if (streamState)
//{
// for (int inx = 0; inx < 2; inx++)
// if (sample[streams[inx]] != null)
// {
// form.SetImage(inx, sample[streams[inx]]);
// }
//}
//else
//{
// if (form.GetProjectionState() && sample.color != null && sample.depth != null)
// {
// bitmap_data = projection.DepthToColorCoordinatesByFunction(sample.color, sample.depth, form.GetDotsState(), out bitmap_width, out bitmap_height);
// form.SetBitmap(0, bitmap_width, bitmap_height, bitmap_data);
// }
// if (form.GetUVMapState() && sample.color != null && sample.depth != null)
// {
// bitmap_data = projection.DepthToColorCoordinatesByUVMAP(sample.color, sample.depth, form.GetDotsState(), out bitmap_width, out bitmap_height);
// form.SetBitmap(0, bitmap_width, bitmap_height, bitmap_data);
// }
// if (form.GetInvUVMapState() && sample.color != null && sample.depth != null)
// {
// bitmap_data = projection.ColorToDepthCoordinatesByInvUVMap(sample.color, sample.depth, form.GetDotsState(), out bitmap_width, out bitmap_height);
// form.SetBitmap(0, bitmap_width, bitmap_height, bitmap_data);
// }
// if (sample.depth != null)
// {
// bitmap_data = GetRGB32Pixels(sample.depth, out bitmap_width, out bitmap_height);
// form.SetBitmap(1, bitmap_width, bitmap_height, bitmap_data);
// }
//}
//form.UpdatePanel();
}
//projection.Dispose();
}
public RealSenseEventArgs(PXCMImage.ImageData source, PXCMImage.ImageInfo info)
: this()
{
this.source = source;
this.info = info;
}
//public float FrameRate
//{
// get
// {
// return RangeF32.min;
// }
//}
public Resolution_ColorDepth_FrameRateCombination(PXCMImage.ImageInfo imageInfo, PXCMRangeF32 frameRate)
{
ImageInfo = imageInfo;
FrameRate = frameRate;
}
public PXCMImage ConvertToPXCMImage(Bitmap bitmap)
{
/* Get a system memory allocator */
PXCMAccelerator accel;
form.session.CreateAccelerator(out accel);
PXCMImage.ImageInfo iinfo = new PXCMImage.ImageInfo();
iinfo.width = (uint)bitmap.Width;
iinfo.height = (uint)bitmap.Height;
iinfo.format = PXCMImage.ColorFormat.COLOR_FORMAT_RGB32;
/* Create the image */
PXCMImage image;
accel.CreateImage(ref iinfo, out image);
PXCMImage.ImageData idata;
image.AcquireAccess(PXCMImage.Access.ACCESS_WRITE, out idata);
BitmapData bdata = new BitmapData();
bdata.Scan0 = idata.buffer.planes[0];
bdata.Stride = idata.buffer.pitches[0];
bdata.PixelFormat = PixelFormat.Format32bppRgb;
bdata.Width = bitmap.Width;
bdata.Height = bitmap.Height;
BitmapData bdata2 = bitmap.LockBits(new Rectangle(0, 0, bitmap.Width, bitmap.Height), ImageLockMode.ReadOnly | ImageLockMode.UserInputBuffer, PixelFormat.Format32bppRgb, bdata);
image.ReleaseAccess(ref idata);
bitmap.UnlockBits(bdata);
return image;
}
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 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);
}
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 void SimplePipeline()
{
bool sts = true;
PXCMSenseManager pp = form.session.CreateSenseManager();
if (pp == null)
{
throw new Exception("Failed to create sense manager");
}
disconnected = false;
/* Set Source & Profile Index */
PXCMCapture.DeviceInfo info = null;
if (this.form.GetRecordState())
{
pp.captureManager.SetFileName(this.form.GetFileName(), true);
form.PopulateDeviceMenu();
if (this.form.Devices.TryGetValue(this.form.GetCheckedDevice(), out info))
{
pp.captureManager.FilterByDeviceInfo(info);
}
}
else if (this.form.GetPlaybackState())
{
pp.captureManager.SetFileName(this.form.GetFileName(), false);
}
else
{
if (this.form.Devices.TryGetValue(this.form.GetCheckedDevice(), out info))
{
pp.captureManager.FilterByDeviceInfo(info);
}
}
/* Set Module */
pp.EnableEmotion(form.GetCheckedModule());
/* Initialization */
form.UpdateStatus("Init Started");
PXCMSenseManager.Handler handler = new PXCMSenseManager.Handler()
{
//GZ onModuleQueryProfile = OnModuleQueryProfile
};
if (pp.Init(handler) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
form.UpdateStatus("Streaming");
this.timer = new FPSTimer(form);
PXCMCaptureManager captureManager = pp.QueryCaptureManager();
if (captureManager == null)
{
throw new Exception("Failed to query capture manager");
}
PXCMCapture.Device device = captureManager.QueryDevice();
if (device != null && !this.form.GetPlaybackState())
{
device.SetDepthConfidenceThreshold(7);
}
//GZ device.SetProperty(PXCMCapture.Device.Property.PROPERTY_DEPTH_CONFIDENCE_THRESHOLD, 7);
while (!form.stop)
{
if (pp.AcquireFrame(true) < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
break;
}
if (!DisplayDeviceConnection(!pp.IsConnected()))
{
/* Display Results */
PXCMEmotion ft = pp.QueryEmotion();
//GZ DisplayPicture(pp.QueryImageByType(PXCMImage.ImageType.IMAGE_TYPE_COLOR));
PXCMCapture.Sample sample = pp.QuerySample();
/* Start of modified code */
// Grab the first BMP in the folder, assume there is one for now
string folder = Path.GetDirectoryName(Process.GetCurrentProcess().MainModule.FileName);
string[] files = Directory.GetFiles(folder, "*.bmp");
Bitmap bitmap = new Bitmap(files[0]);
// Create a PXCMImage from the BMP
PXCMImage.ImageInfo iinfo = new PXCMImage.ImageInfo();
iinfo.width = bitmap.Width;
iinfo.height = bitmap.Height;
iinfo.format = PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32;
PXCMImage imageTEST = form.session.CreateImage(iinfo);
PXCMImage.ImageData idata;
imageTEST.AcquireAccess(PXCMImage.Access.ACCESS_WRITE, out idata);
BitmapData bdata = new BitmapData();
bdata.Scan0 = idata.planes[0];
bdata.Stride = idata.pitches[0];
bdata.PixelFormat = PixelFormat.Format32bppRgb;
bdata.Width = bitmap.Width;
bdata.Height = bitmap.Height;
BitmapData bdata2 = bitmap.LockBits(new Rectangle(0, 0, bitmap.Width, bitmap.Height),
ImageLockMode.ReadOnly | ImageLockMode.UserInputBuffer,
PixelFormat.Format32bppRgb, bdata);
bitmap.UnlockBits(bdata2);
imageTEST.ReleaseAccess(idata);
// Save the BMP
Bitmap savebmp = idata.ToBitmap(0, bitmap.Width, bitmap.Height);
//savebmp.Save(@"O:\unix\projects\instr\production5\research\Francis\result.bmp");
// Put my own PXCMImage into the sample
PXCMCapture.Sample smp = new PXCMCapture.Sample();
smp.color = imageTEST;
// Get the video module from the emotion instance
PXCMVideoModule module = ft.QueryInstance <PXCMVideoModule>();
PXCMSyncPoint sp;
// Process the sample
module.ProcessImageAsync(smp, out sp);
// Synchronize then get emotion data etc.
sp.Synchronize();
/* End of modified code */
DisplayPicture(sample.color);
DisplayLocation(ft);
form.UpdatePanel();
}
pp.ReleaseFrame();
}
}
else
{
form.UpdateStatus("Init Failed");
sts = false;
}
pp.Close();
pp.Dispose();
if (sts)
{
form.UpdateStatus("Stopped");
}
}
/* Displaying Mask Images */
private unsafe void DisplayPicture(PXCMImage depth, PXCMBlobData blobData)
{
if (depth == null)
return;
PXCMImage image = depth;
PXCMImage.ImageInfo info = image.QueryInfo();
int numOfBlobs = blobData.QueryNumberOfBlobs();
if (_maxBlobToShow > numOfBlobs)
{
_maxBlobToShow = numOfBlobs;
}
PXCMBlobData.IBlob[] blobList = new PXCMBlobData.IBlob[_maxBlobToShow];
PXCMPointI32[][] pointOuter = new PXCMPointI32[_maxBlobToShow][];
PXCMPointI32[][] pointInner = new PXCMPointI32[_maxBlobToShow][];
Bitmap picture = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);
PXCMImage.ImageData bdata;
pxcmStatus results = pxcmStatus.PXCM_STATUS_NO_ERROR;
PXCMBlobData.AccessOrderType accessOrder = PXCMBlobData.AccessOrderType.ACCESS_ORDER_LARGE_TO_SMALL;
int accessOrderBy = form.GetAccessOrder();
switch (accessOrderBy)
{
case 1:
accessOrder = PXCMBlobData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR;
break;
case 2:
accessOrder = PXCMBlobData.AccessOrderType.ACCESS_ORDER_RIGHT_TO_LEFT;
break;
case 0:
default:
accessOrder = PXCMBlobData.AccessOrderType.ACCESS_ORDER_LARGE_TO_SMALL;
break;
}
Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);
BitmapData bitmapdata = picture.LockBits(rect, ImageLockMode.ReadWrite, picture.PixelFormat);
for (int j = 0; j < _maxBlobToShow; j++)
{
byte tmp1 = (Byte)(255 - (255 / (_maxBlobToShow) * j));
results = blobData.QueryBlobByAccessOrder(j, accessOrder, out blobList[j]);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
bool isSegmentationImage = true;
results = blobList[j].QuerySegmentationImage(out image);
if (results != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
PXCMImage.ImageInfo imgInfo = new PXCMImage.ImageInfo();
imgInfo.width = 640;
imgInfo.height = 480;
imgInfo.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
PXCMSession session = PXCMSession.CreateInstance();
if (session != null)
{
image = session.CreateImage(info);
if (image == null) return;
}
image.AcquireAccess(PXCMImage.Access.ACCESS_WRITE, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out bdata);
byte* numPtr = (byte*)bitmapdata.Scan0; //dst
byte* numPtr2 = (byte*)bdata.planes[0]; //row
int imagesize = image.info.width * image.info.height;
byte tmp;
for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
{
tmp = (byte)(0);
numPtr[0] = tmp;
numPtr[1] = tmp;
numPtr[2] = tmp;
numPtr[3] = tmp;
}
image.ReleaseAccess(bdata);
isSegmentationImage = false;
}
results = image.AcquireAccess(PXCMImage.Access.ACCESS_READ_WRITE, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8, out bdata);
if (form.GetBlobState() && isSegmentationImage == true)
{
byte* numPtr = (byte*)bitmapdata.Scan0; //dst
byte* numPtr2 = (byte*)bdata.planes[0]; //row
int imagesize = image.info.width * image.info.height;
for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
{
byte tmp = (Byte)(numPtr2[0] == 0 ? 0 : tmp1);
tmp |= numPtr[0];
numPtr[0] = tmp;
numPtr[1] = tmp;
numPtr[2] = tmp;
numPtr[3] = 0xff;
}
}
if ((form.GetContourState()))
{
int contourNumber = blobList[j].QueryNumberOfContours();
if (contourNumber > 0)
{
for (int k = 0; k < contourNumber; ++k)
{
int contourSize = blobList[j].QueryContourSize(k);
if (blobList[j].IsContourOuter(k) == true)
blobList[j].QueryContourPoints(k, out pointOuter[j]);
else
{
blobList[j].QueryContourPoints(k, out pointInner[j]);
}
}
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR && form.GetBlobState() == false)
{
byte* numPtr = (byte*)bitmapdata.Scan0; //dst
byte* numPtr2 = (byte*)bdata.planes[0]; //row
int imagesize = image.info.width * image.info.height;
byte tmp;
for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
{
tmp = (byte)(0);
numPtr[0] = tmp;
numPtr[1] = tmp;
numPtr[2] = tmp;
numPtr[3] = tmp;
}
}
}
}
image.ReleaseAccess(bdata);
image.Dispose();
}
}
picture.UnlockBits(bitmapdata);
form.DisplayBitmap(picture);
///////// that is my polygon zone
Bitmap imageInstance2 = picture;
i++;
Bitmap croppedImage = null;
if (contourMostRight.x > 0) {
int rectWidth = (int)(contourMostRight.x - contourMostLeft.x);
int rectHeight = (int)(contourMostTop.y - contourMostBottom.y);
Rectangle sourceRectangle = new Rectangle(new Point((int)contourMostLeft.x,
(int)contourMostBottom.y),
new Size(rectWidth, rectHeight));
croppedImage = CropImage(imageInstance2, sourceRectangle);
}
String[] origArray = {
"d:\\origG.jpeg",
"d:\\origX.jpeg",
"d:\\origY.jpeg"
};
for (int i = 0; i < origArray.Length; i++)
{
Bitmap orig = null;
if (File.Exists(origArray[i]))
orig = new Bitmap(@origArray[i]);
if (orig != null && croppedImage != null)
{
float diff = 0;
orig = ScaleImage(orig, 150, 100);
croppedImage = ScaleImage(croppedImage, 150, 100);
bool isImageBlank = true;
for (int y = 0; y < orig.Height; y++)
{
for (int x = 0; x < orig.Width; x++)
{
if(croppedImage.GetPixel(x, y).R > 1 && croppedImage.GetPixel(x, y).B > 1
&& croppedImage.GetPixel(x, y).G > 1)
{
isImageBlank = false;
break;
}
}
}
if (!isImageBlank && orig.Size.Width == croppedImage.Size.Width)
{
for (int y = 0; y < orig.Height; y++)
{
for (int x = 0; x < orig.Width; x++)
{
diff += (float)Math.Abs(orig.GetPixel(x, y).R -
croppedImage.GetPixel(x, y).R) / 255;
diff += (float)Math.Abs(orig.GetPixel(x, y).G -
croppedImage.GetPixel(x, y).G) / 255;
diff += (float)Math.Abs(orig.GetPixel(x, y).B -
croppedImage.GetPixel(x, y).B) / 255;
}
}
}
float percentMatch = 100 * diff / (orig.Width * orig.Height * 2);
Console.WriteLine("diff: {0} %", percentMatch);
if (percentMatch >= 90){
Console.WriteLine(origArray[i].ToString());
Environment.Exit(0);
}
}
}
/*
if (croppedImage != null)
{
croppedImage.Save("d:\\cropedImage.jpeg", System.Drawing.Imaging.ImageFormat.Jpeg);
// croppedImage.Save("d:\\origG.jpeg", System.Drawing.Imaging.ImageFormat.Jpeg);
}*/
if (i == 100)
Environment.Exit(0);
picture.Dispose();
for (int i = 0; i < _maxBlobToShow; i++)
{
if (form.GetContourState())
{
if (pointOuter[i] != null && pointOuter[i].Length > 0)
form.DisplayContour(pointOuter[i], i);
if (pointInner[i] != null && pointInner[i].Length > 0)
form.DisplayContour(pointInner[i], i);
}
if (form.GetBlobDataPointsState())
{
form.DisplayBlobDataPoints(blobList[i], i + 1);
}
PXCMPoint3DF32 point = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_CENTER);
contourMostRight = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_LEFT_MOST);
contourMostLeft = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_RIGHT_MOST);
contourMostBottom = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_TOP_MOST);
contourMostTop = blobList[i].QueryExtremityPoint(PXCMBlobData.ExtremityType.EXTREMITY_BOTTOM_MOST);
form.DisplayBlobNumber(point, i + 1);
}
}
private void RunLoop()
{
/* IEnumerable<PXCMCapture.DeviceInfo> devices = this.GetDevices();
* if (devices.Count() == 0)
* {
* throw new Smithers.Reading.FrameData.ScannerNotFoundException("No devices found");
* }
* PXCMCapture.DeviceInfo device = devices.First();
* if (devices.Count() > 1)
* {
* Console.WriteLine(String.Format("More than one device, using this one: {0}", device.name));
* }
* _senseManager.captureManager.FilterByDeviceInfo(device);*/
pxcmStatus status;
status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, Frame.COLOR_WIDTH, Frame.COLOR_HEIGHT, Frame.COLOR_RATE);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
_logger.info("Unable to enable color stream:" + status.ToString());
throw new Smithers.Reading.FrameData.ScannerNotFoundException("Unable to enable color stream");
}
_logger.info("Color Stream Enabled:" + status.ToString());
//status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 0, 0, Frame.DEPTH_RATE);
status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, Frame.DEPTH_WIDTH, Frame.DEPTH_HEIGHT, Frame.DEPTH_RATE);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
_logger.info("Unable to enable depth stream:" + status.ToString());
throw new Smithers.Reading.FrameData.ScannerNotFoundException("Unable to enable depth stream");
}
_logger.info("Depth Stream Enabled:" + status.ToString());
//status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_LEFT, Frame.DEPTH_WIDTH, Frame.DEPTH_HEIGHT, Frame.DEPTH_RATE);
//if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
//{
// throw new Smithers.Reading.FrameData.ScannerNotFoundException("Unable to enable depth stream");
//}
//status = _senseManager.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_RIGHT, Frame.DEPTH_WIDTH, Frame.DEPTH_HEIGHT, Frame.DEPTH_RATE);
//if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
//{
// throw new Smithers.Reading.FrameData.ScannerNotFoundException("Unable to enable depth stream");
//}
status = _senseManager.EnableScenePerception();
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
_logger.info("Unable to enable scene perception:" + status.ToString());
throw new Smithers.Reading.FrameData.ScannerNotFoundException("Scene Perception failed");
}
_logger.info("Scene Perception Enabled:" + status.ToString());
_perceptionHandle = _senseManager.QueryScenePerception();
status = _senseManager.Init();
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
_logger.info("Unable to open sensor in the above mode:" + status.ToString());
throw new Smithers.Reading.FrameData.ScannerNotFoundException("Init failed");
}
_logger.info("Sensor Initialized Successfully:" + status.ToString());
PXCMImage.ImageInfo depthInfo = new PXCMImage.ImageInfo();
PXCMImage.ImageInfo colorInfo = new PXCMImage.ImageInfo();
depthInfo.height = Frame.DEPTH_HEIGHT;
depthInfo.width = Frame.DEPTH_WIDTH;
colorInfo.height = Frame.COLOR_HEIGHT;
colorInfo.width = Frame.COLOR_WIDTH;
/* For UV Mapping & Projection only: Save certain properties */
Projection projection = new Projection(_senseManager.session, _senseManager.captureManager.device, depthInfo, colorInfo);
AccelerometerReading accelerometerReading;
AccelerometerStats acceleroStats = new AccelerometerStats();
//bool firstFrameRecord = true;
// bool firstFrameNotRecord = false;
//pxcmStatus stsInitRecord = pxcmStatus.PXCM_STATUS_PROCESS_FAILED;
while (!_stop)
{
//PXCMCapture.Device.MirrorMode mirrorMode = this.Mirrored ? PXCMCapture.Device.MirrorMode.MIRROR_MODE_HORIZONTAL : PXCMCapture.Device.MirrorMode.MIRROR_MODE_DISABLED;
//{
// _senseManager.captureManager.device.SetMirrorMode(mirrorMode);
//}
/* Wait until a frame is ready: Synchronized or Asynchronous */
//if ((Record || Playback) && firstFrameRecord)
//{
// stsInitRecord = InitRecordPlay();
// if (this.StartCapture != null)
// {
// EventArgs eventArgs = new EventArgs();
// this.StartCapture(this, eventArgs);
// }
// firstFrameRecord = false;
// firstFrameNotRecord = true;
//}
//if (!Record && !Playback && firstFrameNotRecord)
//{
// InitRecordPlay();
// if (this.ShotSavedSuccess != null)
// {
// EventArgs eventArgs = new EventArgs();
// this.ShotSavedSuccess(this, eventArgs);
// }
// firstFrameRecord = true;
// firstFrameNotRecord = false;
//}
//-------
status = _senseManager.AcquireFrame(this.Synced);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
MemoryFrame frame = _pool.ObjectFromPool();
///* Display images */
PXCMCapture.Sample sample;
sample = _senseManager.QueryScenePerceptionSample();
if (sample == null)
{
sample = _senseManager.QuerySample();
}
//if (_perceptionHandle != null && sample !=null)
//{
// _perceptionHandle.GetCameraPose(frame.CameraPose);
// frame.TrackingAccuracy = _perceptionHandle.QueryTrackingAccuracy().ToString();
// frame.VoxelResolution = _perceptionHandle.QueryVoxelResolution().ToString();
// float sceneQuality = _perceptionHandle.CheckSceneQuality(sample);
// if (sceneQuality > 0.3f && _perceptionPaused && !_capturing)
// {
// _perceptionPaused = false;
// _senseManager.PauseScenePerception(false);
// _perceptionHandle.Reset(_initPose);
// }
// else if (!(frame.TrackingAccuracy == "HIGH" || frame.TrackingAccuracy == "MID") && !_capturing)
// {
// ResetScenePerception();
// }
//}
//if (this.Accelerometer != null)
//{
// accelerometerReading = Accelerometer.GetCurrentReading();
// acceleroStats.accX = accelerometerReading.AccelerationX;
// acceleroStats.accY = accelerometerReading.AccelerationY;
// acceleroStats.accZ = accelerometerReading.AccelerationZ;
// acceleroStats.timestamp = accelerometerReading.Timestamp;
// frame.AcceleroStats = acceleroStats;
//}
if (sample.color != null)
{
sample.color.CopyToByteArray(frame.BufferColor);
frame.ColorTimeStamp = sample.color.QueryTimeStamp();
}
if (sample.depth != null)
{
sample.depth.CopyToByteArray(frame.BufferDepth);
sample.depth.CopyToByteArray(frame.BufferDepthPreview, true);
frame.DepthTimeStamp = sample.depth.timeStamp;
}
#if DEBUG
if (sample.left != null)
{
sample.left.CopyToByteArray(frame.BufferIRLeft);
}
if (sample.right != null)
{
sample.right.CopyToByteArray(frame.BufferIRRight);
}
// Get mapping.
if (sample.depth != null)
{
projection.DepthToCameraCoordinates(sample.depth, frame.DepthToCameraMapping);
}
#endif
//frame.LowConfidenceDepthValue = projection.InvalidDepthValue;
using (Handle <MemoryFrame> frameHandle = new Handle <MemoryFrame>(frame, _pool))
{
FrameArrivedEventArgs eventArgs = new FrameArrivedEventArgs()
{
FrameHandle = frameHandle
};
if (this.FrameArrived != null)
{
this.FrameArrived(this, eventArgs);
}
}
_senseManager.ReleaseFrame();
//int bitmap_width, bitmap_height;
//byte[] bitmap_data;
//PXCMCapture.StreamType[] streams = form.QueryStreams();
//bool streamState = false;
//for (int s = 0; s < PXCMCapture.STREAM_LIMIT; s++)
//{
// streamState = streamState || form.GetStreamState(PXCMCapture.StreamTypeFromIndex(s));
//}
//if (streamState)
//{
// for (int inx = 0; inx < 2; inx++)
// if (sample[streams[inx]] != null)
// {
// form.SetImage(inx, sample[streams[inx]]);
// }
//}
//else
//{
// if (form.GetProjectionState() && sample.color != null && sample.depth != null)
// {
// bitmap_data = projection.DepthToColorCoordinatesByFunction(sample.color, sample.depth, form.GetDotsState(), out bitmap_width, out bitmap_height);
// form.SetBitmap(0, bitmap_width, bitmap_height, bitmap_data);
// }
// if (form.GetUVMapState() && sample.color != null && sample.depth != null)
// {
// bitmap_data = projection.DepthToColorCoordinatesByUVMAP(sample.color, sample.depth, form.GetDotsState(), out bitmap_width, out bitmap_height);
// form.SetBitmap(0, bitmap_width, bitmap_height, bitmap_data);
// }
// if (form.GetInvUVMapState() && sample.color != null && sample.depth != null)
// {
// bitmap_data = projection.ColorToDepthCoordinatesByInvUVMap(sample.color, sample.depth, form.GetDotsState(), out bitmap_width, out bitmap_height);
// form.SetBitmap(0, bitmap_width, bitmap_height, bitmap_data);
// }
// if (sample.depth != null)
// {
// bitmap_data = GetRGB32Pixels(sample.depth, out bitmap_width, out bitmap_height);
// form.SetBitmap(1, bitmap_width, bitmap_height, bitmap_data);
// }
//}
//form.UpdatePanel();
}
//projection.Dispose();
}
