You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
629 lines
26 KiB
629 lines
26 KiB
1 year ago
|
using System.Collections;
|
||
|
|
using System.Collections.Generic;
|
||
|
|
using UnityEngine;
|
||
|
|
using com.rfilkov.kinect;
|
||
|
|
|
||
|
|
|
||
|
|
namespace com.rfilkov.kinect
|
||
|
|
{
|
||
|
|
/// <summary>
|
||
|
|
/// KinectFloorDetector is based on the 'Azure Kinect Floor Plane Detection Sample' that demonstrates one way to estimate the floor plane.
|
||
|
|
/// </summary>
|
||
|
|
public class KinectFloorDetector
|
||
|
|
{
|
||
|
|
/// <summary>
|
||
|
|
/// Smoothing factor used for sensor position and rotation update.
|
||
|
|
/// </summary>
|
||
|
|
public float smoothFactor = 5f;
|
||
|
|
|
||
|
|
// reference to the sensor data
|
||
|
|
private KinectInterop.SensorData sensorData = null;
|
||
|
|
private Vector3 spaceScale = Vector3.one;
|
||
|
|
private ulong lastDepthFrameTime = 0;
|
||
|
|
|
||
|
|
// IMU data
|
||
|
|
private Vector3 imuUpVector = Vector3.zero;
|
||
|
|
//private Transform imuVectorTrans = null;
|
||
|
|
|
||
|
|
// data buffers
|
||
|
|
private Vector3[] depth2SpaceTable = null;
|
||
|
|
private int depth2SpaceWidth = 0, depth2SpaceHeight = 0;
|
||
|
|
private float[] histMinMax = null;
|
||
|
|
private float[] planePosNorm = null;
|
||
|
|
|
||
|
|
private int binAggregation = 6;
|
||
|
|
public int minFloorPointCount = 1024;
|
||
|
|
public float planeMaxTiltInDeg = 5f;
|
||
|
|
private float histBinSize = 0f;
|
||
|
|
private int histBufferLength = 0;
|
||
|
|
|
||
|
|
// compute buffers
|
||
|
|
private ComputeBuffer pointCloudSpaceBuffer = null;
|
||
|
|
private ComputeBuffer pointCloudDepthBuffer = null;
|
||
|
|
|
||
|
|
private ComputeBuffer pointCloudPosBuffer = null;
|
||
|
|
private ComputeBuffer pointCloudOfsBuffer = null;
|
||
|
|
private ComputeBuffer pointCloudMaskBuffer = null;
|
||
|
|
|
||
|
|
private ComputeBuffer ofsHistMinMaxBuffer = null;
|
||
|
|
private ComputeBuffer ofsHistBinLeftBuffer = null;
|
||
|
|
private ComputeBuffer ofsHistBinCountBuffer = null;
|
||
|
|
private ComputeBuffer histCumulativeCountBuffer = null;
|
||
|
|
private ComputeBuffer planeIndicesBuffer = null;
|
||
|
|
private ComputeBuffer planePosNormBuffer = null;
|
||
|
|
|
||
|
|
// compute shaders
|
||
|
|
private ComputeShader floorDetOffsetEstShader = null;
|
||
|
|
private int floorDetOffsetEstKernel = -1;
|
||
|
|
|
||
|
|
private ComputeShader floorDetOffsetMinMaxShader = null;
|
||
|
|
private int floorDetOffsetMinMaxKernel = -1;
|
||
|
|
|
||
|
|
private ComputeShader floorDetOffsetHistShader = null;
|
||
|
|
private int floorDetOffsetHistKernel = -1;
|
||
|
|
|
||
|
|
private ComputeShader floorDetPlaneEstShader = null;
|
||
|
|
private int floorDetPlaneEstKernel = -1;
|
||
|
|
|
||
|
|
// results
|
||
|
|
private bool bPlaneValid = false;
|
||
|
|
private Vector3 vPlanePos = Vector3.zero;
|
||
|
|
private Vector3 vPlaneNorm = Vector3.up;
|
||
|
|
private Quaternion qSensorRot = Quaternion.identity;
|
||
|
|
private Plane floorPlane = new Plane();
|
||
|
|
private float fSensorHeight = 1f;
|
||
|
|
|
||
|
|
// time
|
||
|
|
private const float SMOOTH_TIME_THRESHOLD = 1f;
|
||
|
|
private float fLastTimeSecs = 0f;
|
||
|
|
|
||
|
|
// routine params
|
||
|
|
private const int WAIT_FRAMES_BEFORE_GPUGET = 2;
|
||
|
|
private int minDepthDistance = 0;
|
||
|
|
private int maxDepthDistance = 10000;
|
||
|
|
|
||
|
|
//private bool isImuVectorSet = false;
|
||
|
|
private bool isDepthFrameSet = false;
|
||
|
|
|
||
|
|
private MonoBehaviour callerInstance = null;
|
||
|
|
private IEnumerator floorRoutine = null;
|
||
|
|
private bool isRoutineRunning = false;
|
||
|
|
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Checks if a floor plane has been detected or not.
|
||
|
|
/// </summary>
|
||
|
|
/// <returns>true if the floor plane is valid, false otherwise</returns>
|
||
|
|
public bool IsFloorValid()
|
||
|
|
{
|
||
|
|
return bPlaneValid;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Gets the timestamp of the depth frame used for floor plane detection.
|
||
|
|
/// </summary>
|
||
|
|
/// <returns>Depth frame timestamp</returns>
|
||
|
|
public ulong GetDepthTimestamp()
|
||
|
|
{
|
||
|
|
return lastDepthFrameTime;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Gets the floor plane position.
|
||
|
|
/// </summary>
|
||
|
|
/// <returns>Floor plane position</returns>
|
||
|
|
public Vector3 GetFloorPosition()
|
||
|
|
{
|
||
|
|
return vPlanePos;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Gets the floor plane normal.
|
||
|
|
/// </summary>
|
||
|
|
/// <returns>Floor plane normal</returns>
|
||
|
|
public Vector3 GetFloorNormal()
|
||
|
|
{
|
||
|
|
return vPlaneNorm;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Gets the detected floor plane.
|
||
|
|
/// </summary>
|
||
|
|
/// <returns>Detected floor plane</returns>
|
||
|
|
public Plane GetFloorPlane()
|
||
|
|
{
|
||
|
|
return floorPlane;
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Gets the estimated sensor position, in meters.
|
||
|
|
/// </summary>
|
||
|
|
/// <returns>Sensor position, in meters</returns>
|
||
|
|
public Vector3 GetSensorPosition()
|
||
|
|
{
|
||
|
|
return new Vector3(0f, fSensorHeight, 0f);
|
||
|
|
}
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Gets the estimated sensor rotation.
|
||
|
|
/// </summary>
|
||
|
|
/// <returns>Sensor rotation</returns>
|
||
|
|
public Quaternion GetSensorRotation()
|
||
|
|
{
|
||
|
|
return qSensorRot;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Initializes the buffers and shaders used by the floor detector.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="sensorData">Sensor data</param>
|
||
|
|
/// <param name="maxDepthMm">Max depth distance in mm</param>
|
||
|
|
public void InitFloorDetector(MonoBehaviour caller, KinectInterop.SensorData sensorData, int maxDepthMm)
|
||
|
|
{
|
||
|
|
this.callerInstance = caller;
|
||
|
|
this.sensorData = sensorData;
|
||
|
|
|
||
|
|
if (sensorData == null || sensorData.depthImageWidth == 0 || sensorData.depthImageHeight == 0)
|
||
|
|
return;
|
||
|
|
|
||
|
|
if (floorDetOffsetEstShader == null)
|
||
|
|
{
|
||
|
|
floorDetOffsetEstShader = Resources.Load("FloorDetectionOffsetEstShader") as ComputeShader;
|
||
|
|
floorDetOffsetEstKernel = floorDetOffsetEstShader != null ? floorDetOffsetEstShader.FindKernel("EstimatePointCloudPosOfs") : -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (floorDetOffsetMinMaxShader == null)
|
||
|
|
{
|
||
|
|
floorDetOffsetMinMaxShader = Resources.Load("FloorDetectionOffsetMinMaxShader") as ComputeShader;
|
||
|
|
floorDetOffsetMinMaxKernel = floorDetOffsetMinMaxShader != null ? floorDetOffsetMinMaxShader.FindKernel("EstimateOffsetMinMax") : -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (floorDetOffsetHistShader == null)
|
||
|
|
{
|
||
|
|
floorDetOffsetHistShader = Resources.Load("FloorDetectionOffsetHistShader") as ComputeShader;
|
||
|
|
floorDetOffsetHistKernel = floorDetOffsetHistShader != null ? floorDetOffsetHistShader.FindKernel("EstimateOffsetHist") : -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (floorDetPlaneEstShader == null)
|
||
|
|
{
|
||
|
|
floorDetPlaneEstShader = Resources.Load("FloorDetectionPlanePointsShader") as ComputeShader;
|
||
|
|
floorDetPlaneEstKernel = floorDetPlaneEstShader != null ? floorDetPlaneEstShader.FindKernel("EstimatePlanePoints") : -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (pointCloudSpaceBuffer == null)
|
||
|
|
{
|
||
|
|
int spaceBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight * 3;
|
||
|
|
pointCloudSpaceBuffer = new ComputeBuffer(spaceBufferLength, sizeof(float));
|
||
|
|
}
|
||
|
|
|
||
|
|
if(pointCloudDepthBuffer == null)
|
||
|
|
{
|
||
|
|
int depthBufferLength = (sensorData.depthImageWidth * sensorData.depthImageHeight) >> 1;
|
||
|
|
pointCloudDepthBuffer = new ComputeBuffer(depthBufferLength, sizeof(uint));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (pointCloudPosBuffer == null)
|
||
|
|
{
|
||
|
|
int posBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight * 3;
|
||
|
|
pointCloudPosBuffer = new ComputeBuffer(posBufferLength, sizeof(float));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (pointCloudOfsBuffer == null)
|
||
|
|
{
|
||
|
|
int ofsBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight;
|
||
|
|
pointCloudOfsBuffer = new ComputeBuffer(ofsBufferLength, sizeof(float));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (pointCloudMaskBuffer == null)
|
||
|
|
{
|
||
|
|
int maskBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight;
|
||
|
|
pointCloudMaskBuffer = new ComputeBuffer(maskBufferLength, sizeof(int));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (ofsHistMinMaxBuffer == null)
|
||
|
|
{
|
||
|
|
histMinMax = new float[2];
|
||
|
|
ofsHistMinMaxBuffer = new ComputeBuffer(histMinMax.Length, sizeof(float));
|
||
|
|
}
|
||
|
|
|
||
|
|
// hist bin size
|
||
|
|
float planeDisplacementRangeInMeters = 0.050f; // 5 cm in meters
|
||
|
|
//binAggregation = 6;
|
||
|
|
histBinSize = planeDisplacementRangeInMeters / binAggregation;
|
||
|
|
|
||
|
|
float fMaxDepth = (float)maxDepthMm / 1000f;
|
||
|
|
histBufferLength = Mathf.FloorToInt(2 * fMaxDepth / histBinSize) + 1;
|
||
|
|
//Debug.Log("histBinSize: " + histBinSize + ", histBufferLength: " + histBufferLength);
|
||
|
|
|
||
|
|
if (ofsHistBinLeftBuffer == null)
|
||
|
|
{
|
||
|
|
ofsHistBinLeftBuffer = new ComputeBuffer(histBufferLength, sizeof(float));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (ofsHistBinCountBuffer == null)
|
||
|
|
{
|
||
|
|
ofsHistBinCountBuffer = new ComputeBuffer(histBufferLength, sizeof(uint));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (histCumulativeCountBuffer == null)
|
||
|
|
{
|
||
|
|
histCumulativeCountBuffer = new ComputeBuffer(histBufferLength, sizeof(uint));
|
||
|
|
}
|
||
|
|
|
||
|
|
if(planeIndicesBuffer == null)
|
||
|
|
{
|
||
|
|
int planeIndicesLength = sensorData.depthImageWidth * sensorData.depthImageHeight;
|
||
|
|
planeIndicesBuffer = new ComputeBuffer(planeIndicesLength, sizeof(uint));
|
||
|
|
}
|
||
|
|
|
||
|
|
if(planePosNormBuffer == null)
|
||
|
|
{
|
||
|
|
planePosNorm = new float[4 * 3]; // pos & norm are v3
|
||
|
|
planePosNormBuffer = new ComputeBuffer(planePosNorm.Length, sizeof(float));
|
||
|
|
}
|
||
|
|
|
||
|
|
spaceScale = sensorData.sensorSpaceScale;
|
||
|
|
//minFloorPointCount = 1024;
|
||
|
|
//planeMaxTiltInDeg = 5f;
|
||
|
|
imuUpVector = Vector3.up;
|
||
|
|
|
||
|
|
bPlaneValid = false;
|
||
|
|
|
||
|
|
if(callerInstance != null)
|
||
|
|
{
|
||
|
|
isRoutineRunning = true;
|
||
|
|
floorRoutine = UpdateFloorAsync();
|
||
|
|
callerInstance.StartCoroutine(floorRoutine);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Releases the buffers and shaders used by the floor detector.
|
||
|
|
/// </summary>
|
||
|
|
public void FinishFloorDetector()
|
||
|
|
{
|
||
|
|
if(isRoutineRunning)
|
||
|
|
{
|
||
|
|
isRoutineRunning = false;
|
||
|
|
callerInstance.StopCoroutine(floorRoutine);
|
||
|
|
floorRoutine = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (pointCloudSpaceBuffer != null)
|
||
|
|
{
|
||
|
|
pointCloudSpaceBuffer.Dispose();
|
||
|
|
pointCloudSpaceBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (pointCloudDepthBuffer != null)
|
||
|
|
{
|
||
|
|
pointCloudDepthBuffer.Dispose();
|
||
|
|
pointCloudDepthBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (pointCloudPosBuffer != null)
|
||
|
|
{
|
||
|
|
pointCloudPosBuffer.Dispose();
|
||
|
|
pointCloudPosBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (pointCloudOfsBuffer != null)
|
||
|
|
{
|
||
|
|
pointCloudOfsBuffer.Dispose();
|
||
|
|
pointCloudOfsBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (pointCloudMaskBuffer != null)
|
||
|
|
{
|
||
|
|
pointCloudMaskBuffer.Dispose();
|
||
|
|
pointCloudMaskBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (ofsHistMinMaxBuffer != null)
|
||
|
|
{
|
||
|
|
ofsHistMinMaxBuffer.Dispose();
|
||
|
|
ofsHistMinMaxBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (ofsHistBinLeftBuffer != null)
|
||
|
|
{
|
||
|
|
ofsHistBinLeftBuffer.Dispose();
|
||
|
|
ofsHistBinLeftBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (ofsHistBinCountBuffer != null)
|
||
|
|
{
|
||
|
|
ofsHistBinCountBuffer.Dispose();
|
||
|
|
ofsHistBinCountBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (histCumulativeCountBuffer != null)
|
||
|
|
{
|
||
|
|
histCumulativeCountBuffer.Dispose();
|
||
|
|
histCumulativeCountBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(planeIndicesBuffer != null)
|
||
|
|
{
|
||
|
|
planeIndicesBuffer.Dispose();
|
||
|
|
planeIndicesBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(planePosNormBuffer != null)
|
||
|
|
{
|
||
|
|
planePosNormBuffer.Dispose();
|
||
|
|
planePosNormBuffer = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (floorDetOffsetEstShader != null)
|
||
|
|
{
|
||
|
|
floorDetOffsetEstShader = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (floorDetOffsetMinMaxShader != null)
|
||
|
|
{
|
||
|
|
floorDetOffsetMinMaxShader = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (floorDetOffsetHistShader != null)
|
||
|
|
{
|
||
|
|
floorDetOffsetHistShader = null;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(floorDetPlaneEstShader != null)
|
||
|
|
{
|
||
|
|
floorDetPlaneEstShader = null;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
///// <summary>
|
||
|
|
///// Updates the IMU up vector from the sample.
|
||
|
|
///// </summary>
|
||
|
|
///// <param name="imuAcc">IMU accelerometer sample</param>
|
||
|
|
///// <param name="accDepthRot">Extrinsics rotation between the accelerometer and depth sensor</param>
|
||
|
|
//public void UpdateImuUpVector(Vector3 imuAcc, float[] accDepthRot)
|
||
|
|
//{
|
||
|
|
// Vector3 Rx = new Vector3(accDepthRot[0], accDepthRot[1], accDepthRot[2]);
|
||
|
|
// Vector3 Ry = new Vector3(accDepthRot[3], accDepthRot[4], accDepthRot[5]);
|
||
|
|
// Vector3 Rz = new Vector3(accDepthRot[6], accDepthRot[7], accDepthRot[8]);
|
||
|
|
// Vector3 depthAcc = new Vector3( Vector3.Dot(Rx, imuAcc), Vector3.Dot(Ry, imuAcc), Vector3.Dot(Rz, imuAcc));
|
||
|
|
|
||
|
|
// //Vector3 depthGravity = depthAcc * -1f;
|
||
|
|
// //imuUpVector = (depthGravity * -1f).normalized;
|
||
|
|
// imuUpVector = depthAcc.normalized;
|
||
|
|
// //isImuVectorSet = true;
|
||
|
|
|
||
|
|
// //Debug.Log("imuUpVector: " + imuUpVector);
|
||
|
|
//}
|
||
|
|
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Updates the IMU up vector.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="imuUpVector">IMU up vector</param>
|
||
|
|
public void UpdateImuUpVector(Vector3 imuUpVector)
|
||
|
|
{
|
||
|
|
this.imuUpVector = imuUpVector.normalized;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/// <summary>
|
||
|
|
/// Executes the floor detector shaders with the current depth frame data.
|
||
|
|
/// </summary>
|
||
|
|
/// <param name="depthFrame">Depth frame data</param>
|
||
|
|
/// <param name="depthFrameTime">Depth frame time</param>
|
||
|
|
/// <param name="depthFrameLock">Depth frame lock object</param>
|
||
|
|
/// <param name="minDistance">Min depth distance, in meters</param>
|
||
|
|
/// <param name="maxDistance">Max depth distance, in meters</param>
|
||
|
|
/// <returns>true if the floor plane is detected, false otherwise</returns>
|
||
|
|
public bool UpdateFloorDetector(ushort[] depthFrame, ulong depthFrameTime, ref object depthFrameLock, float minDistance, float maxDistance)
|
||
|
|
{
|
||
|
|
if (sensorData == null || depthFrame == null || sensorData.depthImageWidth == 0 || sensorData.depthImageHeight == 0)
|
||
|
|
return false;
|
||
|
|
if (lastDepthFrameTime == depthFrameTime)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
lastDepthFrameTime = depthFrameTime;
|
||
|
|
|
||
|
|
minDepthDistance = (int)(minDistance * 1000f);
|
||
|
|
maxDepthDistance = (int)(maxDistance * 1000f);
|
||
|
|
|
||
|
|
if (depth2SpaceWidth != sensorData.depthImageWidth || depth2SpaceHeight != sensorData.depthImageHeight)
|
||
|
|
{
|
||
|
|
depth2SpaceTable = sensorData.sensorInterface.GetDepthCameraSpaceTable(sensorData);
|
||
|
|
depth2SpaceWidth = sensorData.depthImageWidth;
|
||
|
|
depth2SpaceHeight = sensorData.depthImageHeight;
|
||
|
|
|
||
|
|
pointCloudSpaceBuffer.SetData(depth2SpaceTable);
|
||
|
|
depth2SpaceTable = null;
|
||
|
|
//Debug.Log("Set space table for width: " + depth2SpaceWidth + ", height: " + depth2SpaceHeight);
|
||
|
|
}
|
||
|
|
|
||
|
|
// FloorDetectionOffsetEstShader
|
||
|
|
//lock(depthFrameLock)
|
||
|
|
{
|
||
|
|
KinectInterop.SetComputeBufferData(pointCloudDepthBuffer, depthFrame, depthFrame.Length >> 1, sizeof(uint));
|
||
|
|
}
|
||
|
|
|
||
|
|
isDepthFrameSet = true;
|
||
|
|
|
||
|
|
////Debug.Log("imuUpVector: " + imuUpVector);
|
||
|
|
//if(imuVectorTrans == null)
|
||
|
|
//{
|
||
|
|
// GameObject imuVectorObj = GameObject.CreatePrimitive(PrimitiveType.Cube);
|
||
|
|
// imuVectorObj.name = "ImuVectorObj";
|
||
|
|
|
||
|
|
// imuVectorTrans = imuVectorObj.transform;
|
||
|
|
// imuVectorTrans.localScale = new Vector3(0.1f, 0.2f, 0.5f);
|
||
|
|
// imuVectorTrans.position = new Vector3(0, 1f, 1f);
|
||
|
|
//}
|
||
|
|
|
||
|
|
//imuVectorTrans.rotation = Quaternion.LookRotation(imuUpVector.normalized);
|
||
|
|
|
||
|
|
if(bPlaneValid)
|
||
|
|
{
|
||
|
|
bPlaneValid = false;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// updates the floor parameters async
|
||
|
|
private IEnumerator UpdateFloorAsync()
|
||
|
|
{
|
||
|
|
while(isRoutineRunning)
|
||
|
|
{
|
||
|
|
// wait for imu vector & depth frame
|
||
|
|
while (/**!isImuVectorSet ||*/ !isDepthFrameSet)
|
||
|
|
{
|
||
|
|
yield return null;
|
||
|
|
}
|
||
|
|
|
||
|
|
//isImuVectorSet = false;
|
||
|
|
isDepthFrameSet = false;
|
||
|
|
|
||
|
|
KinectInterop.SetComputeShaderInt2(floorDetOffsetEstShader, "PointCloudRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
||
|
|
//KinectInterop.SetComputeShaderFloat2(floorDetOffsetEstShader, "SpaceScale", sensorData.sensorSpaceScale.x, sensorData.sensorSpaceScale.y);
|
||
|
|
KinectInterop.SetComputeShaderFloat3(floorDetOffsetEstShader, "ImuUpVector", imuUpVector);
|
||
|
|
|
||
|
|
floorDetOffsetEstShader.SetInt("MinDepth", minDepthDistance);
|
||
|
|
floorDetOffsetEstShader.SetInt("MaxDepth", maxDepthDistance);
|
||
|
|
|
||
|
|
floorDetOffsetEstShader.SetBuffer(floorDetOffsetEstKernel, "SpaceTable", pointCloudSpaceBuffer);
|
||
|
|
floorDetOffsetEstShader.SetBuffer(floorDetOffsetEstKernel, "DepthMap", pointCloudDepthBuffer);
|
||
|
|
|
||
|
|
floorDetOffsetEstShader.SetBuffer(floorDetOffsetEstKernel, "PointCloudPos", pointCloudPosBuffer);
|
||
|
|
floorDetOffsetEstShader.SetBuffer(floorDetOffsetEstKernel, "PointCloudOfs", pointCloudOfsBuffer);
|
||
|
|
floorDetOffsetEstShader.SetBuffer(floorDetOffsetEstKernel, "PointCloudMask", pointCloudMaskBuffer);
|
||
|
|
|
||
|
|
floorDetOffsetEstShader.Dispatch(floorDetOffsetEstKernel, sensorData.depthImageWidth / 8, sensorData.depthImageHeight / 8, 1);
|
||
|
|
|
||
|
|
// FloorDetectionOffsetMinMaxShader
|
||
|
|
KinectInterop.SetComputeShaderInt2(floorDetOffsetMinMaxShader, "PointCloudRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
||
|
|
floorDetOffsetMinMaxShader.SetInt("OfsHistBinLength", histBufferLength);
|
||
|
|
floorDetOffsetMinMaxShader.SetBuffer(floorDetOffsetMinMaxKernel, "PointCloudOfs", pointCloudOfsBuffer);
|
||
|
|
floorDetOffsetMinMaxShader.SetBuffer(floorDetOffsetMinMaxKernel, "PointCloudMask", pointCloudMaskBuffer);
|
||
|
|
|
||
|
|
floorDetOffsetMinMaxShader.SetBuffer(floorDetOffsetMinMaxKernel, "OfsMinMax", ofsHistMinMaxBuffer);
|
||
|
|
floorDetOffsetMinMaxShader.SetBuffer(floorDetOffsetMinMaxKernel, "OfsHistBinCount", ofsHistBinCountBuffer);
|
||
|
|
|
||
|
|
floorDetOffsetMinMaxShader.Dispatch(floorDetOffsetMinMaxKernel, 1, 1, 1);
|
||
|
|
|
||
|
|
//ofsHistMinMaxBuffer.GetData(histMinMax);
|
||
|
|
//Debug.Log("Hist min: " + histMinMax[0] + ", max: " + histMinMax[1]);
|
||
|
|
|
||
|
|
// FloorDetectionOffsetHistShader
|
||
|
|
KinectInterop.SetComputeShaderInt2(floorDetOffsetHistShader, "PointCloudRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
||
|
|
//floorDetOffsetHistShader.SetInt("PointCloudOfsLength", sensorData.depthImageWidth * sensorData.depthImageHeight);
|
||
|
|
floorDetOffsetHistShader.SetInt("OfsHistBinLength", histBufferLength);
|
||
|
|
floorDetOffsetHistShader.SetFloat("BinSize", histBinSize);
|
||
|
|
|
||
|
|
floorDetOffsetHistShader.SetBuffer(floorDetOffsetHistKernel, "PointCloudOfs", pointCloudOfsBuffer);
|
||
|
|
floorDetOffsetHistShader.SetBuffer(floorDetOffsetHistKernel, "PointCloudMask", pointCloudMaskBuffer);
|
||
|
|
floorDetOffsetHistShader.SetBuffer(floorDetOffsetHistKernel, "OfsMinMax", ofsHistMinMaxBuffer);
|
||
|
|
|
||
|
|
floorDetOffsetHistShader.SetBuffer(floorDetOffsetHistKernel, "OfsHistBinCount", ofsHistBinCountBuffer);
|
||
|
|
//floorDetOffsetHistShader.SetBuffer(floorDetOffsetHistKernel, "OfsHistBinLeft", ofsHistBinLeftBuffer);
|
||
|
|
|
||
|
|
floorDetOffsetHistShader.Dispatch(floorDetOffsetHistKernel, sensorData.depthImageWidth / 1, sensorData.depthImageHeight / 1, 1);
|
||
|
|
//floorDetOffsetHistShader.Dispatch(floorDetOffsetHistKernel, 1, 1, 1);
|
||
|
|
|
||
|
|
// FloorDetectionPlanePointsShader
|
||
|
|
floorDetPlaneEstShader.SetInt("OfsHistBinLength", histBufferLength);
|
||
|
|
floorDetPlaneEstShader.SetInt("PointCloudOfsLength", sensorData.depthImageWidth * sensorData.depthImageHeight);
|
||
|
|
floorDetPlaneEstShader.SetFloat("BinSize", histBinSize);
|
||
|
|
floorDetPlaneEstShader.SetInt("BinAggregation", binAggregation);
|
||
|
|
floorDetPlaneEstShader.SetInt("MinimumFloorPointCount", minFloorPointCount / 4);
|
||
|
|
|
||
|
|
floorDetPlaneEstShader.SetBuffer(floorDetPlaneEstKernel, "OfsHistBinCount", ofsHistBinCountBuffer);
|
||
|
|
floorDetPlaneEstShader.SetBuffer(floorDetPlaneEstKernel, "PointCloudPos", pointCloudPosBuffer);
|
||
|
|
floorDetPlaneEstShader.SetBuffer(floorDetPlaneEstKernel, "PointCloudOfs", pointCloudOfsBuffer);
|
||
|
|
floorDetPlaneEstShader.SetBuffer(floorDetPlaneEstKernel, "PointCloudMask", pointCloudMaskBuffer);
|
||
|
|
floorDetPlaneEstShader.SetBuffer(floorDetPlaneEstKernel, "OfsMinMax", ofsHistMinMaxBuffer);
|
||
|
|
|
||
|
|
floorDetPlaneEstShader.SetBuffer(floorDetPlaneEstKernel, "OfsHistBinLeft", ofsHistBinLeftBuffer);
|
||
|
|
floorDetPlaneEstShader.SetBuffer(floorDetPlaneEstKernel, "HistCumulativeCount", histCumulativeCountBuffer);
|
||
|
|
floorDetPlaneEstShader.SetBuffer(floorDetPlaneEstKernel, "InlierIndices", planeIndicesBuffer);
|
||
|
|
floorDetPlaneEstShader.SetBuffer(floorDetPlaneEstKernel, "PlanePosNorm", planePosNormBuffer);
|
||
|
|
|
||
|
|
floorDetPlaneEstShader.Dispatch(floorDetPlaneEstKernel, 1, 1, 1);
|
||
|
|
|
||
|
|
// wait some frames before GetData()
|
||
|
|
for (int i = 0; i < WAIT_FRAMES_BEFORE_GPUGET; i++)
|
||
|
|
{
|
||
|
|
yield return null;
|
||
|
|
}
|
||
|
|
|
||
|
|
//uint[] histCumCount = new uint[histBufferLength];
|
||
|
|
//histCumulativeCountBuffer.GetData(histCumCount);
|
||
|
|
//uint maxDiffCount = 0;
|
||
|
|
|
||
|
|
//System.Text.StringBuilder sbCumCount = new System.Text.StringBuilder();
|
||
|
|
//for(int i = 1; (i + binAggregation) < histCumCount.Length; i++) // i += binAggregation
|
||
|
|
//{
|
||
|
|
// uint diffCount = histCumCount[i + binAggregation - 1] - histCumCount[i - 1];
|
||
|
|
// if (maxDiffCount < diffCount)
|
||
|
|
// maxDiffCount = diffCount;
|
||
|
|
|
||
|
|
// if (diffCount > 0)
|
||
|
|
// sbCumCount.Append(i).Append('-').Append(diffCount).Append(" ");
|
||
|
|
//}
|
||
|
|
//Debug.Log("histCumCount(" + maxDiffCount + "): " + sbCumCount);
|
||
|
|
|
||
|
|
planePosNormBuffer.GetData(planePosNorm);
|
||
|
|
|
||
|
|
vPlanePos = new Vector3(planePosNorm[0], planePosNorm[1], planePosNorm[2]);
|
||
|
|
vPlaneNorm = new Vector3(planePosNorm[3], planePosNorm[4], planePosNorm[5]);
|
||
|
|
//Vector3 vPlaneOfs = new Vector3(planePosNorm[6], planePosNorm[7], planePosNorm[8]);
|
||
|
|
//Vector3 vPlaneOfs2 = new Vector3(planePosNorm[9], planePosNorm[10], planePosNorm[11]);
|
||
|
|
|
||
|
|
bPlaneValid = (vPlaneNorm != Vector3.zero);
|
||
|
|
|
||
|
|
if (bPlaneValid)
|
||
|
|
{
|
||
|
|
//Debug.Log("Plane pos: " + vPlanePos + ", norm: " + vPlaneNorm.normalized + ", rot: " + qSensorRot.eulerAngles + ", ofs: " + vPlaneOfs + ", ofs2: " + vPlaneOfs2);
|
||
|
|
|
||
|
|
vPlaneNorm = vPlaneNorm.normalized;
|
||
|
|
if (Vector3.Dot(vPlaneNorm, imuUpVector) < 0f)
|
||
|
|
{
|
||
|
|
vPlaneNorm = -vPlaneNorm;
|
||
|
|
//Debug.Log("Inverted plane normal: " + vPlaneNorm);
|
||
|
|
}
|
||
|
|
|
||
|
|
float floorTiltInDeg = Mathf.Acos(Vector3.Dot(vPlaneNorm, imuUpVector)) * Mathf.Rad2Deg;
|
||
|
|
if (floorTiltInDeg < planeMaxTiltInDeg)
|
||
|
|
{
|
||
|
|
// For reduced jitter, use gravity for floor normal.
|
||
|
|
vPlaneNorm = imuUpVector;
|
||
|
|
//Debug.Log("Used gravity for normal: " + vPlaneNorm + ", tiltAngle: " + floorTiltInDeg);
|
||
|
|
}
|
||
|
|
|
||
|
|
// get results
|
||
|
|
float fCurTimeSecs = Time.time;
|
||
|
|
bool bSmoothResult = (fCurTimeSecs - fLastTimeSecs) < SMOOTH_TIME_THRESHOLD;
|
||
|
|
//Debug.Log("SmoothResult: " + bSmoothResult);
|
||
|
|
fLastTimeSecs = fCurTimeSecs;
|
||
|
|
|
||
|
|
vPlanePos = new Vector3(vPlanePos.x * spaceScale.x, vPlanePos.y * spaceScale.y, vPlanePos.z * spaceScale.z);
|
||
|
|
vPlaneNorm = new Vector3(vPlaneNorm.x * spaceScale.x, vPlaneNorm.y * spaceScale.y, vPlaneNorm.z * spaceScale.z);
|
||
|
|
|
||
|
|
Quaternion curSensorRot = Quaternion.FromToRotation(vPlaneNorm, Vector3.up);
|
||
|
|
qSensorRot = bSmoothResult ? Quaternion.Slerp(qSensorRot, curSensorRot, smoothFactor * Time.deltaTime) : curSensorRot;
|
||
|
|
|
||
|
|
floorPlane = new Plane(vPlaneNorm, vPlanePos);
|
||
|
|
float curSensorHeight = floorPlane.GetDistanceToPoint(Vector3.zero);
|
||
|
|
fSensorHeight = bSmoothResult ? Mathf.Lerp(fSensorHeight, curSensorHeight, smoothFactor * Time.deltaTime) : curSensorHeight;
|
||
|
|
|
||
|
|
//Debug.Log("Floor pos: " + vPlanePos + ", norm: " + vPlaneNorm + ", rot: " + qSensorRot.eulerAngles + ", height: " + curSensorHeight + ", smoothed: " + fSensorHeight);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|