void Update()
{
if (_KinectSensor != null)
{
if (_KinectSensor.RawDepthImage != null)
{
short[] depthImage = _KinectSensor.RawDepthImage;
System.Buffer.BlockCopy(depthImage, 0, _DepthRawData, 0, _DepthRawData.Length);
// _PointCloudRenderer.UpdateColorTexture(_KinectSensor.TransformedColorImage);
_PointCloudRenderer.UpdateDepthTexture(_DepthRawData);
}
System.Numerics.Vector3 accel = _KinectSensor.ImuSample.AccelerometerSample;
if (LowPassFilter)
{
_Accel[0] = accel.X;
_Accel[1] = accel.Y;
_Accel[2] = accel.Z;
_LowPassFilter.Apply(_Accel, ref _AccelOut);
accel.X = _AccelOut[0];
accel.Y = _AccelOut[1];
accel.Z = _AccelOut[2];
}
UnityEngine.Vector3 kinectOrientation = OrientationEstimator.EstimateForAzureKinect(accel);
this.transform.eulerAngles = kinectOrientation;
}
}
void Update()
{
_KinectSensor = _AzureKinectManager.SensorList[_DeviceNumber];
if (_KinectSensor != null)
{
if (_KinectSensor.RawColorImage != null)
{
_ColorImageTexture.LoadRawTextureData(_KinectSensor.RawColorImage);
_ColorImageTexture.Apply();
}
if (_KinectSensor.PointCloud != null)
{
Short3[] pointCloud = _KinectSensor.PointCloud;
Vector3[] vertices = new Vector3[pointCloud.Length];
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = new Vector3(pointCloud[i].X * 0.001f, pointCloud[i].Y * -0.001f, pointCloud[i].Z * 0.001f);
}
_PointCloudMesh.UpdateVertices(vertices);
_PointCloudMesh.UpdateColorTexture(_KinectSensor.RawColorImage);
}
System.Numerics.Vector3 accel = _KinectSensor.ImuSample.AccelerometerSample;
if (LowPassFilter)
{
_Accel[0] = accel.X;
_Accel[1] = accel.Y;
_Accel[2] = accel.Z;
_LowPassFilter.Apply(_Accel, ref _AccelOut);
accel.X = _AccelOut[0];
accel.Y = _AccelOut[1];
accel.Z = _AccelOut[2];
}
UnityEngine.Vector3 kinectOrientation = OrientationEstimator.EstimateFromAccelerometerForUnity(accel);
this.transform.eulerAngles = kinectOrientation;
}
}
void Update()
{
if (_KinectSensor != null)
{
if (_KinectSensor.PointCloud != null && _KinectSensor.ImuSample != null)
{
int downsampleStep = 2;
int minimumFloorPointCount = 1024 / (downsampleStep * downsampleStep);
if (_FloorDetector.TryDetectFloorPlane(out AzureKinectToolkit.Plane? plane, _KinectSensor.PointCloud,
_KinectSensor.DepthImageWidth, _KinectSensor.DepthImageHeight, downsampleStep,
_KinectSensor.ImuSample, _KinectSensor.DeviceCalibration, minimumFloorPointCount))
{
Vector3 planeOrigin = new Vector3(plane.Value.Origin.X, plane.Value.Origin.Y, plane.Value.Origin.Z);
Vector3 planeNormal = Vector3.Normalize(new Vector3(plane.Value.Normal.X, plane.Value.Normal.Y, plane.Value.Normal.Z));
// Vector3 forwardDirection = planeOrigin - Vector3.Dot(planeOrigin, planeNormal) * planeNormal;
// float forwardDistance = forwardDirection.magnitude;
float height = Vector3.Dot(planeOrigin, planeNormal);
// Vector3 kinectPos = new Vector3(0, -height, -forwardDistance);
Vector3 kinectPos = new Vector3(0, -height, 0);
if (_LowPassFilterEnabled)
{
_KinectPos[0] = kinectPos.x;
_KinectPos[1] = kinectPos.y;
_KinectPos[2] = kinectPos.z;
_LowPassFilter.Apply(_KinectPos, ref _KinectPosOut);
kinectPos.x = _KinectPosOut[0];
kinectPos.y = _KinectPosOut[1];
kinectPos.z = _KinectPosOut[2];
}
_KinectObject.transform.position = kinectPos;
}
}
}
}