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();
}
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();
}