static void
cam_renderer_draw (cam_renderer_t *cr)
{
if (! cr->renderer->renderer.enabled) return;
if (! cr->last_image) return;
// create the texture object if necessary
if (! cr->texture) {
cr->texture = glutil_texture_new (cr->last_image->width,
cr->last_image->height,
cr->last_image->width * 3 * cr->last_image->height);
}
// upload the texture to the graphics card if necessary
if (!cr->is_uploaded) {
int stride = 0;
GLenum gl_format;
uint8_t *tex_src = NULL;
if (cr->last_image->pixelformat == 0 ||
cr->last_image->pixelformat == PIXEL_FORMAT_GRAY ||
cr->last_image->pixelformat == PIXEL_FORMAT_BAYER_BGGR ||
cr->last_image->pixelformat == PIXEL_FORMAT_BAYER_RGGB ||
cr->last_image->pixelformat == PIXEL_FORMAT_BAYER_GRBG ||
cr->last_image->pixelformat == PIXEL_FORMAT_BAYER_GBRG) {
stride = cr->last_image->width;
gl_format = GL_LUMINANCE;
tex_src = cr->last_image->image;
} else if (cr->last_image->pixelformat == PIXEL_FORMAT_MJPEG) {
lcmtypes_image_t * msg = cr->last_image;
// might need to JPEG decompress...
stride = cr->last_image->width * 3;
int buf_size = msg->height * stride;
if (cr->uncompressed_buffer_size < buf_size) {
cr->uncompresed_buffer =
realloc (cr->uncompresed_buffer, buf_size);
cr->uncompressed_buffer_size = buf_size;
}
jpeg_decompress_to_8u_rgb (msg->image, msg->size,
cr->uncompresed_buffer, msg->width, msg->height, stride);
gl_format = GL_RGB;
tex_src = cr->uncompresed_buffer;
} else {
return;
}
glutil_texture_upload (cr->texture, gl_format, GL_UNSIGNED_BYTE,
stride, tex_src);
cr->is_uploaded = 1;
}
// draw the image
glColor3f (1, 1, 1);
glutil_texture_draw (cr->texture);
}
void draw_images (botlcm_image_t **img, int nimg, double ratio, double *user_trans)
{
for (int i=0;i<nimg;i++) {
botlcm_image_t *im = img[i];
if (!im) continue;
gboolean greyscale = img[i]->pixelformat == CAM_PIXEL_FORMAT_GRAY;
GLUtilTexture *tex = glutil_texture_new (im->width, im->height, im->row_stride * im->height);
if (!tex) continue;
glutil_texture_upload (tex, greyscale ? GL_LUMINANCE : GL_RGB, GL_UNSIGNED_BYTE, im->row_stride, im->data);
int xmin = i * im->width * ratio + user_trans[0];
int xmax = (i+1) * im->width * ratio + user_trans[0];
int ymin = 0 + user_trans[1];
int ymax = im->height*ratio + user_trans[1];
glutil_texture_draw_pt (tex, xmin, ymin, xmin, ymax, xmax, ymax, xmax, ymin);
glutil_texture_free (tex);
}
}
/* draw image sub-region
*/
void draw_img_subregion (botlcm_image_t **img, int nimg, int col, int row, int sensorid, float scale, float orientation, int feature_type, int wcol, int wrow)
{
if (sensorid < 0 || sensorid > 4) {
printf ("mmm... sensorid = %d for feature at pos %d %d (orientation: %.3f)\n", sensorid, col, row, orientation);
}
/* SURF scale is different */
if (feature_type == NAVLCM_FEATURES_PARAM_T_SURF64 || feature_type == NAVLCM_FEATURES_PARAM_T_SURF128)
scale *= 8;
/* SIFT scale is different */
if (feature_type == NAVLCM_FEATURES_PARAM_T_SIFT || feature_type == NAVLCM_FEATURES_PARAM_T_SIFT2)
scale *= 1.414 * 3 * (4 + 1) / 2.0;
/* FAST scale is different */
if (feature_type == NAVLCM_FEATURES_PARAM_T_FAST)
scale = 15;
assert (0 <= sensorid && sensorid <= 4);
botlcm_image_t *image = img[sensorid];
assert (image);
int size = 100;
unsigned char *data = (unsigned char*)calloc(3*size*size,sizeof(unsigned char));
if (sensorid == 0)
wcol += 500;
// fill pixels
for (int c=0;c<size;c++) {
for (int r=0;r<size;r++) {
int cp = col + (int)((1.0 * c - size/2)/(size/2) * scale);
int rp = row + (int)((1.0 * r - size/2)/(size/2) * scale);
if (0 <= cp && cp < image->width && 0 <= rp && rp < image->height) {
data[3*(r*size+c)] = image->data[rp*image->width + cp];
data[3*(r*size+c)+1] = image->data[rp*image->width + cp];
data[3*(r*size+c)+2] = image->data[rp*image->width + cp];
}
}
}
// draw orientation as a line
for (int r=0;r<size;r++) {
int cp = size/2 + (int)(1.0 * r - size/2) * cos (orientation);
int rp = size/2 + (int)(1.0 * r - size/2) * sin (orientation);
if (0 <= cp && cp < size && 0 <= rp && rp < size) {
data[3*(rp*size+cp)] = 255;
data[3*(rp*size+cp)+1] = 255;
data[3*(rp*size+cp)+2] = 0;
}
}
// draw red cross at the center
for (int r=0;r<20;r++) {
data[3*(size/2*size+size/2+r-10)] = 255;
data[3*(size/2*size+size/2+r-10)+1] = 0;
data[3*(size/2*size+size/2+r-10)+2] = 0;
}
for (int r=0;r<20;r++) {
data[3*((size/2+r-10)*size+size/2)] = 255;
data[3*((size/2+r-10)*size+size/2)+1] = 0;
data[3*((size/2+r-10)*size+size/2)+2] = 0;
}
GLUtilTexture *tex = glutil_texture_new (size, size, 3*size*size);
if (tex) {
int xmin = wcol;
int xmax = xmin+size-1;
int ymin = wrow;
int ymax = ymin+size-1;
// on a red background
glColor3f (1,0,0);
glBegin (GL_QUADS);
glVertex2f (xmin-4, ymin-4);
glVertex2f (xmin-4, ymax+4);
glVertex2f (xmax+4, ymax+4);
glVertex2f (xmax+4, ymin-4);
glEnd ();
glutil_texture_upload (tex, GL_RGB, GL_UNSIGNED_BYTE, 3*size, data);
glutil_texture_draw_pt (tex, xmin, ymin, xmin, ymax, xmax, ymax, xmax, ymin);
glutil_texture_free (tex);
}
free (data);
}
