private void ReprojectPointCloud()
{
if (_depthTextureFloat == null)
{
return;
}
var widthDepth = _depthTextureFloat.width;
var heightDepth = _depthTextureFloat.height;
var widthCamera = _cameraTexture.width;
if (_cloud == null || _cloud.Length != widthDepth * heightDepth)
{
_cloud = new LidarPoint[widthDepth * heightDepth];
_cameraManager.TryGetIntrinsics(out var intrinsic);
var ratio = (float)widthDepth / (float)widthCamera;
_fx = intrinsic.focalLength.x * ratio;
_fy = intrinsic.focalLength.y * ratio;
_cx = intrinsic.principalPoint.x * ratio;
_cy = intrinsic.principalPoint.y * ratio;
}
var depthPixels = _depthTextureFloat.GetPixels();
var confidenceMap = _confidenceTexR8.GetPixels32();
for (var iY = 0; iY < heightDepth; iY++)
{
for (var iX = 0; iX < widthDepth; iX++)
{
var iPoint = iY * widthDepth + iX;
_cloud[iPoint].Color = _cameraTexture.GetPixelBilinear((float)iX / (widthDepth),
(float)iY / (heightDepth));
var depth = depthPixels[iPoint].r;
_cloud[iPoint].IsValid = depth > Near && depth < Far && confidenceMap[iPoint].r == 2;
if (!_cloud[iPoint].IsValid)
{
continue;
}
_cloud[iPoint].PositionCameraSpace.x = -depth * (iX - _cx) / _fx;
_cloud[iPoint].PositionCameraSpace.y = -depth * (iY - _cy) / _fy;
_cloud[iPoint].PositionCameraSpace.z = depth;
var worldPosition = _cameraManager.transform.TransformPoint(_cloud[iPoint].PositionCameraSpace);
_cloud[iPoint].WorldPosition = worldPosition;
_cloud[iPoint].SnappedPositionWorld.x = Mathf.RoundToInt(100 * worldPosition.x);
_cloud[iPoint].SnappedPositionWorld.y = Mathf.RoundToInt(100 * worldPosition.y);
_cloud[iPoint].SnappedPositionWorld.z = Mathf.RoundToInt(100 * worldPosition.z);
}
}
CloudUpdateEvent?.Invoke(_cloud);
}
private unsafe void UpdateCameraImage()
{
if (!_cameraManager.TryAcquireLatestCpuImage(out var image))
{
CloudUpdateEvent?.Invoke(GenerateDebugCloud());
return;
}
using (image)
{
if (_cameraTexture == null || !HasEqualDimensions(_cameraTexture, image))
{
_cameraTexture = new Texture2D(image.width, image.height, TextureFormat.RGBA32, false);
}
var conversionParams =
new XRCpuImage.ConversionParams(image, TextureFormat.RGBA32, XRCpuImage.Transformation.MirrorY);
var textureRaw = _cameraTexture.GetRawTextureData <byte>();
image.Convert(conversionParams, new IntPtr(textureRaw.GetUnsafePtr()), textureRaw.Length);
_cameraTexture.Apply();
_debugViews.Camera.texture = _cameraTexture;
}
}
