inline void drawOriginAxes(Reprojection &reproject, DebugImage &dbgImg,
double size = 0.02) {
auto drawLine = [&](Eigen::Vector3d const &axis, cv::Vec3b color) {
auto beginWindowPoint =
dbgImg.vecToWindowCoords(reproject(axis * 0.5 * size));
auto endWindowPoint =
dbgImg.vecToWindowCoords(reproject(axis * -0.5 * size));
cv::line(dbgImg.image(), beginWindowPoint.point,
endWindowPoint.point, cv::Scalar(color));
};
drawLine(Eigen::Vector3d::UnitX(), CVCOLOR_RED);
drawLine(Eigen::Vector3d::UnitY(), CVCOLOR_GREEN);
drawLine(Eigen::Vector3d::UnitZ(), CVCOLOR_BLUE);
}
void ColorSpaceView::updateDisplayLists()
{
SYNC_WITH(console);
DebugImage* image = nullptr;
DebugImage* raw = nullptr;
RobotConsole::Images& currentImages = upperCam ? console.upperCamImages : console.lowerCamImages;
RobotConsole::Images::const_iterator i = currentImages.find(name);
if(i != currentImages.end())
image = i->second.image;
i = currentImages.find("raw image");
if(i != currentImages.end())
raw = i->second.image;
if(image && (channel < 3 || raw))
{
if(!channel)
OpenGLMethods::paintCubeToOpenGLList(256, 256, 256,
cubeId, true,
127, //scale
-127, -127, -127, // offsets
int(background.x() * 255) ^ 0xc0,
int(background.y() * 255) ^ 0xc0,
int(background.z() * 255) ^ 0xc0);
else
OpenGLMethods::paintCubeToOpenGLList(image->getImageWidth(), image->height, 128,
cubeId, true,
127, //scale
-image->getImageWidth() / 2, -image->height / 2, -65, // offsets
int(background.x() * 255) ^ 0xc0,
int(background.y() * 255) ^ 0xc0,
int(background.z() * 255) ^ 0xc0);
OpenGLMethods::paintImagePixelsToOpenGLList(*image, colorModel, channel - 1, false, colorsId);
lastTimeStamp = image->timeStamp;
}
else
{
glNewList(cubeId, GL_COMPILE_AND_EXECUTE);
glEndList();
glNewList(colorsId, GL_COMPILE_AND_EXECUTE);
glEndList();
lastTimeStamp = 0;
}
}
void ImageWidget::copyImage(const DebugImage& srcImage)
{
int width = srcImage.width;
int height = srcImage.height;
const QImage::Format desiredFormat = QImage::Format::Format_RGB32;
if(!imageData || !imageDataStorage ||
!(imageData->width() == srcImage.getImageWidth() && imageData->height() == height) ||
imageData->format() != desiredFormat)
{
if(imageData)
delete imageData;
if(imageDataStorage)
Memory::alignedFree(imageDataStorage);
imageDataStorage = Memory::alignedMalloc(srcImage.getImageWidth() * height * 4 + 256, 32);
imageData = new QImage(reinterpret_cast<unsigned char*>(imageDataStorage), srcImage.getImageWidth(), height, desiredFormat);
}
imageView.console.debugImageConverter.convertToBGRA(srcImage, imageData->scanLine(0));
if(imageView.gain != 1.f)
{
unsigned* p = (unsigned*)imageData->scanLine(0);
float gain = imageView.gain;
for(unsigned* pEnd = p + width * height; p < pEnd; ++p)
{
int r = (int)(gain * (float)((*p >> 16) & 0xff));
int g = (int)(gain * (float)((*p >> 8) & 0xff));
int b = (int)(gain * (float)((*p) & 0xff));
*p = (r < 0 ? 0 : r > 255 ? 255 : r) << 16 |
(g < 0 ? 0 : g > 255 ? 255 : g) << 8 |
(b < 0 ? 0 : b > 255 ? 255 : b) |
0xff000000;
}
}
void OpenGLMethods::paintImagePixelsToOpenGLList(const DebugImage& image, int colorModel, int zComponent, bool polygons, int listID, int x1, int x2, int y1, int y2)
{
// Build a new list
::glNewList(listID, GL_COMPILE_AND_EXECUTE);
if(polygons)
{
glPolygonMode(GL_FRONT, GL_FILL);
glPolygonMode(GL_BACK, GL_FILL);
glBegin(GL_QUADS);
}
else
glBegin(GL_POINTS);
TImage<PixelTypes::BGRAPixel> rgbImage(image.getImageWidth(), image.height);
image.convertToBGRA(rgbImage[0]);
ColorChannels* convertedImage;
bool allocated = false;
switch(colorModel)
{
case 0: //yuv
if(image.type == PixelTypes::YUV)
{
convertedImage = const_cast<ColorChannels*>(image.getView<ColorChannels>()[0]);
}
else
{
convertedImage = new ColorChannels[rgbImage.width * rgbImage.height];
allocated = true;
PixelTypes::YUVPixel* dest = reinterpret_cast<PixelTypes::YUVPixel*>(convertedImage);
switch(image.type)
{
case PixelTypes::BGRA:
for(const PixelTypes::BGRAPixel* src = image.getView<PixelTypes::BGRAPixel>()[0], *srcEnd = image.getView<PixelTypes::BGRAPixel>()[image.height]; src < srcEnd; src++, dest++)
ColorModelConversions::fromRGBToYUV(src->r, src->g, src->b, dest->y, dest->u, dest->v);
break;
case PixelTypes::RGB:
for(const PixelTypes::RGBPixel* src = image.getView<PixelTypes::RGBPixel>()[0], *srcEnd = image.getView<PixelTypes::RGBPixel>()[image.height]; src < srcEnd; src++, dest++)
ColorModelConversions::fromRGBToYUV(src->r, src->g, src->b, dest->y, dest->u, dest->v);
break;
case PixelTypes::YUYV:
for(const PixelTypes::YUYVPixel* src = image.getView<PixelTypes::YUYVPixel>()[0], *srcEnd = image.getView<PixelTypes::YUYVPixel>()[image.height]; src < srcEnd; src++, dest++)
{
dest->y = src->y0;
dest->u = src->u;
dest->v = src->v;
dest++;
dest->y = src->y1;
dest->u = src->u;
dest->v = src->v;
}
break;
case PixelTypes::Colored:
for(const PixelTypes::BGRAPixel* src = rgbImage[0], *srcEnd = rgbImage[rgbImage.height]; src < srcEnd; src++, dest++)
ColorModelConversions::fromRGBToYUV(src->r, src->g, src->b, dest->y, dest->u, dest->v);
break;
case PixelTypes::Grayscale:
for(const PixelTypes::GrayscaledPixel* src = image.getView<PixelTypes::GrayscaledPixel>()[0], *srcEnd = image.getView<PixelTypes::GrayscaledPixel>()[image.height]; src < srcEnd; src++, dest++)
{
dest->y = *src;
dest->u = dest->v = 128;
}
break;
default:
ASSERT(false);
}
}
break;
case 1: //rgb
convertedImage = reinterpret_cast<ColorChannels*>(rgbImage[0]);
break;
case 2: //hsi
convertedImage = new ColorChannels[rgbImage.width * rgbImage.height];
allocated = true;
PixelTypes::HSIPixel* dest = reinterpret_cast<PixelTypes::HSIPixel*>(convertedImage);
for(const PixelTypes::BGRAPixel* src = rgbImage[0], *srcEnd = rgbImage[rgbImage.height]; src < srcEnd; src++, dest++)
ColorModelConversions::fromRGBToHSI(src->r, src->g, src->b, dest->h, dest->s, dest->i);
break;
}
float tmp = 2.0f / 255.0f;
float x, y, xn, yn;
float z00, z01, z10, z11;
for(int j = y1; j < y2 - 2; j++)
{
for(int i = x1; i < x2 - 2; i++)
{
if(zComponent == -1)
{
unsigned char* channels = convertedImage[j * rgbImage.width + i].channels;
if(colorModel == 0) //(padding is at 0 by yuv)
channels++;
x = -1.0f + channels[2] * tmp;
y = -1.0f + channels[0] * tmp;
z00 = -1.0f + channels[1] * tmp;
glColor3ub(rgbImage[j][i].r, rgbImage[j][i].g, rgbImage[j][i].b);
glVertex3d(x, y, z00);
}
else
{
x = (+i - rgbImage.width / 2.f) * tmp;
y = (-j + rgbImage.height / 2.f) * tmp;
xn = (+i + 1 - rgbImage.width / 2.f) * tmp;
yn = (-j - 1 + rgbImage.height / 2.f) * tmp;
z00 = (-0.5f + convertedImage[j * rgbImage.width + i].channels[zComponent] * tmp / 2);
if(polygons)
{
z01 = -0.5f + convertedImage[(j + 1) * rgbImage.width + i].channels[zComponent] * tmp / 2;
z10 = -0.5f + convertedImage[j * rgbImage.width + i + 1].channels[zComponent] * tmp / 2;
z11 = -0.5f + convertedImage[(j + 1) * rgbImage.width + i + 1].channels[zComponent] * tmp / 2;
glColor3ub(rgbImage[j][i].r, rgbImage[j][i].g, rgbImage[j][i].b);
glVertex3d(x, y, z00);
glColor3ub(rgbImage[j][i + 1].r, rgbImage[j][i + 1].g, rgbImage[j][i + 1].b);
glVertex3d(xn, y, z10);
glColor3ub(rgbImage[j + 1][i + 1].r, rgbImage[j + 1][i + 1].g, rgbImage[j + 1][i + 1].b);
glVertex3d(xn, yn, z11);
glColor3ub(rgbImage[j + 1][i].r, rgbImage[j + 1][i].g, rgbImage[j + 1][i].b);
glVertex3d(x, yn, z01);
}
else
{
glColor3ub(rgbImage[j][i].r, rgbImage[j][i].g, rgbImage[j][i].b);
glVertex3d(x, y, z00);
}
}
}
}
if(allocated)
{
delete[] convertedImage;
}
glEnd();
glPolygonMode(GL_FRONT, GL_LINE);
glPolygonMode(GL_BACK, GL_LINE);
::glEndList();
}
