Pixy (CMUcam5) – Getting RAW picture for OpenCV

Pixy (CMUcam5) camera from Charmed Labs for image processing on a Raspberry Pi for an autonomous robot. The inclusion of two servo drives for pan and tilt made it an obvious choice for a robot that wants to look around.

However one drawback is that the camera doesn’t return an image for processing. The CMUcam5 Pixy contains an NXP LPC4330 processor for image processing which is configured through the PixyMon software provided to detect objects of various colors and output the location of the detected objects at 50fps through USB. Useful for simple applications but not in my case.

Pixy Demo
CMUcam5 Pixy Demo from Charmed Labs

So I went ahead and peaked into the PixyMon code to extract the image that I can later process in my Raspberry Pi using OpenCV and with a huge help from the community was able to properly extract the RAW image data and process it.

Here I provide a not so explained version of a code that get the raw picture from the camera and interpolates it to get a proper RGB Image which can be used in OpenCV.

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <string.h>
#include <math.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <ctime>
#include "pixy.h"
 
using namespace cv;
using namespace std;
 
void interpolateBayer(unsigned int width, unsigned int x, unsigned int y, unsigned char *pixel, unsigned int &r, unsigned int &g, unsigned int &b);
Mat renderBA81(uint8_t renderFlags, uint16_t width, uint16_t height, uint32_t frameLen, uint8_t *frame);
Mat getImage();
 
int main(int argc, char * argv[])
{
    int pixy_init_status;
    int return_value;
    int32_t response;
 
    pixy_init_status = pixy_init();
 
    if(!pixy_init_status == 0)
    {
        printf("pixy_init(): ");
        pixy_error(pixy_init_status);
        return pixy_init_status;
    }
 
    return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS);
 
    return_value = pixy_rcs_set_position(1, 900);
    return_value = pixy_rcs_set_position(0, 500);
 
    Mat pixy_image = getImage();
 
    namedWindow("Image", CV_WINDOW_NORMAL);
    imshow("Image", pixy_image);
 
    return 0;
}
 
Mat getImage()
{
    unsigned char *pixels;
    int32_t response, fourcc;
    int8_t renderflags;
    int return_value, res;
    uint16_t rwidth, rheight;
    uint32_t  numPixels;
    uint16_t height,width;
    uint16_t mode;
 
    return_value = pixy_command("run", END_OUT_ARGS, &response, END_IN_ARGS);
    return_value = pixy_command("stop", END_OUT_ARGS, &response, END_IN_ARGS);
 
    return_value = pixy_command("cam_getFrame",  // String id for remote procedure
                                 0x01, 0x21,      // mode
                                 0x02,   0,        // xoffset
                                 0x02,   0,         // yoffset
                                 0x02, 320,       // width
                                 0x02, 200,       // height
                                 0,            // separator
                                 &response, &fourcc, &renderflags, &rwidth, &rheight, &numPixels, &pixels, 0);
 
    return renderBA81(renderflags,rwidth,rheight,numPixels,pixels);
}
 
inline void interpolateBayer(uint16_t width, uint16_t x, uint16_t y, uint8_t *pixel, uint8_t* r, uint8_t* g, uint8_t* b)
{
    if (y&1)
    {
        if (x&1)
        {
            *r = *pixel;
            *g = (*(pixel-1)+*(pixel+1)+*(pixel+width)+*(pixel-width))>>2;
            *b = (*(pixel-width-1)+*(pixel-width+1)+*(pixel+width-1)+*(pixel+width+1))>>2;
        }
        else
        {
            *r = (*(pixel-1)+*(pixel+1))>>1;
            *g = *pixel;
            *b = (*(pixel-width)+*(pixel+width))>>1;
        }
    }
    else
    {
        if (x&1)
        {
            *r = (*(pixel-width)+*(pixel+width))>>1;
            *g = *pixel;
            *b = (*(pixel-1)+*(pixel+1))>>1;
        }
        else
        {
            *r = (*(pixel-width-1)+*(pixel-width+1)+*(pixel+width-1)+*(pixel+width+1))>>2;
            *g = (*(pixel-1)+*(pixel+1)+*(pixel+width)+*(pixel-width))>>2;
            *b = *pixel;
        }
    }
 
}
 
Mat renderBA81(uint8_t renderFlags, uint16_t width, uint16_t height, uint32_t frameLen, uint8_t *frame)
{
    uint16_t x, y;
    uint8_t r, g, b;
    Mat imageRGB;
    Mat imageHSV;
 
    frame += width;
    uchar data[3*((height-2)*(width-2))];
 
    uint m = 0;
    for (y=1; y<height-1; y++)
    {
        frame++;
        for (x=1; x<width-1; x++, frame++)
        {
            interpolateBayer(width, x, y, frame, &r, &g, &b);
            data[m++] = b;
            data[m++] = g;
            data[m++] = r;
        }
        frame++;
    }
 
    imageRGB =  Mat(height - 2,width -2, CV_8UC3, data);
 
    cvtColor (imageRGB,imageHSV,CV_BGR2HSV);
 
    return imageHSV;
}

The size of the image is limited by the buffer memory available in the processor. High resolution image of stationary objects can be obtained by getting multiple images using different offset values in the “cam_getFrame” command which should be stitched together later.