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; } }