private void CalculatePointPositions()
{
// print("CalculatePointPositions");
var px_to_deg = new Vector2(
_hFOV / _depthMapWidth,
_vFOV / _depthMapHeight
);
int skipped_points = 0;
for (int y = 0; y < _depthMapHeight; y++)
{
for (int x = 0; x < _depthMapWidth; x++)
{
int index = (y * _depthMapWidth) + x;
if (skipped_points < _skipPoints)
{
skipped_points++;
_pointPositions[index] = Vector3.zero;
continue;
}
else
{
skipped_points = 0;
}
// var pos = _pointPositions[index];
var distanceZ = _rawDepthMap[index] / 1000f;
var x_from_center = x - _depthMapWidth / 2;
var angleX = x_from_center * px_to_deg.x;
var y_from_center = y - _depthMapHeight / 2;
y_from_center = -y_from_center;
var angleY = y_from_center * px_to_deg.y;
_pointPositions[index] = _lastPosition + _lastRotation * new Vector3(
distanceZ * Mathf.Sin(angleX),
distanceZ * Mathf.Sin(angleY),
distanceZ
);
// print(_pointPositions[index]);
}
}
_pointCloudData.Initialize(_pointPositions);
}
void InitDepthVideoStreamForCurrentDevice()
{
if (_depthVideoStream != null)
{
_depthVideoStream.Stop();
}
_depthVideoStream = _currentDevice.CreateVideoStream(Device.SensorType.Depth);
_hFOV = _depthVideoStream.HorizontalFieldOfView;
_vFOV = _depthVideoStream.VerticalFieldOfView;
_depthMapWidth = _depthVideoStream.VideoMode.Resolution.Width;
_depthMapHeight = _depthVideoStream.VideoMode.Resolution.Height;
Debug.Log($"H_FOV: {_hFOV/Math.PI*180}");
Debug.Log($"V_FOV: {_vFOV/Math.PI*180}");
Debug.Log($"depth resolution: {_depthMapWidth}x{_depthMapHeight}");
// init raw depth data
_rawDepthMap = new short[(int)(_depthMapWidth * _depthMapHeight)];
// Mesh rendering
_depthMeshRenderer.Init(
_depthMapWidth,
_depthMapHeight,
_depthVideoStream.HorizontalFieldOfView,
_depthVideoStream.VerticalFieldOfView
);
// PointCloud
_pointPositions = new List <Vector3>();
for (int i = 0; i < _depthMapWidth * _depthMapHeight; i++)
{
_pointPositions.Add(new Vector3());
}
_pointCloudData = ScriptableObject.CreateInstance <PointCloudData>();
_pointCloudData.Initialize(_pointPositions);
_pointCloudRenderer.sourceData = _pointCloudData;
// Texture
_textureRenderer.Init(_depthMapWidth, _depthMapHeight, _depthVideoStream.MaxPixelValue);
}