Skip to content

Commit

Permalink
Merged PR 69077: December 2022 release
Browse files Browse the repository at this point in the history
  • Loading branch information
@chitsaw
chitsaw committed Dec 7, 2022
1 parent 419d35f commit 706ae03
Show file tree
Hide file tree
Showing 317 changed files with 12,161 additions and 6,876 deletions.
2 changes: 1 addition & 1 deletion Directory.Build.props
View file
Expand Up @@ -6,7 +6,7 @@
<Company>Microsoft Corporation</Company>
<Owners>microsoft,psi</Owners>
<Authors>Microsoft</Authors>
<AssemblyVersion>0.17.52.1</AssemblyVersion>
<AssemblyVersion>0.18.72.1</AssemblyVersion>
<FileVersion>$(AssemblyVersion)</FileVersion>
<Version>$(AssemblyVersion)-beta</Version>
<SignAssembly>false</SignAssembly>
Expand Down
7 changes: 0 additions & 7 deletions Psi.sln
View file
Expand Up @@ -152,8 +152,6 @@ Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Build", "Build", "{4026C2BE
Build\Test.Psi.ruleset = Build\Test.Psi.ruleset
EndProjectSection
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Microsoft.Psi.Media.Native.x64", "Sources\Media\Microsoft.Psi.Media.Native.x64\Microsoft.Psi.Media.Native.x64.vcxproj", "{C50F7F21-BEB0-4366-B73F-859EEBC3ED42}"
EndProject
Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "RealSense", "RealSense", "{64BBFFEA-7CFB-49F9-AC74-3AD1D13245FC}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Microsoft.Psi.RealSense.Windows.x64", "Sources\RealSense\Microsoft.Psi.RealSense.Windows.x64\Microsoft.Psi.RealSense.Windows.x64.csproj", "{7B73D864-9997-4637-8765-44C17FD09CE1}"
Expand Down Expand Up @@ -393,10 +391,6 @@ Global
{4478A162-4FE9-4737-A630-3899DC5935C6}.Debug|Any CPU.Build.0 = Debug|Any CPU
{4478A162-4FE9-4737-A630-3899DC5935C6}.Release|Any CPU.ActiveCfg = Release|Any CPU
{4478A162-4FE9-4737-A630-3899DC5935C6}.Release|Any CPU.Build.0 = Release|Any CPU
{C50F7F21-BEB0-4366-B73F-859EEBC3ED42}.Debug|Any CPU.ActiveCfg = Debug|x64
{C50F7F21-BEB0-4366-B73F-859EEBC3ED42}.Debug|Any CPU.Build.0 = Debug|x64
{C50F7F21-BEB0-4366-B73F-859EEBC3ED42}.Release|Any CPU.ActiveCfg = Release|x64
{C50F7F21-BEB0-4366-B73F-859EEBC3ED42}.Release|Any CPU.Build.0 = Release|x64
{7B73D864-9997-4637-8765-44C17FD09CE1}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{7B73D864-9997-4637-8765-44C17FD09CE1}.Debug|Any CPU.Build.0 = Debug|Any CPU
{7B73D864-9997-4637-8765-44C17FD09CE1}.Release|Any CPU.ActiveCfg = Release|Any CPU
Expand Down Expand Up @@ -582,7 +576,6 @@ Global
{B9F00634-88A1-40EF-9DAD-814A307AD81F} = {C6976E06-9D12-4398-8096-C23D04E63F61}
{BE194924-7162-405D-BF6E-E6086BAA12F1} = {3F77CC04-2E58-452B-8107-0C93E7944D4E}
{4478A162-4FE9-4737-A630-3899DC5935C6} = {CB8286F5-167B-4416-8FE9-9B97FCF146D5}
{C50F7F21-BEB0-4366-B73F-859EEBC3ED42} = {AD8FE445-240A-4791-8C64-A5E2D6E67FF9}
{64BBFFEA-7CFB-49F9-AC74-3AD1D13245FC} = {A0856299-D28A-4513-B964-3FA5290FF160}
{7B73D864-9997-4637-8765-44C17FD09CE1} = {64BBFFEA-7CFB-49F9-AC74-3AD1D13245FC}
{DAB8847B-DE0A-45E2-A7DA-30432A36525B} = {64BBFFEA-7CFB-49F9-AC74-3AD1D13245FC}
Expand Down
116 changes: 59 additions & 57 deletions Sources/Calibration/Microsoft.Psi.Calibration/CalibrationExtensions.cs
View file
Expand Up @@ -7,6 +7,7 @@ namespace Microsoft.Psi.Calibration
using System.Collections.Generic;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Spatial.Euclidean;
using MathNet.Spatial.Units;
using Microsoft.Psi;
using Microsoft.Psi.Imaging;

Expand Down Expand Up @@ -196,8 +197,8 @@ public static ICameraIntrinsics CreateCameraIntrinsics(this ImageBase image, dou
double z = pointInCameraSpace.X * colorExtrinsicsInverse[2, 0] + pointInCameraSpace.Y * colorExtrinsicsInverse[2, 1] + pointInCameraSpace.Z * colorExtrinsicsInverse[2, 2] + colorExtrinsicsInverse[2, 3];
var pointInDepthCameraSpace = new Point3D(x, y, z);
var colorCameraOriginInDepthCameraSpace = new Point3D(colorExtrinsicsInverse[0, 3], colorExtrinsicsInverse[1, 3], colorExtrinsicsInverse[2, 3]);
var searchLine = new Line3D(colorCameraOriginInDepthCameraSpace, pointInDepthCameraSpace);
return IntersectLineWithDepthMesh(depthDeviceCalibrationInfo.DepthIntrinsics, searchLine, depthImage.Resource);
var searchRay = new Ray3D(colorCameraOriginInDepthCameraSpace, pointInDepthCameraSpace - colorCameraOriginInDepthCameraSpace);
return depthImage.Resource.ComputeRayIntersection(depthDeviceCalibrationInfo.DepthIntrinsics, searchRay);
}

/// <summary>
Expand All @@ -213,41 +214,6 @@ public static ICameraIntrinsics CreateCameraIntrinsics(this ImageBase image, dou
string name = nameof(ProjectTo3D))
=> source.PipeTo(new ProjectTo3D(source.Out.Pipeline, name), deliveryPolicy);

/// <summary>
/// Performs a ray/mesh intersection with the depth map.
/// </summary>
/// <param name="depthIntrinsics">The intrinsics for the depth camera.</param>
/// <param name="line">Ray to intersect against depth map.</param>
/// <param name="depthImage">Depth map to ray cast against.</param>
/// <param name="maxDistance">The maximum distance to search for.</param>
/// <param name="skipFactor">Distance to march on each step along ray.</param>
/// <param name="undistort">Whether undistortion should be applied to the point.</param>
/// <returns>Returns point of intersection.</returns>
public static Point3D? IntersectLineWithDepthMesh(ICameraIntrinsics depthIntrinsics, Line3D line, DepthImage depthImage, double maxDistance = 5, double skipFactor = 0.05, bool undistort = true)
{
// max distance to check for intersection with the scene
var delta = skipFactor * (line.EndPoint - line.StartPoint).Normalize();

// size of increment along the ray
int maxSteps = (int)(maxDistance / delta.Length);
var hypothesisPoint = line.StartPoint;
for (int i = 0; i < maxSteps; i++)
{
hypothesisPoint += delta;

// get the mesh distance at the extended point
float meshDistance = GetMeshDepthAtPoint(depthIntrinsics, depthImage, hypothesisPoint, undistort);

// if the mesh distance is less than the distance to the point we've hit the mesh
if (!float.IsNaN(meshDistance) && (meshDistance < hypothesisPoint.X))
{
return hypothesisPoint;
}
}

return null;
}

/// <summary>
/// Use the Rodrigues formula for transforming a given rotation from axis-angle representation to a 3x3 matrix.
/// Where 'r' is a rotation vector:
Expand Down Expand Up @@ -299,13 +265,15 @@ public static Matrix<double> AxisAngleToMatrix(Vector<double> vectorRotation)
}

