/*---------------------------------------------------------------
drawImage
---------------------------------------------------------------*/
void CCanvas::drawImage(
int x,
int y,
const utils::CImage &img )
{
MRPT_START
int img_lx = img.getWidth();
int img_ly = img.getHeight();
if (img.isColor())
{
for (int xx=0;xx<img_lx;xx++)
for (int yy=0;yy<img_ly;yy++)
setPixel(x+xx,y+yy, *((int*)img(xx,yy)) );
}
else
{
unsigned char c;
int col;
for (int xx=0;xx<img_lx;xx++)
for (int yy=0;yy<img_ly;yy++)
{
c = *((unsigned char *)img(xx,yy));
col = c | (c<<8) | (c<<16);
setPixel(x+xx,y+yy, col );
}
}
MRPT_END
}
/*---------------------------------------------------------------
getAsImageFiltered
---------------------------------------------------------------*/
void COccupancyGridMap2D::getAsImageFiltered(
utils::CImage &img,
bool verticalFlip,
bool forceRGB ) const
{
getAsImage(img,verticalFlip,forceRGB);
// Do filtering to improve the noisy peaks in grids:
// ----------------------------------------------------
#if 0
CTicTac t;
#endif
if (insertionOptions.CFD_features_gaussian_size!=0) img.filterGaussianInPlace( round( insertionOptions.CFD_features_gaussian_size ) );
if (insertionOptions.CFD_features_median_size!=0) img.filterMedianInPlace( round( insertionOptions.CFD_features_median_size ) );
#if 0
cout << "[COccupancyGridMap2D::getAsImageFiltered] Filtered in: " << t.Tac()*1000 << " ms" << endl;
#endif
}
/*---------------------------------------------------------------
drawImage
---------------------------------------------------------------*/
void CEnhancedMetaFile::drawImage(
int x,
int y,
const utils::CImage &img
)
{
try
{
LPBITMAPINFO pBmpInfo = (LPBITMAPINFO) new unsigned char[sizeof(BITMAPINFOHEADER) + (256 * sizeof(RGBQUAD))];
// LPBITMAPINFO pBmpInfo = (LPBITMAPINFO) new unsigned char[sizeof(BITMAPINFOHEADER) ];
unsigned int imgWidth = (unsigned int)img.getWidth();
unsigned int imgHeight = (unsigned int)img.getHeight();
pBmpInfo->bmiHeader.biSize = sizeof( BITMAPINFOHEADER );
pBmpInfo->bmiHeader.biWidth = imgWidth;
pBmpInfo->bmiHeader.biHeight = imgHeight;
pBmpInfo->bmiHeader.biPlanes = 1;
// pBmpInfo->bmiHeader.biBitCount = 24;
pBmpInfo->bmiHeader.biBitCount = 8;
pBmpInfo->bmiHeader.biCompression = BI_RGB;
pBmpInfo->bmiHeader.biSizeImage = 0;
pBmpInfo->bmiHeader.biXPelsPerMeter =
pBmpInfo->bmiHeader.biYPelsPerMeter = 0;
pBmpInfo->bmiHeader.biClrUsed = 0;
pBmpInfo->bmiHeader.biClrImportant = 0;
// Palette
for (unsigned char i=0; i<255; i++)
{
pBmpInfo->bmiColors[i].rgbRed = i;
pBmpInfo->bmiColors[i].rgbGreen = i;
pBmpInfo->bmiColors[i].rgbBlue = i;
pBmpInfo->bmiColors[i].rgbReserved = 0;
}
// unsigned int lineBytes = 3*bmp.Width;
unsigned int lineBytes = imgWidth;
if (lineBytes % 2) lineBytes++;
if (lineBytes % 4) lineBytes+=2;
BYTE *ptrBits = new BYTE[lineBytes * imgHeight];
for (unsigned int py=0;py<imgHeight;py++)
for (unsigned int px=0;px<imgWidth;px++)
ptrBits[(py*lineBytes+px)+0] = *img(px,py);
HBITMAP hBitmap = CreateDIBitmap(
(HDC)m_hdc.get(),
&pBmpInfo->bmiHeader,
CBM_INIT,
ptrBits,
pBmpInfo,
DIB_RGB_COLORS);
ASSERT_(hBitmap!=NULL);
BITMAP bm;
GetObject(hBitmap,sizeof(bm),&bm);
HDC hdcMem = CreateCompatibleDC( (HDC)m_hdc.get() );
HBITMAP hbmT = (HBITMAP)SelectObject(hdcMem,hBitmap);
BitBlt(
(HDC)m_hdc.get(),
x,
y,
(int)(m_scale*imgWidth),
(int)(m_scale*imgHeight),
hdcMem,
0,
0,
SRCCOPY);
SelectObject(hdcMem,hbmT);
DeleteDC(hdcMem);
// Free mem:
// ---------------------------------------
DeleteObject( hBitmap );
delete[] pBmpInfo;
delete[] ptrBits;
}
catch(...)
{
THROW_EXCEPTION("Unexpected runtime error!!");
}
}
/*---------------------------------------------------------------
getAsImage
---------------------------------------------------------------*/
void COccupancyGridMap2D::getAsImage(
utils::CImage &img,
bool verticalFlip,
bool forceRGB,
bool tricolor ) const
{
if (!tricolor)
{
if (!forceRGB)
{ // 8bit gray-scale
img.resize(size_x,size_y,1,true); //verticalFlip);
const cellType *srcPtr = &map[0];
unsigned char *destPtr;
for (unsigned int y=0;y<size_y;y++)
{
if (!verticalFlip)
destPtr = img(0,size_y-1-y);
else destPtr = img(0,y);
for (unsigned int x=0;x<size_x;x++)
{
*destPtr++ = l2p_255(*srcPtr++);
}
}
}
else
{ // 24bit RGB:
img.resize(size_x,size_y,3,true); //verticalFlip);
const cellType *srcPtr = &map[0];
unsigned char *destPtr;
for (unsigned int y=0;y<size_y;y++)
{
if (!verticalFlip)
destPtr = img(0,size_y-1-y);
else destPtr = img(0,y);
for (unsigned int x=0;x<size_x;x++)
{
uint8_t c = l2p_255(*srcPtr++);
*destPtr++ = c;
*destPtr++ = c;
*destPtr++ = c;
}
}
}
}
else
{
// TRICOLOR: 0, 0.5, 1
if (!forceRGB)
{ // 8bit gray-scale
img.resize(size_x,size_y,1,true); //verticalFlip);
const cellType *srcPtr = &map[0];
unsigned char *destPtr;
for (unsigned int y=0;y<size_y;y++)
{
if (!verticalFlip)
destPtr = img(0,size_y-1-y);
else destPtr = img(0,y);
for (unsigned int x=0;x<size_x;x++)
{
uint8_t c = l2p_255(*srcPtr++);
if (c<120)
c=0;
else if (c>136)
c=255;
else c = 127;
*destPtr++ = c;
}
}
}
else
{ // 24bit RGB:
img.resize(size_x,size_y,3,true); //verticalFlip);
const cellType *srcPtr = &map[0];
unsigned char *destPtr;
for (unsigned int y=0;y<size_y;y++)
{
if (!verticalFlip)
destPtr = img(0,size_y-1-y);
else destPtr = img(0,y);
for (unsigned int x=0;x<size_x;x++)
{
uint8_t c = l2p_255(*srcPtr++);
if (c<120)
c=0;
else if (c>136)
c=255;
else c = 127;
*destPtr++ = c;
*destPtr++ = c;
*destPtr++ = c;
}
}
}
}
}
