@@ -7,6 +7,7 @@ namespace Microsoft.Psi.Calibration
77 using System . Collections . Generic ;
88 using MathNet . Numerics . LinearAlgebra ;
99 using MathNet . Spatial . Euclidean ;
10+ using MathNet . Spatial . Units ;
1011 using Microsoft . Psi ;
1112 using Microsoft . Psi . Imaging ;
1213
@@ -196,8 +197,8 @@ public static ICameraIntrinsics CreateCameraIntrinsics(this ImageBase image, dou
196197 double z = pointInCameraSpace . X * colorExtrinsicsInverse [ 2 , 0 ] + pointInCameraSpace . Y * colorExtrinsicsInverse [ 2 , 1 ] + pointInCameraSpace . Z * colorExtrinsicsInverse [ 2 , 2 ] + colorExtrinsicsInverse [ 2 , 3 ] ;
197198 var pointInDepthCameraSpace = new Point3D ( x , y , z ) ;
198199 var colorCameraOriginInDepthCameraSpace = new Point3D ( colorExtrinsicsInverse [ 0 , 3 ] , colorExtrinsicsInverse [ 1 , 3 ] , colorExtrinsicsInverse [ 2 , 3 ] ) ;
199- var searchLine = new Line3D ( colorCameraOriginInDepthCameraSpace , pointInDepthCameraSpace ) ;
200- return IntersectLineWithDepthMesh ( depthDeviceCalibrationInfo . DepthIntrinsics , searchLine , depthImage . Resource ) ;
200+ var searchRay = new Ray3D ( colorCameraOriginInDepthCameraSpace , pointInDepthCameraSpace - colorCameraOriginInDepthCameraSpace ) ;
201+ return depthImage . Resource . ComputeRayIntersection ( depthDeviceCalibrationInfo . DepthIntrinsics , searchRay ) ;
201202 }
202203
203204 /// <summary>
@@ -213,41 +214,6 @@ public static IProducer<List<Point3D>> ProjectTo3D(
213214 string name = nameof ( ProjectTo3D ) )
214215 => source . PipeTo ( new ProjectTo3D ( source . Out . Pipeline , name ) , deliveryPolicy ) ;
215216
216- /// <summary>
217- /// Performs a ray/mesh intersection with the depth map.
218- /// </summary>
219- /// <param name="depthIntrinsics">The intrinsics for the depth camera.</param>
220- /// <param name="line">Ray to intersect against depth map.</param>
221- /// <param name="depthImage">Depth map to ray cast against.</param>
222- /// <param name="maxDistance">The maximum distance to search for.</param>
223- /// <param name="skipFactor">Distance to march on each step along ray.</param>
224- /// <param name="undistort">Whether undistortion should be applied to the point.</param>
225- /// <returns>Returns point of intersection.</returns>
226- public static Point3D ? IntersectLineWithDepthMesh ( ICameraIntrinsics depthIntrinsics , Line3D line , DepthImage depthImage , double maxDistance = 5 , double skipFactor = 0.05 , bool undistort = true )
227- {
228- // max distance to check for intersection with the scene
229- var delta = skipFactor * ( line . EndPoint - line . StartPoint ) . Normalize ( ) ;
230-
231- // size of increment along the ray
232- int maxSteps = ( int ) ( maxDistance / delta . Length ) ;
233- var hypothesisPoint = line . StartPoint ;
234- for ( int i = 0 ; i < maxSteps ; i ++ )
235- {
236- hypothesisPoint += delta ;
237-
238- // get the mesh distance at the extended point
239- float meshDistance = GetMeshDepthAtPoint ( depthIntrinsics , depthImage , hypothesisPoint , undistort ) ;
240-
241- // if the mesh distance is less than the distance to the point we've hit the mesh
242- if ( ! float . IsNaN ( meshDistance ) && ( meshDistance < hypothesisPoint . X ) )
243- {
244- return hypothesisPoint ;
245- }
246- }
247-
248- return null ;
249- }
250-
251217 /// <summary>
252218 /// Use the Rodrigues formula for transforming a given rotation from axis-angle representation to a 3x3 matrix.
253219 /// Where 'r' is a rotation vector:
@@ -299,13 +265,15 @@ public static Matrix<double> AxisAngleToMatrix(Vector<double> vectorRotation)
299265 }
300266
301267 /// <summary>
302- /// Convert a rotation matrix to axis-angle representation (a unit vector scaled by the angular distance to rotate).
268+ /// Convert a rotation matrix to axis-angle representation (a unit vector scaled by the angular distance in radians to rotate).
303269 /// </summary>
304270 /// <param name="m">Input rotation matrix.</param>
305- /// <returns>Same rotation in axis-angle representation (L2-Norm of the vector represents angular distance).</returns>
306271 /// <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>
307- public static Vector < double > MatrixToAxisAngle ( Matrix < double > m , double epsilon = 0.01 * Math . PI / 180 )
272+ /// <returns>Same rotation in axis-angle representation (L2-Norm of the vector represents angular distance in radians).</returns>
273+ public static Vector < double > MatrixToAxisAngle ( Matrix < double > m , Angle ? epsilon = null )
308274 {
275+ epsilon ??= Angle . FromDegrees ( 0.01 ) ;
276+
309277 if ( m . RowCount != 3 || m . ColumnCount != 3 )
310278 {
311279 throw new InvalidOperationException ( "The input must be a valid 3x3 rotation matrix in order to compute its axis-angle representation." ) ;
@@ -317,7 +285,7 @@ public static Vector<double> MatrixToAxisAngle(Matrix<double> m, double epsilon
317285 // Create the axis vector.
318286 var v = Vector < double > . Build . Dense ( 3 , 0 ) ;
319287
320- if ( double . IsNaN ( angle ) || angle < epsilon )
288+ if ( double . IsNaN ( angle ) || angle < epsilon . Value . Radians )
321289 {
322290 // If the angular distance to rotate is 0, we just return a vector of all zeroes.
323291 return v ;
@@ -427,28 +395,62 @@ public static void Project(Matrix<double> cameraMatrix, Vector<double> distCoeff
427395 projectedPoint = new Point2D ( fx * xpp + cx , fy * ypp + cy ) ;
428396 }
429397
430- private static float GetMeshDepthAtPoint ( ICameraIntrinsics depthIntrinsics , DepthImage depthImage , Point3D point , bool undistort )
398+ /// <summary>
399+ /// Computes a ray intersection with a depth image mesh.
400+ /// </summary>
401+ /// <param name="depthImage">Depth image mesh to ray cast against.</param>
402+ /// <param name="depthIntrinsics">The intrinsics for the depth camera.</param>
403+ /// <param name="ray">Ray to intersect against depth image mesh.</param>
404+ /// <param name="maxDistance">The maximum distance to search for (default is 5 meters).</param>
405+ /// <param name="skipFactor">Distance to march on each step along ray (default is 5 cm).</param>
406+ /// <param name="undistort">Whether undistortion should be applied to the point.</param>
407+ /// <returns>Returns point of intersection, or null if no intersection was found.</returns>
408+ /// <remarks>
409+ /// The ray is assumed to be defined relative to the pose of the depth camera,
410+ /// i.e., (0, 0, 0) is the position of the camera itself.
411+ /// </remarks>
412+ public static Point3D ? ComputeRayIntersection ( this DepthImage depthImage , ICameraIntrinsics depthIntrinsics , Ray3D ray , double maxDistance = 5 , double skipFactor = 0.05 , bool undistort = true )
431413 {
432- if ( ! depthIntrinsics . TryGetPixelPosition ( point , undistort , out var depthPixel ) )
433- {
434- return float . NaN ;
435- }
414+ // max distance to check for intersection with the scene
415+ int maxSteps = ( int ) ( maxDistance / skipFactor ) ;
436416
437- int x = ( int ) Math . Round ( depthPixel . X ) ;
438- int y = ( int ) Math . Round ( depthPixel . Y ) ;
439- if ( ( x < 0 ) || ( x >= depthImage . Width ) || ( y < 0 ) || ( y >= depthImage . Height ) )
440- {
441- return float . NaN ;
442- }
417+ // size of increment along the ray
418+ var delta = skipFactor * ray . Direction ;
443419
444- int byteOffset = ( int ) ( ( y * depthImage . Stride ) + ( x * 2 ) ) ;
445- var depth = BitConverter . ToUInt16 ( depthImage . ReadBytes ( 2 , byteOffset ) , 0 ) ;
446- if ( depth == 0 )
420+ var hypothesisPoint = ray . ThroughPoint ;
421+ for ( int i = 0 ; i < maxSteps ; i ++ )
447422 {
448- return float . NaN ;
423+ hypothesisPoint += delta ;
424+
425+ // get the mesh distance at the hypothesis point
426+ if ( depthIntrinsics . TryGetPixelPosition ( hypothesisPoint , undistort , out var depthPixel ) &&
427+ depthImage . TryGetPixel ( ( int ) Math . Floor ( depthPixel . X ) , ( int ) Math . Floor ( depthPixel . Y ) , out var depthValue ) &&
428+ depthValue != 0 )
429+ {
430+ // if the mesh distance is less than the distance to the point we've hit the mesh
431+ var meshDistanceMeters = ( double ) depthValue * depthImage . DepthValueToMetersScaleFactor ;
432+ if ( depthImage . DepthValueSemantics == DepthValueSemantics . DistanceToPlane )
433+ {
434+ if ( meshDistanceMeters < hypothesisPoint . X )
435+ {
436+ return hypothesisPoint ;
437+ }
438+ }
439+ else if ( depthImage . DepthValueSemantics == DepthValueSemantics . DistanceToPoint )
440+ {
441+ if ( meshDistanceMeters < hypothesisPoint . ToVector3D ( ) . Length )
442+ {
443+ return hypothesisPoint ;
444+ }
445+ }
446+ else
447+ {
448+ throw new ArgumentException ( $ "Unhandled { nameof ( DepthValueSemantics ) } : { depthImage . DepthValueSemantics } ") ;
449+ }
450+ }
449451 }
450452
451- return ( float ) depth / 1000 ;
453+ return null ;
452454 }
453455
454456 private static double CalibrateCamera (
0 commit comments