public static void depthTo3D ( CameraPinholeBrown param , Planar < GrayU8 > rgb , GrayU16 depth , FastQueue < Point3D_F64 > cloud , FastQueueArray_I32 cloudColor ) { cloud . reset ( ) ; cloudColor . reset ( ) ; RemoveBrownPtoN_F64 p2n = new RemoveBrownPtoN_F64 ( ) ; p2n . setK ( param . fx , param . fy , param . skew , param . cx , param . cy ) . setDistortion ( param . radial , param . t1 , param . t2 ) ; Point2D_F64 n = new Point2D_F64 ( ) ; GrayU8 colorR = rgb . getBand ( 0 ) ; GrayU8 colorG = rgb . getBand ( 1 ) ; GrayU8 colorB = rgb . getBand ( 2 ) ; 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 ; int color [ ] = cloudColor . grow ( ) ; color [ 0 ] = colorR . unsafe_get ( x , y ) ; color [ 1 ] = colorG . unsafe_get ( x , y ) ; color [ 2 ] = colorB . unsafe_get ( x , y ) ; } } }
Creates a point cloud from a depth image and saves the color information . The depth and color images are assumed to be aligned .