void OpenNICamera::copyImage()
{
_imageGen.GetMetaData( _rgbData );
size_t w = _rgbData.XRes();
size_t h = _rgbData.YRes();
if( w != _rgb.width() ||
h != _rgb.height() ){
_depth.reallocate( w, h, IFormat::GRAY_UINT16 );
}
size_t stride;
uint8_t* ptr = _rgb.map( &stride );
const uint8_t* iptr = _rgbData.Data();
size_t bytesPerLine = w * _rgbData.BytesPerPixel();
SIMD* simd = SIMD::instance();
if( bytesPerLine == stride ){
simd->Memcpy( ptr, iptr, h * bytesPerLine );
} else {
uint8_t* p = ptr;
while( h-- ){
simd->Memcpy( p, iptr, bytesPerLine );
p += stride;
iptr += bytesPerLine;
}
}
_rgb.unmap( ptr );
}
void OpenNICamera::copyDepth()
{
_depthGen.GetMetaData( _depthData );
if( _depthData.XRes() != _depth.width() ||
_depthData.YRes() != _depth.height() ){
_depth.reallocate( _depthData.XRes(),
_depthData.YRes(),
IFormat::GRAY_UINT16 );
}
size_t stride;
uint16_t* ptr = _depth.map<uint16_t>( &stride );
const XnDepthPixel* dpixel = _depthData.Data();
SIMD* simd = SIMD::instance();
if( _depthData.XRes() == stride ){
simd->Memcpy( ( uint8_t* )ptr, ( const uint8_t* )dpixel, _depth.width() * _depth.height() * sizeof( uint16_t ) );
} else {
uint16_t* p = ptr;
size_t h = _depth.height();
size_t w = _depth.width();
size_t bytesPerLine = w * sizeof( uint16_t );
while( h-- ){
simd->Memcpy( ( uint8_t* )p, ( const uint8_t* )dpixel, bytesPerLine );
p += stride;
dpixel += w;
}
}
_depth.unmap( ptr );
}
void GLTexture::toImage( Image& img, IFormatType itype ) const
{
GLBuffer pbo( GL_PIXEL_PACK_BUFFER );
pbo.alloc( GL_STREAM_READ, IFormat::GRAY_FLOAT.bpp * _width * _height );
pbo.bind();
bind();
//glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
glGetTexImage( _target, 0, GL_DEPTH_COMPONENT, GL_FLOAT, NULL );
unbind();
pbo.unbind();
img.reallocate( _width, _height, IFormat::GRAY_FLOAT );
uint8_t* src = ( uint8_t* ) pbo.map( GL_READ_ONLY );
size_t dstride, sstride;
uint8_t* dst = img.map( &dstride );
uint8_t* pdst = dst;
SIMD* simd = SIMD::instance();
size_t n = _height;
sstride = IFormat::GRAY_FLOAT.bpp * _width;
while( n-- ) {
simd->Memcpy( pdst, src, sstride );
src += sstride;
pdst += dstride;
}
pbo.unmap();
img.unmap( dst );
}
static void copyData( Image& dst, const uint8_t* p, size_t pStride )
{
IMapScoped<uint8_t> map( dst );
size_t h = dst.height();
SIMD* simd = SIMD::instance();
size_t cStride = Math::min( map.stride(), pStride );
while( h-- ){
simd->Memcpy( map.ptr(), p, cStride );
map++;
p += pStride;
}
}
void Image::copyRect( int x, int y, const Image& img, const Recti & rect )
{
checkFormat( img, __PRETTY_FUNCTION__, __LINE__, _mem->_format );
int tx, ty;
tx = -x + rect.x;
ty = -y + rect.y;
Recti rdst( 0, 0, ( int ) _mem->_width, ( int ) _mem->_height );
rdst.translate( tx, ty );
Recti rsrc( 0, 0, ( int ) img._mem->_width, ( int ) img._mem->_height );
rsrc.intersect( rect );
rsrc.intersect( rdst );
if( rsrc.isEmpty() )
return;
rdst.copy( rsrc );
rdst.translate( -tx, -ty );
SIMD* simd = SIMD::instance();
size_t dstride;
uint8_t* dst = map( &dstride );
uint8_t* dbase = dst;
dst += rdst.y * dstride + bpp() * rdst.x;
size_t sstride;
const uint8_t* src = img.map( &sstride );
const uint8_t* sbase = src;
src += rsrc.y * sstride + rsrc.x * img.bpp();
size_t n = rsrc.width * img.bpp();
size_t i = rsrc.height;
while( i-- ) {
simd->Memcpy( dst, src, n );
src += sstride;
dst += dstride;
}
img.unmap( sbase );
unmap( dbase );
}
void CVTRawLoader::load( Image& img, const String& path )
{
int fd = open( path.c_str(), O_RDONLY );
if( fd == -1 ){
String error( "Could not open file: " );
error += path;
throw CVTException( error.c_str() );
}
// get the size of the file
struct stat fileInfo;
if( fstat( fd, &fileInfo ) < 0 ){
close( fd );
throw CVTException( "Could not get file information" );
}
size_t fileSize = fileInfo.st_size;
// file is opened -> map it
void * origPtr = mmap( 0, fileSize, PROT_READ, MAP_FILE | MAP_SHARED, fd, 0 );
uint8_t* ptr = ( uint8_t* )origPtr;
if( origPtr == MAP_FAILED ){
char * errorMsg = strerror( errno );
String error( "mmap failed: " );
error += errorMsg;
close( fd );
throw CVTException( error.c_str() );
}
close( fd );
// header: width, height, stride, IFormat
uint32_t savedStride, width, height, formatId;
width = *( ( uint32_t* )ptr ); ptr+= ( sizeof( uint32_t ) );
height = *( ( uint32_t* )ptr ); ptr+= ( sizeof( uint32_t ) );
savedStride = *( ( uint32_t* )ptr ); ptr+= ( sizeof( uint32_t ) );
formatId = *( ( uint32_t* )ptr ); ptr+= ( sizeof( uint32_t ) );
img.reallocate( width, height, IFormat::formatForId( ( IFormatID ) formatId ) );
uint8_t *p, *punmap;
size_t stride;
p = punmap = img.map<uint8_t>( &stride );
SIMD* simd = SIMD::instance();
if( savedStride == stride ){
simd->Memcpy( p, ptr, height * stride );
} else {
while( height-- ){
simd->Memcpy( p, ptr, width * img.bpp() );
p += stride;
ptr += savedStride;
}
}
if( munmap( origPtr, fileSize ) < 0 ){
throw CVTException( "Could not unmap memory!");
}
img.unmap( punmap );
}
