/// <summary>
/// Updates the mesh from a specified depth image, intrinsics, and position.
/// </summary>
/// <param name="depthImage">The depth image.</param>
/// <param name="intrinsics">The intrinsics.</param>
/// <param name="position">The position.</param>
public void UpdateMesh(DepthImage depthImage, ICameraIntrinsics intrinsics, CoordinateSystem position)
{
this.depthImage = depthImage;
this.intrinsics = intrinsics;
this.position = position;
this.UpdateVisuals();
}
/// <summary>
/// Converts a camera intrinsics to a tab-delimited text representation.
/// </summary>
/// <param name="cameraIntrinsics">The camera intrinsics.</param>
/// <returns>Tab-delimited text representation.</returns>
internal static string ToText(this ICameraIntrinsics cameraIntrinsics)
=> $"{cameraIntrinsics.Transform.ToText()}\t" +
$"{cameraIntrinsics.RadialDistortion.ToText()}\t" +
$"{cameraIntrinsics.TangentialDistortion.ToText()}\t" +
$"{cameraIntrinsics.FocalLength.ToText()}\t" +
$"{cameraIntrinsics.FocalLengthXY.ToText()}\t" +
$"{cameraIntrinsics.PrincipalPoint.ToText()}\t" +
$"{cameraIntrinsics.ClosedFormDistorts.ToText()}\t" +
$"{cameraIntrinsics.ImageWidth.ToText()}\t" +
$"{cameraIntrinsics.ImageHeight.ToText()}";
private void UpdateDepthFramePoints(DepthImage depthImage, ICameraIntrinsics intrinsics, euclidean.CoordinateSystem position)
{
// Handle null cases by clearing the depthFramePoints
if (depthImage == null || intrinsics == null || position == null)
{
if (this.rawDepth.Length > 0)
{
this.rawDepth = new int[0];
this.depthFramePoints = new System.Windows.Media.Media3D.Point3D[0];
}
return;
}
if (this.depthFramePoints?.Length != (this.depthImage.Width * this.depthImage.Height))
{
this.rawDepth = new int[this.depthImage.Width * this.depthImage.Height];
this.depthFramePoints = new System.Windows.Media.Media3D.Point3D[this.depthImage.Width * this.depthImage.Height];
}
int width = depthImage.Width;
int height = depthImage.Height;
int cx = width / 2;
int cy = height / 2;
double scale = 0.001;
unsafe
{
ushort *depthFrame = (ushort *)((byte *)depthImage.ImageData.ToPointer());
Parallel.For(0, height, iy =>
{
for (int ix = 0; ix < width; ix++)
{
int i = (iy * width) + ix;
this.rawDepth[i] = depthFrame[i];
if (this.rawDepth[i] == 0)
{
this.rawDepth[i] = -1;
this.depthFramePoints[i] = default;
}
else
{
var other = intrinsics.ToCameraSpace(new euclidean.Point2D(ix, iy), this.rawDepth[i] * scale, true);
this.depthFramePoints[i] = other.TransformBy(position).ToPoint3D();
}
}
});
}
}
/// <inheritdoc/>
public override void UpdateData()
{
if (this.depthImage != null)
{
this.depthImage.Dispose();
}
this.depthImage = this.CurrentData?.ViewedObject?.AddRef();
this.intrinsics = this.CurrentData?.CameraIntrinsics;
this.position = this.CurrentData?.CameraPose;
this.UpdateVisuals();
this.UpdateVisibility();
}
/// <inheritdoc/>
public override void UpdateData()
{
if (this.depthImage != null)
{
this.depthImage.Dispose();
}
this.depthImage = this.CurrentData?.ViewedObject?.AddRef();
if (!Equals(this.intrinsics, this.CurrentData?.CameraIntrinsics))
{
this.intrinsics = this.CurrentData?.CameraIntrinsics;
this.cameraSpaceMapping = this.intrinsics?.GetPixelToCameraSpaceMapping(this.depthImage.Resource.DepthValueSemantics, true);
}
this.position = this.CurrentData?.CameraPose;
this.UpdateVisuals();
this.UpdateVisibility();
}
/// <summary>
/// Creates an event handler that handles the FrameArrived event of the MediaFrameReader.
/// </summary>
/// <param name="streamSettings">The stream settings.</param>
/// <param name="encodedImageStream">The stream on which to post the output encoded image.</param>
/// <param name="intrinsicsStream">The stream on which to post the camera intrinsics.</param>
/// <param name="poseStream">The stream on which to post the camera pose.</param>
/// <param name="encodedImageCameraViewStream">The stream on which to post the encoded image camera view.</param>
/// <returns>The event handler.</returns>
private TypedEventHandler <MediaFrameReader, MediaFrameArrivedEventArgs> CreateMediaFrameHandler(
PhotoVideoCameraConfiguration.StreamSettings streamSettings,
Emitter <Shared <EncodedImage> > encodedImageStream,
Emitter <ICameraIntrinsics> intrinsicsStream,
Emitter <CoordinateSystem> poseStream,
Emitter <EncodedImageCameraView> encodedImageCameraViewStream)
{
// Cache the intrinsics
ICameraIntrinsics cameraIntrinsics = null;
return((sender, args) =>
{
using var frame = sender.TryAcquireLatestFrame();
if (frame != null)
{
// Convert frame QPC time to pipeline time
var frameTimestamp = frame.SystemRelativeTime.Value.Ticks;
var originatingTime = this.pipeline.GetCurrentTimeFromElapsedTicks(frameTimestamp);
// Compute the camera intrinsics if needed
if (cameraIntrinsics == null && streamSettings.OutputCameraIntrinsics)
{
cameraIntrinsics = this.GetCameraIntrinsics(frame);
}
// Post the intrinsics
if (streamSettings.OutputCameraIntrinsics)
{
intrinsicsStream.Post(cameraIntrinsics, originatingTime);
}
// Compute the camera pose if needed
var cameraPose = default(CoordinateSystem);
if (streamSettings.OutputPose || streamSettings.OutputEncodedImageCameraView)
{
// Convert the frame coordinate system to world pose in psi basis
cameraPose = frame.CoordinateSystem?.TryConvertSpatialCoordinateSystemToPsiCoordinateSystem();
}
// Post the pose
if (streamSettings.OutputPose)
{
poseStream.Post(cameraPose, originatingTime);
}
if (streamSettings.OutputEncodedImage || streamSettings.OutputEncodedImageCameraView)
{
// Accessing the VideoMediaFrame.SoftwareBitmap property creates a strong reference
// which needs to be Disposed, per the remarks here:
// https://docs.microsoft.com/en-us/uwp/api/windows.media.capture.frames.mediaframereference?view=winrt-19041#remarks
using var frameBitmap = frame.VideoMediaFrame.SoftwareBitmap;
using var sharedEncodedImage = EncodedImagePool.GetOrCreate(frameBitmap.PixelWidth, frameBitmap.PixelHeight, PixelFormat.BGRA_32bpp);
// Copy bitmap data into the shared encoded image
unsafe
{
using var input = frameBitmap.LockBuffer(BitmapBufferAccessMode.Read);
using var inputReference = input.CreateReference();
((UnsafeNative.IMemoryBufferByteAccess)inputReference).GetBuffer(out byte *imageData, out uint size);
// Copy NV12-encoded bytes directly (leaving room for 4-byte header)
sharedEncodedImage.Resource.CopyFrom((IntPtr)imageData, 4, (int)size);
// Add NV12 header to identify encoding
var buffer = sharedEncodedImage.Resource.GetBuffer();
buffer[0] = (byte)'N';
buffer[1] = (byte)'V';
buffer[2] = (byte)'1';
buffer[3] = (byte)'2';
}
// Post encoded image stream
if (streamSettings.OutputEncodedImage)
{
encodedImageStream.Post(sharedEncodedImage, originatingTime);
}
// Post the encoded image camera view stream if requested
if (streamSettings.OutputEncodedImageCameraView)
{
using var encodedImageCameraView = new EncodedImageCameraView(sharedEncodedImage, cameraIntrinsics, cameraPose);
encodedImageCameraViewStream.Post(encodedImageCameraView, originatingTime);
}
}
}
});
}
/// <inheritdoc/> public bool Equals(ICameraIntrinsics other) => other is CameraIntrinsics cameraIntrinsics &&
/// <summary>
/// Initializes a new instance of the <see cref="EncodedDepthImageCameraView"/> class.
/// </summary>
/// <param name="encodedDepthImage">The viewed encoded depth image.</param>
/// <param name="cameraIntrinsics">The camera intrinsics.</param>
/// <param name="cameraPose">The camera pose.</param>
public EncodedDepthImageCameraView(Shared <EncodedDepthImage> encodedDepthImage, ICameraIntrinsics cameraIntrinsics, CoordinateSystem cameraPose)
: base(encodedDepthImage, cameraIntrinsics, cameraPose)
{
}
/// <summary>
/// Initializes a new instance of the <see cref="DepthImageCameraView"/> class.
/// </summary>
/// <param name="depthImage">The depth image.</param>
/// <param name="cameraIntrinsics">The camera intrinsics.</param>
/// <param name="cameraPose">The camera pose.</param>
public DepthImageCameraView(Shared <DepthImage> depthImage, ICameraIntrinsics cameraIntrinsics, CoordinateSystem cameraPose)
: base(depthImage, cameraIntrinsics, cameraPose)
{
}
/// <summary>
/// Initializes a new instance of the <see cref="CameraView{T}"/> class.
/// </summary>
/// <param name="viewedObject">The viewed object.</param>
/// <param name="cameraIntrinsics">The camera intrinsics.</param>
/// <param name="cameraPose">The camera pose.</param>
public CameraView(T viewedObject, ICameraIntrinsics cameraIntrinsics, CoordinateSystem cameraPose)
{
this.ViewedObject = viewedObject;
this.CameraIntrinsics = cameraIntrinsics;
this.CameraPose = cameraPose;
}
/// <summary>
/// Initializes a new instance of the <see cref="ImageCameraView{T}"/> class.
/// </summary>
/// <param name="image">The image.</param>
/// <param name="cameraIntrinsics">The camera intrinsics.</param>
/// <param name="cameraPose">The camera pose.</param>
public ImageCameraView(Shared <TImage> image, ICameraIntrinsics cameraIntrinsics, CoordinateSystem cameraPose)
: base(image?.AddRef(), cameraIntrinsics, cameraPose)
{
}
/// <summary>
/// Initializes a new instance of the <see cref="PointCloud3DCameraView"/> class.
/// </summary>
/// <param name="pointCloud3D">The 3D point cloud.</param>
/// <param name="cameraIntrinsics">Intrinsics of the camera.</param>
/// <param name="cameraPose">Pose of the camera.</param>
public PointCloud3DCameraView(PointCloud3D pointCloud3D, ICameraIntrinsics cameraIntrinsics, CoordinateSystem cameraPose)
: base(pointCloud3D, cameraIntrinsics, cameraPose)
{
}
