public static void depthTo3D ( CameraPinholeBrown param , GrayU16 depth , FastQueue < Point3D_F64 > cloud ) { cloud . reset ( ) ; Point2Transform2_F64 p2n = LensDistortionFactory . narrow ( param ) . undistort_F64 ( true , false ) ; Point2D_F64 n = new Point2D_F64 ( ) ; for ( int y = 0 ; y < depth . height ; y ++ ) { int index = depth . startIndex + y * depth . stride ; for ( int x = 0 ; x < depth . width ; x ++ ) { int mm = depth . data [ index ++ ] & 0xFFFF ; // skip pixels with no depth information if ( mm == 0 ) continue ; // this could all be precomputed to speed it up p2n . compute ( x , y , n ) ; Point3D_F64 p = cloud . grow ( ) ; p . z = mm ; p . x = n . x * p . z ; p . y = n . y * p . z ; } } }
Creates a point cloud from a depth image .