/// <summary>
/// Convert a rotation matrix to axis-angle representation (a unit vector scaled by the angular distance to rotate).
/// Convert a rotation matrix to axis-angle representation (a unit vector scaled by the angular distance in radians to rotate).
/// </summary>
/// <param name="m">Input rotation matrix.</param>
/// <returns>Same rotation in axis-angle representation (L2-Norm of the vector represents angular distance).</returns>
/// <param name="epsilon">An optional angle epsilon parameter used to determine when the specified matrix contains a zero-rotation (by default 0.01 degrees).</param>
public static Vector<double> MatrixToAxisAngle(Matrix<double> m, double epsilon = 0.01 * Math.PI / 180)
/// <returns>Same rotation in axis-angle representation (L2-Norm of the vector represents angular distance in radians).</returns>
public static Vector<double> MatrixToAxisAngle(Matrix<double> m, Angle? epsilon = null)
{
epsilon ??= Angle.FromDegrees(0.01);

if (m.RowCount != 3 || m.ColumnCount != 3)
{
throw new InvalidOperationException("The input must be a valid 3x3 rotation matrix in order to compute its axis-angle representation.");
Expand All @@ -317,7 +285,7 @@ public static Vector<double> MatrixToAxisAngle(Matrix<double> m, double epsilon
// Create the axis vector.
var v = Vector<double>.Build.Dense(3, 0);

if (double.IsNaN(angle) || angle < epsilon)
if (double.IsNaN(angle) || angle < epsilon.Value.Radians)
{
// If the angular distance to rotate is 0, we just return a vector of all zeroes.
return v;
Expand Down Expand Up @@ -427,28 +395,62 @@ public static void Project(Matrix<double> cameraMatrix, Vector<double> distCoeff
projectedPoint = new Point2D(fx * xpp + cx, fy * ypp + cy);
}

private static float GetMeshDepthAtPoint(ICameraIntrinsics depthIntrinsics, DepthImage depthImage, Point3D point, bool undistort)
/// <summary>
/// Computes a ray intersection with a depth image mesh.
/// </summary>
/// <param name="depthImage">Depth image mesh to ray cast against.</param>
/// <param name="depthIntrinsics">The intrinsics for the depth camera.</param>
/// <param name="ray">Ray to intersect against depth image mesh.</param>
/// <param name="maxDistance">The maximum distance to search for (default is 5 meters).</param>
/// <param name="skipFactor">Distance to march on each step along ray (default is 5 cm).</param>
/// <param name="undistort">Whether undistortion should be applied to the point.</param>
/// <returns>Returns point of intersection, or null if no intersection was found.</returns>
/// <remarks>
/// The ray is assumed to be defined relative to the pose of the depth camera,
/// i.e., (0, 0, 0) is the position of the camera itself.
/// </remarks>
public static Point3D? ComputeRayIntersection(this DepthImage depthImage, ICameraIntrinsics depthIntrinsics, Ray3D ray, double maxDistance = 5, double skipFactor = 0.05, bool undistort = true)
{
if (!depthIntrinsics.TryGetPixelPosition(point, undistort, out var depthPixel))
{
return float.NaN;
}
// max distance to check for intersection with the scene
int maxSteps = (int)(maxDistance / skipFactor);

int x = (int)Math.Round(depthPixel.X);
int y = (int)Math.Round(depthPixel.Y);
if ((x < 0) || (x >= depthImage.Width) || (y < 0) || (y >= depthImage.Height))
{
return float.NaN;
}
// size of increment along the ray
var delta = skipFactor * ray.Direction;

int byteOffset = (int)((y * depthImage.Stride) + (x * 2));
var depth = BitConverter.ToUInt16(depthImage.ReadBytes(2, byteOffset), 0);
if (depth == 0)
var hypothesisPoint = ray.ThroughPoint;
for (int i = 0; i < maxSteps; i++)
{
return float.NaN;
hypothesisPoint += delta;

// get the mesh distance at the hypothesis point
if (depthIntrinsics.TryGetPixelPosition(hypothesisPoint, undistort, out var depthPixel) &&
depthImage.TryGetPixel((int)Math.Floor(depthPixel.X), (int)Math.Floor(depthPixel.Y), out var depthValue) &&
depthValue != 0)
{
// if the mesh distance is less than the distance to the point we've hit the mesh
var meshDistanceMeters = (double)depthValue * depthImage.DepthValueToMetersScaleFactor;
if (depthImage.DepthValueSemantics == DepthValueSemantics.DistanceToPlane)
{
if (meshDistanceMeters < hypothesisPoint.X)
{
return hypothesisPoint;
}
}
else if (depthImage.DepthValueSemantics == DepthValueSemantics.DistanceToPoint)
{
if (meshDistanceMeters < hypothesisPoint.ToVector3D().Length)
{
return hypothesisPoint;
}
}
else
{
throw new ArgumentException($"Unhandled {nameof(DepthValueSemantics)}: {depthImage.DepthValueSemantics}");
}
}
}

return (float)depth / 1000;
return null;
}

private static double CalibrateCamera(
Expand Down
7 changes: 7 additions & 0 deletions Sources/Calibration/Microsoft.Psi.Calibration/CameraIntrinsics.cs
View file
Expand Up @@ -141,6 +141,13 @@ private set
public Point2D? GetPixelPosition(Point3D point3D, bool distort, bool nullIfOutsideFieldOfView = true)
{
// X points in the depth dimension. Y points to the left, and Z points up.

// If the point is not in front of the camera, we cannot compute the projection
if (point3D.X <= 0)
{
return null;
}

var point2D = new Point2D(-point3D.Y / point3D.X, -point3D.Z / point3D.X);
if (distort)
{
Expand Down
4 changes: 4 additions & 0 deletions Sources/Calibration/Microsoft.Psi.Calibration/ICameraIntrinsics.cs
View file
Expand Up @@ -77,6 +77,8 @@ public interface ICameraIntrinsics : IEquatable<ICameraIntrinsics>
/// <param name="distort">Indicates whether to apply distortion.</param>
/// <param name="nullIfOutsideFieldOfView">Optional flag indicating whether to return null if point is outside the field of view (default true).</param>
/// <returns>Point containing the pixel position.</returns>
/// <remarks>Points that are behind the camera, i.e., with the X value below zero lead to null returns,
/// regardless of value of the <paramref name="nullIfOutsideFieldOfView"/> parameter.</remarks>
Point2D? GetPixelPosition(Point3D point3D, bool distort, bool nullIfOutsideFieldOfView = true);

/// <summary>
Expand All @@ -87,6 +89,8 @@ public interface ICameraIntrinsics : IEquatable<ICameraIntrinsics>
/// <param name="pixelPosition">Output point containing the pixel position.</param>
/// <param name="nullIfOutsideFieldOfView">Optional flag indicating whether to return null if point is outside the field of view (default true).</param>
/// <returns>True if <paramref name="pixelPosition"/> is within field of view, otherwise false.</returns>
/// <remarks>Points that are behind the camera, i.e., with the X value below zero lead to a return value of false,
/// regardless of value of the <paramref name="nullIfOutsideFieldOfView"/> parameter.</remarks>
bool TryGetPixelPosition(Point3D point3D, bool distort, out Point2D pixelPosition, bool nullIfOutsideFieldOfView = true);

/// <summary>
Expand Down

0 comments on commit 706ae03

Please sign in to comment.