904 lines
38 KiB
C#
904 lines
38 KiB
C#
|
using UnityEngine;
|
|||
|
|
using System.Collections;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using Intel.RealSense;
|
|||
|
|
using System.Threading;
|
|||
|
|
using System;
|
|||
|
|
using System.Linq;
|
|||
|
|
using System.Runtime.InteropServices;
|
|||
|
|
using Microsoft.Azure.Kinect.Sensor;
|
|||
|
|
|
|||
|
|
namespace com.rfilkov.kinect
|
|||
|
|
{
|
|||
|
|
public class RealSenseInterface : DepthSensorBase
|
|||
|
|
{
|
|||
|
|
[Tooltip("Color camera resolution.")]
|
|||
|
|
public ColorCameraMode colorCameraMode = ColorCameraMode._640_x_480_30Fps;
|
|||
|
|
public enum ColorCameraMode : int { _320_x_180_30Fps = 10, _320_x_240_30Fps = 15, _424_x_240_30Fps = 20, _640_x_360_30Fps = 30, _640_x_480_30Fps = 35, _848_x_480_30Fps = 40, _960_x_540_30Fps = 50, _1280_x_720_30Fps = 60, _1920_x_1080_30Fps = 70 }
|
|||
|
|
|
|||
|
|
[Tooltip("Depth camera resolution.")]
|
|||
|
|
public DepthCameraMode depthCameraMode = DepthCameraMode._640_x_480_30Fps;
|
|||
|
|
public enum DepthCameraMode : int { _424_x_240_30Fps = 20, _480_x_270_30Fps = 25, _640_x_360_30Fps = 30, _640_x_480_30Fps = 35, _848_x_480_30Fps = 40, _1280_x_720_30Fps = 60 }
|
|||
|
|
|
|||
|
|
|
|||
|
|
// realsense pipeline
|
|||
|
|
private Pipeline m_pipeline = null;
|
|||
|
|
|
|||
|
|
//public bool Streaming { get; protected set; }
|
|||
|
|
private PipelineProfile activeProfile = null;
|
|||
|
|
|
|||
|
|
// current frame number
|
|||
|
|
//private ulong currentFrameNumber = 0;
|
|||
|
|
//private ulong currentFrameTimestamp = 0;
|
|||
|
|
|
|||
|
|
// stream profiles
|
|||
|
|
private VideoStreamProfile depthStreamProfile = null;
|
|||
|
|
private VideoStreamProfile colorStreamProfile = null;
|
|||
|
|
|
|||
|
|
// raw infrared data
|
|||
|
|
protected byte[] rawInfraredImage1 = null;
|
|||
|
|
protected byte[] rawInfraredImage2 = null; // used by BT
|
|||
|
|
protected ushort[] rawInfraredImageBT = null;
|
|||
|
|
|
|||
|
|
// raw IMU data
|
|||
|
|
private RealSensePoseData rsPoseData;
|
|||
|
|
|
|||
|
|
// rs frame holder
|
|||
|
|
protected RealSenseFrames rsFrames = new RealSenseFrames();
|
|||
|
|
|
|||
|
|
|
|||
|
|
[StructLayout(LayoutKind.Sequential)]
|
|||
|
|
struct RealSensePoseData
|
|||
|
|
{
|
|||
|
|
public Vector3 translation;
|
|||
|
|
public Vector3 velocity;
|
|||
|
|
public Vector3 acceleration;
|
|||
|
|
public Quaternion rotation;
|
|||
|
|
public Vector3 angular_velocity;
|
|||
|
|
public Vector3 angular_acceleration;
|
|||
|
|
public int tracker_confidence;
|
|||
|
|
public int mapper_confidence;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// rs frame set
|
|||
|
|
public class RealSenseFrames
|
|||
|
|
{
|
|||
|
|
public VideoFrame colorFrame = null;
|
|||
|
|
public VideoFrame depthFrame = null;
|
|||
|
|
public VideoFrame infraredFrame = null;
|
|||
|
|
public PoseFrame poseFrame = null;
|
|||
|
|
public ulong deviceTimestamp = 0;
|
|||
|
|
|
|||
|
|
public void DisposeFrames()
|
|||
|
|
{
|
|||
|
|
if (colorFrame != null)
|
|||
|
|
{
|
|||
|
|
colorFrame.Dispose();
|
|||
|
|
colorFrame = null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (depthFrame != null)
|
|||
|
|
{
|
|||
|
|
depthFrame.Dispose();
|
|||
|
|
depthFrame = null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (infraredFrame != null)
|
|||
|
|
{
|
|||
|
|
infraredFrame.Dispose();
|
|||
|
|
infraredFrame = null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (poseFrame != null)
|
|||
|
|
{
|
|||
|
|
poseFrame.Dispose();
|
|||
|
|
poseFrame = null;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public override KinectInterop.DepthSensorPlatform GetSensorPlatform()
|
|||
|
|
{
|
|||
|
|
return KinectInterop.DepthSensorPlatform.RealSense;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public override List<KinectInterop.SensorDeviceInfo> GetAvailableSensors()
|
|||
|
|
{
|
|||
|
|
List<KinectInterop.SensorDeviceInfo> alSensorInfo = new List<KinectInterop.SensorDeviceInfo>();
|
|||
|
|
|
|||
|
|
using (var ctx = new Context())
|
|||
|
|
{
|
|||
|
|
DeviceList devices = ctx.QueryDevices();
|
|||
|
|
|
|||
|
|
for(int i = 0; i < devices.Count; i++)
|
|||
|
|
{
|
|||
|
|
using (Intel.RealSense.Device device = devices[i])
|
|||
|
|
{
|
|||
|
|
KinectInterop.SensorDeviceInfo sensorInfo = new KinectInterop.SensorDeviceInfo();
|
|||
|
|
sensorInfo.sensorId = device.Info[CameraInfo.SerialNumber];
|
|||
|
|
sensorInfo.sensorName = device.Info[CameraInfo.Name];
|
|||
|
|
|
|||
|
|
if (sensorInfo.sensorName.StartsWith("Intel RealSense D"))
|
|||
|
|
sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeColor | KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeInfrared;
|
|||
|
|
else if (sensorInfo.sensorName.StartsWith("Intel RealSense T"))
|
|||
|
|
sensorInfo.sensorCaps = KinectInterop.FrameSource.TypePose;
|
|||
|
|
else
|
|||
|
|
sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeNone;
|
|||
|
|
|
|||
|
|
Debug.Log(string.Format(" D{0}: {1}, id: {2}", i, sensorInfo.sensorName, sensorInfo.sensorId));
|
|||
|
|
|
|||
|
|
alSensorInfo.Add(sensorInfo);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//if (alSensorInfo.Count == 0)
|
|||
|
|
//{
|
|||
|
|
// Debug.Log(" No sensor devices found.");
|
|||
|
|
//}
|
|||
|
|
|
|||
|
|
return alSensorInfo;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public override KinectInterop.SensorData OpenSensor(KinectInterop.FrameSource dwFlags, bool bSyncDepthAndColor, bool bSyncBodyAndDepth)
|
|||
|
|
{
|
|||
|
|
// save initial parameters
|
|||
|
|
base.OpenSensor(dwFlags, bSyncDepthAndColor, bSyncBodyAndDepth);
|
|||
|
|
|
|||
|
|
// color settings
|
|||
|
|
int colorWidth = 0, colorHeight = 0, colorFps = 0;
|
|||
|
|
ParseCameraMode(colorCameraMode.ToString(), out colorWidth, out colorHeight, out colorFps);
|
|||
|
|
|
|||
|
|
// depth settings
|
|||
|
|
int depthWidth = 0, depthHeight = 0, depthFps = 0;
|
|||
|
|
ParseCameraMode(colorCameraMode.ToString(), out depthWidth, out depthHeight, out depthFps);
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
m_pipeline = new Pipeline();
|
|||
|
|
|
|||
|
|
using (Config config = new Config())
|
|||
|
|
{
|
|||
|
|
if(deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
|
|||
|
|
{
|
|||
|
|
if (string.IsNullOrEmpty(recordingFile))
|
|||
|
|
{
|
|||
|
|
Debug.LogError("PlayRecording selected, but the path to recording file is missing.");
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(!System.IO.File.Exists(recordingFile))
|
|||
|
|
{
|
|||
|
|
Debug.LogError("PlayRecording selected, but the recording file cannot be found: " + recordingFile);
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// playback from file
|
|||
|
|
Debug.Log("Playing back: " + recordingFile);
|
|||
|
|
config.EnableDeviceFromFile(recordingFile, false);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// get the list of available sensors
|
|||
|
|
List<KinectInterop.SensorDeviceInfo> alSensors = GetAvailableSensors();
|
|||
|
|
if (deviceIndex >= alSensors.Count)
|
|||
|
|
{
|
|||
|
|
Debug.Log(" D" + deviceIndex + " is not available. You can set the device index to -1, to disable it.");
|
|||
|
|
return null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// sensor serial number
|
|||
|
|
string sensorId = alSensors[deviceIndex].sensorId;
|
|||
|
|
config.EnableDevice(sensorId);
|
|||
|
|
|
|||
|
|
// color
|
|||
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
|
|||
|
|
{
|
|||
|
|
config.EnableStream(Stream.Color, -1, colorWidth, colorHeight, Format.Rgb8, colorFps);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// depth
|
|||
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
|
|||
|
|
{
|
|||
|
|
config.EnableStream(Stream.Depth, -1, depthWidth, depthHeight, Format.Z16, depthFps);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// infrared
|
|||
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0 || (dwFlags & KinectInterop.FrameSource.TypeBody) != 0)
|
|||
|
|
{
|
|||
|
|
config.EnableStream(Stream.Infrared, 1, depthWidth, depthHeight, Format.Y8, depthFps);
|
|||
|
|
//config.EnableStream(Stream.Infrared, 2, depthWidth, depthHeight, Format.Y8, depthFps);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// pose
|
|||
|
|
if ((dwFlags & KinectInterop.FrameSource.TypePose) != 0)
|
|||
|
|
{
|
|||
|
|
config.EnableStream(Stream.Pose, Format.SixDOF);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//// record to file
|
|||
|
|
//if(deviceMode == KinectInterop.DepthSensorMode.CreateRecording && !string.IsNullOrEmpty(deviceFilePath))
|
|||
|
|
//{
|
|||
|
|
// if (!string.IsNullOrEmpty(deviceFilePath))
|
|||
|
|
// {
|
|||
|
|
// config.EnableRecordToFile(deviceFilePath);
|
|||
|
|
// }
|
|||
|
|
// else
|
|||
|
|
// {
|
|||
|
|
// Debug.LogError("Record selected, but the path to recording file is missing.");
|
|||
|
|
// }
|
|||
|
|
//}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
activeProfile = m_pipeline.Start(config);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
Debug.LogError("RealSenseInterface: " + ex.Message);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// check if the profile was successfully created
|
|||
|
|
if(activeProfile == null)
|
|||
|
|
return null;
|
|||
|
|
|
|||
|
|
KinectInterop.SensorData sensorData = new KinectInterop.SensorData();
|
|||
|
|
|
|||
|
|
// flip color & depth images vertically
|
|||
|
|
sensorData.colorImageScale = new Vector3(-1f, -1f, 1f);
|
|||
|
|
sensorData.depthImageScale = new Vector3(-1f, -1f, 1f);
|
|||
|
|
sensorData.infraredImageScale = new Vector3(-1f, -1f, 1f);
|
|||
|
|
sensorData.sensorSpaceScale = new Vector3(-1f, -1f, 1f);
|
|||
|
|
|
|||
|
|
// depth camera offset & matrix z-flip
|
|||
|
|
sensorRotOffset = Vector3.zero; // if for instance the depth camera is tilted downwards
|
|||
|
|
sensorRotFlipZ = true;
|
|||
|
|
sensorRotIgnoreY = false;
|
|||
|
|
|
|||
|
|
// color
|
|||
|
|
sensorData.colorImageWidth = colorWidth;
|
|||
|
|
sensorData.colorImageHeight = colorHeight;
|
|||
|
|
|
|||
|
|
sensorData.colorImageFormat = TextureFormat.RGB24;
|
|||
|
|
sensorData.colorImageStride = 3; // 3 bytes per pixel
|
|||
|
|
|
|||
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
|
|||
|
|
{
|
|||
|
|
rawColorImage = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight * 3];
|
|||
|
|
|
|||
|
|
sensorData.colorImageTexture = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGB24, false);
|
|||
|
|
sensorData.colorImageTexture.wrapMode = TextureWrapMode.Clamp;
|
|||
|
|
sensorData.colorImageTexture.filterMode = FilterMode.Point;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// depth
|
|||
|
|
sensorData.depthImageWidth = depthWidth;
|
|||
|
|
sensorData.depthImageHeight = depthHeight;
|
|||
|
|
|
|||
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
|
|||
|
|
{
|
|||
|
|
rawDepthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|||
|
|
sensorData.depthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// infrared
|
|||
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0 || (dwFlags & KinectInterop.FrameSource.TypeBody) != 0)
|
|||
|
|
{
|
|||
|
|
rawInfraredImage1 = new byte[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|||
|
|
rawInfraredImage2 = new byte[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|||
|
|
rawInfraredImageBT = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|||
|
|
|
|||
|
|
rawInfraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|||
|
|
sensorData.infraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|||
|
|
|
|||
|
|
minInfraredValue = 0f;
|
|||
|
|
maxInfraredValue = 1000f;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Debug.Log("RealSense-sensor opened");
|
|||
|
|
|
|||
|
|
return sensorData;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// parses the given camera mode and returns width, height and fps
|
|||
|
|
private void ParseCameraMode(string sCameraMode, out int camWidth, out int camHeight, out int camFps)
|
|||
|
|
{
|
|||
|
|
camWidth = camHeight = camFps = 0;
|
|||
|
|
|
|||
|
|
if (string.IsNullOrEmpty(sCameraMode))
|
|||
|
|
throw new Exception("Invalid camera mode" + sCameraMode);
|
|||
|
|
|
|||
|
|
// _640_x_360_30Fps
|
|||
|
|
string[] asModeParts = sCameraMode.Split("_".ToCharArray());
|
|||
|
|
if(asModeParts.Length != 5)
|
|||
|
|
throw new Exception("Invalid camera mode" + sCameraMode);
|
|||
|
|
|
|||
|
|
camWidth = int.Parse(asModeParts[1]);
|
|||
|
|
camHeight = int.Parse(asModeParts[3]);
|
|||
|
|
|
|||
|
|
int iF = asModeParts[4].IndexOf('F');
|
|||
|
|
if (iF >= 0)
|
|||
|
|
asModeParts[4] = asModeParts[4].Substring(0, iF);
|
|||
|
|
|
|||
|
|
camFps = int.Parse(asModeParts[4]);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public override void CloseSensor(KinectInterop.SensorData sensorData)
|
|||
|
|
{
|
|||
|
|
base.CloseSensor(sensorData);
|
|||
|
|
|
|||
|
|
if (activeProfile != null)
|
|||
|
|
{
|
|||
|
|
activeProfile.Dispose();
|
|||
|
|
activeProfile = null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (m_pipeline != null)
|
|||
|
|
{
|
|||
|
|
m_pipeline.Dispose();
|
|||
|
|
m_pipeline = null;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Debug.Log("RealSense-sensor closed");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public override void PollSensorFrames(KinectInterop.SensorData sensorData)
|
|||
|
|
{
|
|||
|
|
FrameSet frames;
|
|||
|
|
if (m_pipeline.PollForFrames(out frames))
|
|||
|
|
{
|
|||
|
|
//using (frames)
|
|||
|
|
// RaiseSampleEvent(frames);
|
|||
|
|
|
|||
|
|
using (frames)
|
|||
|
|
{
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
rsFrames.colorFrame = frames.ColorFrame;
|
|||
|
|
rsFrames.depthFrame = frames.DepthFrame;
|
|||
|
|
rsFrames.infraredFrame = frames.InfraredFrame;
|
|||
|
|
rsFrames.poseFrame = frames.PoseFrame;
|
|||
|
|
rsFrames.deviceTimestamp = (ulong)(frames.Timestamp * 1000.0);
|
|||
|
|
|
|||
|
|
//currentFrameNumber = frames.Number;
|
|||
|
|
//currentFrameTimestamp = (ulong)(frames.Timestamp * 1000.0);
|
|||
|
|
|
|||
|
|
// check for color-depth sync
|
|||
|
|
if(!isSyncDepthAndColor || (rsFrames.colorFrame != null && rsFrames.depthFrame != null))
|
|||
|
|
{
|
|||
|
|
ProcessCameraFrames(sensorData, rsFrames);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
rsFrames.DisposeFrames();
|
|||
|
|
//pipelineFrameNumber++;
|
|||
|
|
}
|
|||
|
|
catch (Exception e)
|
|||
|
|
{
|
|||
|
|
Debug.LogException(e);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void ProcessCameraFrames(KinectInterop.SensorData sensorData, RealSenseFrames frames)
|
|||
|
|
{
|
|||
|
|
Capture btCapture = null;
|
|||
|
|
if (bodyTracker != null)
|
|||
|
|
{
|
|||
|
|
// body frame
|
|||
|
|
if (frames.depthFrame != null && frames.infraredFrame != null)
|
|||
|
|
{
|
|||
|
|
Capture capture = GetBodyTrackerCapture(sensorData, frames);
|
|||
|
|
btCapture = PollBodyFrame(sensorData, capture);
|
|||
|
|
capture?.Dispose();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// check for body & depth sync
|
|||
|
|
if (!isSyncBodyAndDepth || btCapture != null || bodyTracker == null)
|
|||
|
|
{
|
|||
|
|
if(isSyncBodyAndDepth && btCapture != null)
|
|||
|
|
{
|
|||
|
|
// body-tracker frame
|
|||
|
|
if (btCapture.Color != null && rawColorImage != null && rawColorTimestamp == sensorData.lastColorFrameTime)
|
|||
|
|
{
|
|||
|
|
lock (colorFrameLock)
|
|||
|
|
{
|
|||
|
|
//btCapture.Color.CopyBytesTo(rawColorImage, 0, 0, rawColorImage.Length);
|
|||
|
|
KinectInterop.CopyBytes(btCapture.Color.GetBuffer(), (int)btCapture.Color.Size, rawColorImage, rawColorImage.Length, sizeof(byte));
|
|||
|
|
rawColorTimestamp = (ulong)btCapture.Color.DeviceTimestamp.Ticks;
|
|||
|
|
//Debug.Log("RawColorTimestamp: " + rawColorTimestamp);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (btCapture.Depth != null && rawDepthImage != null && rawDepthTimestamp == sensorData.lastDepthFrameTime)
|
|||
|
|
{
|
|||
|
|
lock (depthFrameLock)
|
|||
|
|
{
|
|||
|
|
//btCapture.Depth.CopyTo(rawDepthImage, 0, 0, rawDepthImage.Length);
|
|||
|
|
KinectInterop.CopyBytes(btCapture.Depth.GetBuffer(), (int)btCapture.Depth.Size, rawDepthImage, rawDepthImage.Length, sizeof(ushort));
|
|||
|
|
rawDepthTimestamp = (ulong)btCapture.Depth.DeviceTimestamp.Ticks;
|
|||
|
|
//Debug.Log("RawDepthTimestamp: " + rawDepthTimestamp);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (btCapture.IR != null && rawInfraredImage != null && rawInfraredTimestamp == sensorData.lastInfraredFrameTime)
|
|||
|
|
{
|
|||
|
|
lock (infraredFrameLock)
|
|||
|
|
{
|
|||
|
|
//btCapture.IR.CopyTo(rawInfraredImage, 0, 0, rawInfraredImage.Length);
|
|||
|
|
KinectInterop.CopyBytes(btCapture.IR.GetBuffer(), (int)btCapture.IR.Size, rawInfraredImage, rawInfraredImage.Length, sizeof(ushort));
|
|||
|
|
rawInfraredTimestamp = (ulong)btCapture.IR.DeviceTimestamp.Ticks;
|
|||
|
|
//Debug.Log("RawInfraredTimestamp: " + rawInfraredTimestamp);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// sensor frame
|
|||
|
|
if (frames.colorFrame != null && rawColorImage != null && rawColorTimestamp == sensorData.lastColorFrameTime)
|
|||
|
|
{
|
|||
|
|
lock (colorFrameLock)
|
|||
|
|
{
|
|||
|
|
KinectInterop.CopyBytes(frames.colorFrame.Data, rawColorImage.Length, rawColorImage, rawColorImage.Length, sizeof(byte));
|
|||
|
|
rawColorTimestamp = frames.deviceTimestamp;
|
|||
|
|
//Debug.Log("RawColorTimestamp: " + rawColorTimestamp);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (frames.depthFrame != null && rawDepthImage != null && rawDepthTimestamp == sensorData.lastDepthFrameTime)
|
|||
|
|
{
|
|||
|
|
lock (depthFrameLock)
|
|||
|
|
{
|
|||
|
|
frames.depthFrame.CopyTo<ushort>(rawDepthImage);
|
|||
|
|
rawDepthTimestamp = frames.deviceTimestamp;
|
|||
|
|
//Debug.Log("RawDepthTimestamp: " + rawDepthTimestamp);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (frames.infraredFrame != null && rawInfraredImage != null && rawInfraredTimestamp == sensorData.lastInfraredFrameTime)
|
|||
|
|
{
|
|||
|
|
lock (infraredFrameLock)
|
|||
|
|
{
|
|||
|
|
frames.infraredFrame.CopyTo<byte>(rawInfraredImage1);
|
|||
|
|
for (int i = 0; i < rawInfraredImage1.Length; i++)
|
|||
|
|
{
|
|||
|
|
rawInfraredImage[i] = (ushort)(rawInfraredImage1[i] << 4);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
rawInfraredTimestamp = frames.deviceTimestamp;
|
|||
|
|
//Debug.Log("RawInfraredTimestamp: " + rawInfraredTimestamp);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// color & depth stream profiles
|
|||
|
|
if(frames.colorFrame != null)
|
|||
|
|
{
|
|||
|
|
colorStreamProfile = frames.colorFrame.Profile.As<VideoStreamProfile>();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(frames.depthFrame != null)
|
|||
|
|
{
|
|||
|
|
depthStreamProfile = frames.depthFrame.Profile.As<VideoStreamProfile>();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// dispose body capture
|
|||
|
|
if (btCapture != null)
|
|||
|
|
{
|
|||
|
|
btCapture.Dispose();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// check for pose frame
|
|||
|
|
if (frames.poseFrame != null)
|
|||
|
|
{
|
|||
|
|
frames.poseFrame.CopyTo(out rsPoseData);
|
|||
|
|
|
|||
|
|
lock (poseFrameLock)
|
|||
|
|
{
|
|||
|
|
rawPosePosition = new Vector3(rsPoseData.translation.x, rsPoseData.translation.y, -rsPoseData.translation.z); // (1, 1, -1)
|
|||
|
|
rawPoseRotation = new Quaternion(-rsPoseData.rotation.x, -rsPoseData.rotation.y, rsPoseData.rotation.z, rsPoseData.rotation.w); // (-1, -1, 1, 1);
|
|||
|
|
|
|||
|
|
rawPoseTimestamp = frames.deviceTimestamp;
|
|||
|
|
//Debug.Log("RawPoseTimestamp: " + rawPoseTimestamp);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// converts the raw image data to capture
|
|||
|
|
protected Capture GetBodyTrackerCapture(KinectInterop.SensorData sensorData, RealSenseFrames rsFrames)
|
|||
|
|
{
|
|||
|
|
Capture capture = new Capture();
|
|||
|
|
|
|||
|
|
int depthW = sensorData.depthImageWidth;
|
|||
|
|
int depthH = sensorData.depthImageHeight;
|
|||
|
|
|
|||
|
|
if (rsFrames.colorFrame != null && rawColorImage != null)
|
|||
|
|
{
|
|||
|
|
Image colorImage = new Image(ImageFormat.ColorBGRA32, sensorData.colorImageWidth, sensorData.colorImageHeight, sensorData.colorImageWidth * 4);
|
|||
|
|
KinectInterop.CopyBytes(rsFrames.colorFrame.Data, colorImage.GetBuffer(), rawColorImage.Length);
|
|||
|
|
colorImage.DeviceTimestamp = TimeSpan.FromTicks((long)rsFrames.deviceTimestamp);
|
|||
|
|
capture.Color = colorImage;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (rsFrames.depthFrame != null && rawDepthImage != null)
|
|||
|
|
{
|
|||
|
|
Image depthImage = new Image(ImageFormat.Depth16, depthW, depthH, depthW * sizeof(ushort));
|
|||
|
|
KinectInterop.CopyBytes(rsFrames.depthFrame.Data, depthImage.GetBuffer(), rawDepthImage.Length * sizeof(ushort));
|
|||
|
|
depthImage.DeviceTimestamp = TimeSpan.FromTicks((long)rsFrames.deviceTimestamp);
|
|||
|
|
capture.Depth = depthImage;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (rsFrames.infraredFrame != null && rawInfraredImage2 != null && rawInfraredImageBT != null)
|
|||
|
|
{
|
|||
|
|
Image infraredFrame = new Image(ImageFormat.IR16, depthW, depthH, depthW * sizeof(ushort));
|
|||
|
|
rsFrames.infraredFrame.CopyTo<byte>(rawInfraredImage2);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < rawInfraredImage2.Length; i++)
|
|||
|
|
{
|
|||
|
|
rawInfraredImageBT[i] = (ushort)(rawInfraredImage2[i] << 4);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
KinectInterop.CopyBytes(rawInfraredImageBT, rawInfraredImageBT.Length, sizeof(ushort), infraredFrame.GetBuffer(), (int)infraredFrame.Size);
|
|||
|
|
infraredFrame.DeviceTimestamp = TimeSpan.FromTicks((long)rsFrames.deviceTimestamp);
|
|||
|
|
capture.IR = infraredFrame;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return capture;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public override bool UpdateSensorData(KinectInterop.SensorData sensorData, KinectManager kinectManager, bool isPlayMode)
|
|||
|
|
{
|
|||
|
|
base.UpdateSensorData(sensorData, kinectManager, isPlayMode);
|
|||
|
|
|
|||
|
|
if(sensorData.colorCamIntr == null && colorStreamProfile != null)
|
|||
|
|
{
|
|||
|
|
lock (colorFrameLock)
|
|||
|
|
{
|
|||
|
|
Intel.RealSense.Intrinsics colorCamIntr = colorStreamProfile.GetIntrinsics();
|
|||
|
|
|
|||
|
|
if (colorCamIntr.model != Distortion.None)
|
|||
|
|
{
|
|||
|
|
GetCameraIntrinsics(colorCamIntr, ref sensorData.colorCamIntr);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (sensorData.depthCamIntr == null && depthStreamProfile != null)
|
|||
|
|
{
|
|||
|
|
lock (depthFrameLock)
|
|||
|
|
{
|
|||
|
|
Intel.RealSense.Intrinsics depthCamIntr = depthStreamProfile.GetIntrinsics();
|
|||
|
|
//Debug.Log("RS distType: " + depthCamIntr.model);
|
|||
|
|
|
|||
|
|
if (depthCamIntr.model != Distortion.None || deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
|
|||
|
|
{
|
|||
|
|
GetCameraIntrinsics(depthCamIntr, ref sensorData.depthCamIntr);
|
|||
|
|
|
|||
|
|
if (depthCamIntr.model == Distortion.None && deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
|
|||
|
|
{
|
|||
|
|
// workaround for playback mode (model & coeffs are missing there)
|
|||
|
|
sensorData.depthCamIntr.distType = KinectInterop.DistortionType.BrownConrady;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// body & body-index data
|
|||
|
|
if ((frameSourceFlags & KinectInterop.FrameSource.TypeBody) != 0 && depthCamIntr.width == 640 && depthCamIntr.height == 480)
|
|||
|
|
{
|
|||
|
|
Calibration cal = GetBodyTrackerCalibration(sensorData.depthCamIntr);
|
|||
|
|
InitBodyTracking(frameSourceFlags, sensorData, cal, true);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(sensorData.depth2ColorExtr == null && depthStreamProfile != null && colorStreamProfile != null)
|
|||
|
|
{
|
|||
|
|
lock (depthFrameLock)
|
|||
|
|
{
|
|||
|
|
lock (colorFrameLock)
|
|||
|
|
{
|
|||
|
|
Intel.RealSense.Extrinsics depth2ColorExtr = depthStreamProfile.GetExtrinsicsTo(colorStreamProfile);
|
|||
|
|
GetCameraExtrinsics(depth2ColorExtr, ref sensorData.depth2ColorExtr);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (sensorData.color2DepthExtr == null && colorStreamProfile != null && depthStreamProfile != null)
|
|||
|
|
{
|
|||
|
|
lock (colorFrameLock)
|
|||
|
|
{
|
|||
|
|
lock (depthFrameLock)
|
|||
|
|
{
|
|||
|
|
Intel.RealSense.Extrinsics color2DepthExtr = colorStreamProfile.GetExtrinsicsTo(depthStreamProfile);
|
|||
|
|
GetCameraExtrinsics(color2DepthExtr, ref sensorData.color2DepthExtr);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// gets the given camera intrinsics
|
|||
|
|
private void GetCameraIntrinsics(Intel.RealSense.Intrinsics camIntr, ref KinectInterop.CameraIntrinsics intr)
|
|||
|
|
{
|
|||
|
|
intr = new KinectInterop.CameraIntrinsics();
|
|||
|
|
|
|||
|
|
intr.width = camIntr.width;
|
|||
|
|
intr.height = camIntr.height;
|
|||
|
|
|
|||
|
|
intr.ppx = camIntr.ppx;
|
|||
|
|
intr.ppy = camIntr.ppy;
|
|||
|
|
|
|||
|
|
intr.fx = camIntr.fx;
|
|||
|
|
intr.fy = camIntr.fy;
|
|||
|
|
|
|||
|
|
intr.distCoeffs = new float[camIntr.coeffs.Length];
|
|||
|
|
camIntr.coeffs.CopyTo(intr.distCoeffs, 0);
|
|||
|
|
|
|||
|
|
intr.distType = (KinectInterop.DistortionType)camIntr.model;
|
|||
|
|
|
|||
|
|
EstimateFOV(intr);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// gets the given camera extrinsics
|
|||
|
|
private void GetCameraExtrinsics(Intel.RealSense.Extrinsics camExtr, ref KinectInterop.CameraExtrinsics extr)
|
|||
|
|
{
|
|||
|
|
extr = new KinectInterop.CameraExtrinsics();
|
|||
|
|
|
|||
|
|
extr.rotation = new float[camExtr.rotation.Length];
|
|||
|
|
camExtr.rotation.CopyTo(extr.rotation, 0);
|
|||
|
|
|
|||
|
|
extr.translation = new float[camExtr.translation.Length];
|
|||
|
|
camExtr.translation.CopyTo(extr.translation, 0);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// unprojects plane point into the space
|
|||
|
|
protected override Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth)
|
|||
|
|
{
|
|||
|
|
float x = (pixel.x - intr.ppx) / intr.fx;
|
|||
|
|
float y = (pixel.y - intr.ppy) / intr.fy;
|
|||
|
|
|
|||
|
|
if (intr.distType == KinectInterop.DistortionType.InverseBrownConrady)
|
|||
|
|
{
|
|||
|
|
float r2 = x * x + y * y;
|
|||
|
|
float f = 1 + intr.distCoeffs[0] * r2 + intr.distCoeffs[1] * r2 * r2 + intr.distCoeffs[4] * r2 * r2 * r2;
|
|||
|
|
|
|||
|
|
float ux = x * f + 2 * intr.distCoeffs[2] * x * y + intr.distCoeffs[3] * (r2 + 2 * x * x);
|
|||
|
|
float uy = y * f + 2 * intr.distCoeffs[3] * x * y + intr.distCoeffs[2] * (r2 + 2 * y * y);
|
|||
|
|
|
|||
|
|
x = ux;
|
|||
|
|
y = uy;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vector3 point = new Vector3(depth * x, depth * y, depth);
|
|||
|
|
|
|||
|
|
return point;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// projects space point onto a plane
|
|||
|
|
protected override Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point)
|
|||
|
|
{
|
|||
|
|
float x = point.x / point.z;
|
|||
|
|
float y = point.y / point.z;
|
|||
|
|
|
|||
|
|
if (intr.distType == KinectInterop.DistortionType.ModifiedBrownConrady)
|
|||
|
|
{
|
|||
|
|
float r2 = x * x + y * y;
|
|||
|
|
float f = 1f + intr.distCoeffs[0] * r2 + intr.distCoeffs[1] * r2 * r2 + intr.distCoeffs[4] * r2 * r2 * r2;
|
|||
|
|
|
|||
|
|
x *= f;
|
|||
|
|
y *= f;
|
|||
|
|
|
|||
|
|
float dx = x + 2f * intr.distCoeffs[2] * x * y + intr.distCoeffs[3] * (r2 + 2 * x * x);
|
|||
|
|
float dy = y + 2f * intr.distCoeffs[3] * x * y + intr.distCoeffs[2] * (r2 + 2 * y * y);
|
|||
|
|
|
|||
|
|
x = dx;
|
|||
|
|
y = dy;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (intr.distType == KinectInterop.DistortionType.Theta)
|
|||
|
|
{
|
|||
|
|
float r = (float)Math.Sqrt(x * x + y * y);
|
|||
|
|
float rd = (1f / intr.distCoeffs[0] * (float)Math.Atan(2f * r * (float)Math.Tan(intr.distCoeffs[0] / 2f)));
|
|||
|
|
|
|||
|
|
x *= rd / r;
|
|||
|
|
y *= rd / r;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Vector2 pixel = new Vector2(x * intr.fx + intr.ppx, y * intr.fy + intr.ppy);
|
|||
|
|
|
|||
|
|
return pixel;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// transforms a point from one space to another
|
|||
|
|
protected override Vector3 TransformPoint(KinectInterop.CameraExtrinsics extr, Vector3 point)
|
|||
|
|
{
|
|||
|
|
float toPointX = extr.rotation[0] * point.x + extr.rotation[3] * point.y + extr.rotation[6] * point.z + extr.translation[0];
|
|||
|
|
float toPointY = extr.rotation[1] * point.x + extr.rotation[4] * point.y + extr.rotation[7] * point.z + extr.translation[1];
|
|||
|
|
float toPointZ = extr.rotation[2] * point.x + extr.rotation[5] * point.y + extr.rotation[8] * point.z + extr.translation[2];
|
|||
|
|
|
|||
|
|
return new Vector3(toPointX, toPointY, toPointZ);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public override void PollCoordTransformFrames(KinectInterop.SensorData sensorData)
|
|||
|
|
{
|
|||
|
|
if (lastDepthCoordFrameTime != rawDepthTimestamp)
|
|||
|
|
{
|
|||
|
|
lastDepthCoordFrameTime = rawDepthTimestamp;
|
|||
|
|
|
|||
|
|
// depth2color frame
|
|||
|
|
if (depth2ColorDataFrame != null)
|
|||
|
|
{
|
|||
|
|
lock (depth2ColorFrameLock)
|
|||
|
|
{
|
|||
|
|
TransformDepthFrameToColorFrame(sensorData, depth2ColorDataFrame);
|
|||
|
|
|
|||
|
|
lastDepth2ColorFrameTime = lastDepthCoordFrameTime;
|
|||
|
|
//Debug.Log("Depth2ColorFrameTime: " + lastDepth2ColorFrameTime);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// color2depth frame
|
|||
|
|
if (color2DepthDataFrame != null)
|
|||
|
|
{
|
|||
|
|
lock (color2DepthFrameLock)
|
|||
|
|
{
|
|||
|
|
TransformColorFrameToDepthFrame(sensorData, color2DepthDataFrame);
|
|||
|
|
|
|||
|
|
lastColor2DepthFrameTime = lastDepthCoordFrameTime;
|
|||
|
|
//Debug.Log("Color2DepthFrameTime: " + lastColor2DepthFrameTime);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// transforms the depth frame to depth-aligned color frame
|
|||
|
|
protected bool TransformDepthFrameToColorFrame(KinectInterop.SensorData sensorData, byte[] d2cColorFrame)
|
|||
|
|
{
|
|||
|
|
if (rawDepthImage != null && rawColorImage != null && sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.depth2ColorExtr != null)
|
|||
|
|
{
|
|||
|
|
int depthImageW = sensorData.depthImageWidth;
|
|||
|
|
int depthImageH = sensorData.depthImageHeight;
|
|||
|
|
|
|||
|
|
int colorImageW = sensorData.colorImageWidth;
|
|||
|
|
int colorImageS = sensorData.colorImageStride;
|
|||
|
|
|
|||
|
|
int mapImageLen = depthImageW * depthImageH * colorImageS;
|
|||
|
|
if (d2cColorFrame == null || d2cColorFrame.Length != mapImageLen)
|
|||
|
|
{
|
|||
|
|
throw new Exception("d2cColorFrame is not big enough. Should be " + mapImageLen + " bytes.");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
for (int dy = 0, dIndex = 0, d2cIndex = 0; dy < depthImageH; dy++)
|
|||
|
|
{
|
|||
|
|
for (int dx = 0; dx < depthImageW; dx++, dIndex++, d2cIndex += colorImageS)
|
|||
|
|
{
|
|||
|
|
ushort depthVal = rawDepthImage[dIndex];
|
|||
|
|
|
|||
|
|
if (depthVal != 0)
|
|||
|
|
{
|
|||
|
|
Vector2 depthPos = new Vector2(dx, dy);
|
|||
|
|
|
|||
|
|
Vector3 depthSpacePos = UnprojectPoint(sensorData.depthCamIntr, depthPos, (float)depthVal / 1000f);
|
|||
|
|
Vector3 colorSpacePos = TransformPoint(sensorData.depth2ColorExtr, depthSpacePos);
|
|||
|
|
Vector2 colorFramePos = ProjectPoint(sensorData.colorCamIntr, colorSpacePos);
|
|||
|
|
|
|||
|
|
int cIndex = ((int)colorFramePos.x + (int)colorFramePos.y * colorImageW) * colorImageS;
|
|||
|
|
|
|||
|
|
if (cIndex >= 0 && (cIndex + colorImageS - 1) < rawColorImage.Length)
|
|||
|
|
{
|
|||
|
|
d2cColorFrame[d2cIndex] = rawColorImage[cIndex];
|
|||
|
|
d2cColorFrame[d2cIndex + 1] = rawColorImage[cIndex + 1];
|
|||
|
|
d2cColorFrame[d2cIndex + 2] = rawColorImage[cIndex + 2];
|
|||
|
|
//d2cColorFrame[d2cIndex + 3] = 255;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
d2cColorFrame[d2cIndex] = 0;
|
|||
|
|
d2cColorFrame[d2cIndex + 1] = 0;
|
|||
|
|
d2cColorFrame[d2cIndex + 2] = 0;
|
|||
|
|
//d2cColorFrame[d2cIndex + 3] = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
// transforms the color frame to color-aligned depth frame
|
|||
|
|
protected bool TransformColorFrameToDepthFrame(KinectInterop.SensorData sensorData, ushort[] c2dDepthFrame)
|
|||
|
|
{
|
|||
|
|
if (rawDepthImage != null && rawColorImage != null && sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.depth2ColorExtr != null)
|
|||
|
|
{
|
|||
|
|
int depthImageW = sensorData.depthImageWidth;
|
|||
|
|
int depthImageH = sensorData.depthImageHeight;
|
|||
|
|
|
|||
|
|
int colorImageW = sensorData.colorImageWidth;
|
|||
|
|
int colorImageH = sensorData.colorImageHeight;
|
|||
|
|
|
|||
|
|
int mapImageLen = sensorData.colorImageWidth * sensorData.colorImageHeight;
|
|||
|
|
if (c2dDepthFrame == null || c2dDepthFrame.Length != mapImageLen)
|
|||
|
|
{
|
|||
|
|
throw new Exception("c2dDepthFrame is not big enough. Should be " + mapImageLen + " ushorts.");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//Intrinsics depthIntr = depthStreamProfile.GetIntrinsics();
|
|||
|
|
//Intrinsics colorIntr = colorStreamProfile.GetIntrinsics();
|
|||
|
|
//Extrinsics d2cExtr = depthStreamProfile.GetExtrinsicsTo(colorStreamProfile);
|
|||
|
|
|
|||
|
|
for (int dy = 0, dIndex = 0; dy < depthImageH; dy++)
|
|||
|
|
{
|
|||
|
|
for (int dx = 0; dx < depthImageW; dx++, dIndex++)
|
|||
|
|
{
|
|||
|
|
ushort depthVal = sensorData.depthImage[dIndex];
|
|||
|
|
|
|||
|
|
if (depthVal != 0)
|
|||
|
|
{
|
|||
|
|
float depth = (float)depthVal / 1000f;
|
|||
|
|
|
|||
|
|
Vector2 depthPos1 = new Vector2(dx - 0.5f, dy - 0.5f);
|
|||
|
|
Vector3 depthSpacePos1 = UnprojectPoint(sensorData.depthCamIntr, depthPos1, depth);
|
|||
|
|
Vector3 colorSpacePos1 = TransformPoint(sensorData.depth2ColorExtr, depthSpacePos1);
|
|||
|
|
Vector2 colorPos1 = ProjectPoint(sensorData.colorCamIntr, colorSpacePos1);
|
|||
|
|
|
|||
|
|
int colorPos1X = Mathf.RoundToInt(colorPos1.x);
|
|||
|
|
int colorPos1Y = Mathf.RoundToInt(colorPos1.y);
|
|||
|
|
|
|||
|
|
Vector2 depthPos2 = new Vector2(dx + 0.5f, dy + 0.5f);
|
|||
|
|
Vector3 depthSpacePos2 = UnprojectPoint(sensorData.depthCamIntr, depthPos2, depth);
|
|||
|
|
Vector3 colorSpacePos2 = TransformPoint(sensorData.depth2ColorExtr, depthSpacePos2);
|
|||
|
|
Vector2 colorPos2 = ProjectPoint(sensorData.colorCamIntr, colorSpacePos2);
|
|||
|
|
|
|||
|
|
int colorPos2X = Mathf.RoundToInt(colorPos2.x);
|
|||
|
|
int colorPos2Y = Mathf.RoundToInt(colorPos2.y);
|
|||
|
|
|
|||
|
|
if (colorPos1X < 0 || colorPos1Y < 0 || colorPos2X >= colorImageW || colorPos2Y >= colorImageH)
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
for (int y = colorPos1Y; y <= colorPos2Y; y++)
|
|||
|
|
{
|
|||
|
|
int cIndex = y * colorImageW + colorPos1X;
|
|||
|
|
|
|||
|
|
for (int x = colorPos1X; x <= colorPos2X; x++, cIndex++)
|
|||
|
|
{
|
|||
|
|
c2dDepthFrame[cIndex] = depthVal;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//c2dDepthFrame[cIndex] = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|