Pi Eyes Stage 4

Last time round I got to the point at which I was pulling data out of the camera and using opengl to efficiently render it to screen. There's a few performance improvements to be made, but it runs at a respectable frame rate and could hit 30hz at 720p no problem.

Next up it's time to get the camera feed from the native YUV format into RGB ready for image processing. I'll also be needing to get some down sampled versions of the image, as the more expensive image processing algorithms aren't fast enough to run on a hi def video feed. This'll be a fairly breif post though, as it's late and my brain is going to sleep...

The Image Resizer

My first port of call was the image resize component built into mmal (thanks to a handy tip on this post), which uses the hardware resizer to take an image and... well... resize it! However, as a handy side effect it can also convert between YUV and RGB in the process. At this point massive thanks goes out to Mr Jason Van Cleave, who made all the mmal component documentation available on his web site.

So in short, I need to adjust the camera code so it:

• Create an image resize component (I eventually worked out its the "vc.ril.resizer" component)
• Connect it to the camera's video output port (the one we're currently displaying)
• Set it's input format to that of the video output, and the output format to RGBA. We leave the image sizes the same for now though, so its not really doing any resizing - just the conversion

I do a little code cleanup first so it's easier to add to, plonk in the new code and after a few iterations...

We're in business! Code here:

http://www.cheerfulprogrammer.com/downloads/pi_eyes_stage4/picam_rgb.zip

Unfortunately on the first attempt performance appeared to be very poor. Interestingly though if I remove the actual rendering of the image it runs fine. This leads me to believe that the image resizer is chomping through most of the gpu time and consequentially I can't render fast enough. This is really annoying as I frankly don't see why it should be so slow - maybe it's just some interplay between opengl and mmal. 

I'll know more about the resizer performance once I get multiple resizers running, generating different downsampled images and we'll see what the actual costs are. If necessary I'm fairly confident I could write a shader that did the convert and downsample quite efficiently. I'm now getting 15hz, which I'm not happy about but it'll do for the moment.

A quick restructure

My next goal is to get multiple outputs at different resolutions coming out of the camera. This allows me to analyse the data at different levels in order to pick and choose where I spend my cpu. It should be doable using the 'video splitter' component, but it raises a few problems in terms of my architecture.

Right now the camera code simply runs, then calls a callback for each frame. Once the splitter is running I'll be receiving blocks of data constantly from different sources and will need a nice way of managing this and providing it in an api to the user. As a result, before going onto the multiple output world, I'm going to adjust the camera code so it internally buffers up the frames and allows the user to read them in a syncronous manner. If I can make use of the queuing system in mmal then I should be able to set it up as follows:

The basic idea is that the camera wraps up all the mmal stuff as usual, but rather than providing a callback, the application simply calls 'ReadFrame' to get the current frame from the camera. It passes in a 'level' to choose the downsampling level (0=full res, 1=half res, 2=quarter res) and obviously a place to put the data. 

Internally those output queues will be added to by the internal callbacks on the resizer output ports. Crucially, the resizer buffers will be passed directly into the output queue. A buffer will then only be returned to the resizer when:

• The application calls ReadFrame, thus the buffer is no longer needed
• An output queue contains more than x (potentially 1) entry, indicating the application isn't reading data fast enough (or at all) so frames can be dropped

This'll all be a lot easier if I can use the mmal queue code, but if not I'll roll my own. 

The only problem with this plan is that it involves fiddling around with a complex api and reworking lots of fiddly code, and it's past my bed time. Even coders need sleep, so I'll have to get to downsampling another day.

Pi Eyes Stage 3

For the past few days I've been working on getting a pair of raspberry pi camera modules working and accessing their data in a c++ program ready for image processing. Last night I got to the first version of my camera api which can be found here. So far I can:

• Start the camera up
• Have a callback triggered once per frame that gets past the buffer + its size
• Shut the camera down

Very soon I'll get to work on converting the YUV data the camera submits into nice friendly RGB data, and get downsampling of the images going. Both will either need to be done using mmal, or through my own GPU code if they stand a chance of being usable for real time processing. Once they're going I'll be in a great position to get more complex stuff like feature analysis working.

However, while I've got a host of ideas of how to move forwards, the first thing to do is get the output from the camera rendering on screen so I can actually see it in action. As such my next goal is to get opengl going, and render the output from the camera callback to the screen. Initially it'll look like garbage (as it'll actually be yuv data), but it'll be garbage that changes when things move in front of the camera! Once it's working I'll be in a position to do things like downsampling, rgb conversion etc and actually see the results to verify they're functioning correctly.

Getting OpenGL going

I've not endeavered to get opengl working on the pi yet, but there's a couple of examples called hello_triangle and hello_triangle2. On looking at them, hello_triangle2 is both the simplest and most interesting as it uses shaders to do its rendering. I start by copying the graphics init code from hello_triangle2, and then add begin/end frame calls that basically clear the screen and swap the buffers respectively. This rather uninteresting photo is the result:

OK so it's not much, but crucially it shows opengl is operating correctly - I have a render loop that is clearing the screen to blue (and all the while I'm still reading the camera every frame in the background).

Shaders, Buffers and Boxes

I'm not gonna mess with fixed function pipelines and then have to go back and change it to shaders as soon as I want something funky - this is the 21st century after all! As a result I need to get shaders working which I've never done in opengl. From the example it basically seems to be a case of:

• Load/set source code for a shader
• Compile it, and get a shader id
• Create a 'program', and assign it a vertex shader and a fragment shader

So I come up with this code inside a little GfxShader class:

bool GfxShader::LoadVertexShader(const char* filename)
{
    //cheeky bit of code to read the whole file into memory
    assert(!Src);
    FILE* f = fopen(filename, "rb");
    assert(f);
    fseek(f,0,SEEK_END);
    int sz = ftell(f);
    fseek(f,0,SEEK_SET);
    Src = new GLchar[sz+1];
    fread(Src,1,sz,f);
    Src[sz] = 0; //null terminate it!
    fclose(f);

    //now create and compile the shader
    GlShaderType = GL_VERTEX_SHADER;
    Id = glCreateShader(GlShaderType);
    glShaderSource(Id, 1, (const GLchar**)&Src, 0);
    glCompileShader(Id);
    check();
    printf("Compiled vertex shader:\n%s\n",Src);

    return true;
}

That just loads up a file, and fires it through the open gl code to create a shader program. Next, I knock together the simplest vertex shader and fragment shader I can think of:

SimpleVertShader.glsl:

attribute vec4 vertex;
void main(void) 
{
    vec4 pos = vertex;
    gl_Position = pos;
};

SimpleFragShader.glsl:

void main(void) 
{
    gl_FragColor = float4(1,1,1,1);
};

And now it's time to try and render a triangle using those shaders!!! Please note - at time of writing I still don't know if this is going to work, or if those shaders are entirely wrong... Unless I've missed something, it appears the old way of specifying vertices 1 by 1 isn't present in OpenGLES2 (although it's very possible I've missed something), so I'm gonna need to create me a vertex buffer. I knock together these bits to create and draw it...

Create it...

    //create an ickle vertex buffer
    static const GLfloat quad_vertex_positions[] = {
        0.0f, 0.0f,    1.0f, 1.0f,
        1.0f, 0.0f, 1.0f, 1.0f,
        1.0f, 1.0f, 1.0f, 1.0f,
        0.0f, 1.0f, 1.0f, 1.0f
    };
    glGenBuffers(1, &GQuadVertexBuffer);
    glBindBuffer(GL_ARRAY_BUFFER, GQuadVertexBuffer);
    glBufferData(GL_ARRAY_BUFFER, sizeof(quad_vertex_positions), quad_vertex_positions, GL_STATIC_DRAW);
    check();

Draw it...

    glUseProgram(GSimpleProg.GetId());
    printf("gl error: %d\n",glGetError());
    check();
    glBindBuffer(GL_ARRAY_BUFFER, GQuadVertexBuffer);
    GLuint loc = glGetAttribLocation(GSimpleProg.GetId(),"vertex");
    glVertexAttribPointer(loc, 4, GL_FLOAT, 0, 16, 0);
    glEnableVertexAttribArray(loc);
    check();
    glDrawArrays ( GL_TRIANGLE_STRIP, 0, 4 );
    check();
    glFinish();
    glFlush();
    check();

But glUseProgram is giving me errors so its thinking hat time....

And... an hour of fiddling later I've discovered open gl doesn't return an error if the shader compiling or linking into a program fails. Instead it returns happy success, unless you specifically ask it how things went! Having fixed some compile errors in my earlier shaders I run it and am presented with my first quad:

And after adding offset and scale uniforms and passing them into this draw function...

void DrawWhiteRect(float x0, float y0, float x1, float y1)
{
    glUseProgram(GSimpleProg.GetId());
    check();

    glUniform2f(glGetUniformLocation(GSimpleProg.GetId(),"offset"),x0,y0);
    glUniform2f(glGetUniformLocation(GSimpleProg.GetId(),"scale"),x1-x0,y1-y0);

    glBindBuffer(GL_ARRAY_BUFFER, GQuadVertexBuffer);
    check();

    GLuint loc = glGetAttribLocation(GSimpleProg.GetId(),"vertex");
    check();

    glVertexAttribPointer(loc, 4, GL_FLOAT, 0, 16, 0);
    check();

    glEnableVertexAttribArray(loc);
    check();

    glDrawArrays ( GL_TRIANGLE_STRIP, 0, 4 );
    check();

    glFinish();
    check();

    glFlush();
    check();
}

Hmm - not sure what the glFlush does yet. One for later though. The point is I can make a box anywhere I want:

OK, it's.....

Texture Time

My ultimate goal here is to get the camera texture on screen, which will involve filling an open gl texture with data each frame and then displaying it on a quad like the one above. Before getting that far I'm just gonna try filling a texture with random data each frame and seeing where that gets me...

...half an hour later... well having grasped opengles2 a bit better, that was actually fairly easy. We have a 32x32 random texture (and a code base that's getting messier by the second):

Woohooo!

From camera to texture

This is it folks. If I can get from camera output to something on screen at a decent frame rate then it paves the way for much wonders on this raspberry pi. I'll start with a hacky not-thread-safe approach which will also waste a bit of cpu time doing extra memcpys and generally be bad. But quick to write.

So we've got a callback in the app that is submitting new frames from a separate thread, and a call on the main frame to render a texture on screen. I just need to get the data from the thread into the texure and all will be well. I start by bodgily setting my earlier 'random texture' to be exactly the right size for a 1280x720 camera image, resulting in something a little more 'trippy':

Now to try regenerating that random data each frame - works although very slow. Not even worth uploading the video I made of it really!

However, I now have code that generates some new data, pumps it to open gl and then draws it on screen. All I need to do now is use my camera feed as the 'new data' instead of random numbers. I write a new function that takes a pointer to a buffer and copies it into the block of memory I was previously filling with random numbers. Remember my camera data is still in YUV so it'll not fill a full RGB texture (and will look weird), so I make it keep copying until it fills the buffer - this gives me a good measure of performance. A bit of jiggery pokery and...

Eureka!!!

At 1080p the memcpys involved (probably 2 - one from camera -> scratch space, another from scratch space -> open gl texture) are heavy enough and it hits about 10fps. But at 720p (still easily enough for some tasty image fun) it's in at around 25fps. With a little clever engineering I can remove 1 of those copies, so it'll hit a solid 30fps. Here's a video to show it in action:

Pretty tasty yes? Although please note - when I say 'copies it into the cpu' I mean 'into cpu accesible memory'. One doesn't make sense, the other does...

All code is here, although it's in a bit of a state right now so don't take anything as 'the right way to do it' - especially the graphics bits!

http://www.cheerfulprogrammer.com/downloads/pi_eyes_stage3/picam_rendering.zip

Next Steps

Now that I can see what I'm generating (and can do it at an appreciable frame rate) I'm going to look at using the mmal image resizer component to create downsampled versions of the feed (for different granularity in image processing) and in theory do the rgb conversion (if the documentation is telling the truth...).

Firs though, I need to order a takeaway.

Pi Eyes Stage 2

Right, in my last post I had got the raspberry pi camera modules up and running, but hit a bit of a blocker in terms of accessing the actual camera feeds in c++. Fortunately a very clever chap called Pierre Raufast had documented his reworking of the raspivid application here. It'd suffered a little over time, probably just due to newer versions of its dependencies so I redid some of his work and ended up with camcv.c, which sets up the camera and provides a point at which we can access each frame of the camera feed. My next task is to rewrite it from scratch, then experiment with decoding the data in an optimal way. First though, just so this post isn't entirely code - a picture of the latest setup:

My raspberry pi 'stereo camera rig'. Good old balsa wood and insulating tape.

Quick Instructions on getting the code

In this post I get to my first version of a working camera api, which can be downloaded here:

http://www.cheerfulprogrammer.com/downloads/pi_eyes_stage2/picam.zip

Note that to use it, you'll need to download and build the raspberry pi userland code from here. Mine is stored in /opt/vc/userland-master.

I'll go into more details once I have something I'm really happy with!

Writing the actual code...

The basic architecture of the camera system is quite simple once you get over the total lack of documentation of the fairly complex mmal layer... We basically:

• Start up mmal
• Create a 'camera component'
• Tell its 'video output port' to call a callback each time it fills in a new buffer
• In the callback we:
• Lock the buffer
• Read it
• Unlock it
• Give it back to be the port to be recycled
• And when all is done, we kill the camera component and bail out

The callback is called from a seperate thread, so once things are moving the main application can carry on as normal. This lends itself well to a simple initial api of just:

• StartCamera(some setup options + callback pointer)
• StopCamera()

The main bit of code I'm going to keep from the raspberry pi userland code is the raspicamcontrol stuff, which wraps up setting parameters on the camera in a simple api.

.... imagine moments of intense programming with blondie on in the background here ....

It's a few hours later and I've finished revision one. I've got a basic camera api that gets initialised, does stuff for a while then shuts down. Here's the first 'application' that uses it:

#include <stdio.h>
#include <unistd.h>
#include "camera.h"

void CameraCallback(CCamera* cam, const void* buffer, int buffer_length)
{
    printf("Do stuff with %d bytes of data\n",buffer_length);
}

int main(int argc, const char **argv)
{
    printf("PI Cam api tester\n");
    StartCamera(1280,720,30,CameraCallback);
    sleep(10);
    StopCamera();
}

Neat! With that code I run the application and get this print out

PI Cam api tester
Creating video port pool with 3 buffers of size 1382400
mmal: mmal_vc_port_parameter_set: failed to set port parameter 64:0:ENOSYS
mmal: Function not implemented
Sent buffer 0 to video port
Sent buffer 0 to video port
Sent buffer 0 to video port
Camera successfully created
Do stuff with 1382400 bytes of data
Do stuff with 1382400 bytes of data
Do stuff with 1382400 bytes of data
//... repeat every frame for 10 seconds...
Do stuff with 1382400 bytes of data
Do stuff with 1382400 bytes of data
Do stuff with 1382400 bytes of data

Shutting down camera

Most of the magic is inside 2 files:

• CameraControl.h and CameraControl.cpp - basically just a version of the original RaspiCamControl.c without command line parsing in.
• Camera.h and Camera.cpp - my new camera api

I've posted the full files online for download, but will go over the key bits here. The beefy one is the camera initialization - CCamera::Init

Camera Initialisation

Basic setup / creation of camera component

bool CCamera::Init(int width, int height, int framerate, CameraCBFunction callback)
{
    //init broadcom host - QUESTION: can this be called more than once??
    bcm_host_init();

    //store basic parameters
    Width = width;       
    Height = height;
    FrameRate = framerate;
    Callback = callback;

    // Set up the camera_parameters to default
    raspicamcontrol_set_defaults(&CameraParameters);

    MMAL_COMPONENT_T *camera = 0;
    MMAL_ES_FORMAT_T *format;
    MMAL_PORT_T *preview_port = NULL, *video_port = NULL, *still_port = NULL;
    MMAL_STATUS_T status;

    //create the camera component
    status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
    if (status != MMAL_SUCCESS)
    {
        printf("Failed to create camera component\n");
        return false;
    }

    //check we have output ports
    if (!camera->output_num)
    {
        printf("Camera doesn't have output ports");
        mmal_component_destroy(camera);
        return false;
    }

    //get the 3 ports
    preview_port = camera->output[MMAL_CAMERA_PREVIEW_PORT];
    video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
    still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT];

    // Enable the camera, and tell it its control callback function
    status = mmal_port_enable(camera->control, CameraControlCallback);
    if (status != MMAL_SUCCESS)
    {
        printf("Unable to enable control port : error %d", status);
        mmal_component_destroy(camera);
        return false;
    }

    //  set up the camera configuration
    {
        MMAL_PARAMETER_CAMERA_CONFIG_T cam_config;
        cam_config.hdr.id = MMAL_PARAMETER_CAMERA_CONFIG;
        cam_config.hdr.size = sizeof(cam_config);
        cam_config.max_stills_w = Width;
        cam_config.max_stills_h = Height;
        cam_config.stills_yuv422 = 0;
        cam_config.one_shot_stills = 0;
        cam_config.max_preview_video_w = Width;
        cam_config.max_preview_video_h = Height;
        cam_config.num_preview_video_frames = 3;
        cam_config.stills_capture_circular_buffer_height = 0;
        cam_config.fast_preview_resume = 0;
        cam_config.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC;
        mmal_port_parameter_set(camera->control, &cam_config.hdr);
    }

This first section is pretty simple, albiet fairly long. It's just:

• Creating the mmal camera component
• Getting the 3 'output ports'. The main one we're interested in is the video port, but as far as I can tell the others still need setting up for correct operation.
• Enabling the 'control' port and providing a callback. This basically gives the camera a way of providing us with info about changes of state. Not doing anything with this yet though.
• Filling out a camera config structure, then sending it to the camera control port

Setting output port formats

Now we have a camera component running, the next step is to configure those output ports:

    // setup preview port format - QUESTION: Needed if we aren't using preview?
    format = preview_port->format;
    format->encoding = MMAL_ENCODING_OPAQUE;
    format->encoding_variant = MMAL_ENCODING_I420;
    format->es->video.width = Width;
    format->es->video.height = Height;
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = Width;
    format->es->video.crop.height = Height;
    format->es->video.frame_rate.num = FrameRate;
    format->es->video.frame_rate.den = 1;
    status = mmal_port_format_commit(preview_port);
    if (status != MMAL_SUCCESS)
    {
        printf("Couldn't set preview port format : error %d", status);
        mmal_component_destroy(camera);
        return false;
    }

    //setup video port format
    format = video_port->format;
    format->encoding = MMAL_ENCODING_I420; //not opaque, as we want to read it!
    format->encoding_variant = MMAL_ENCODING_I420; 
    format->es->video.width = Width;
    format->es->video.height = Height;
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = Width;
    format->es->video.crop.height = Height;
    format->es->video.frame_rate.num = FrameRate;
    format->es->video.frame_rate.den = 1;
    status = mmal_port_format_commit(video_port);
    if (status != MMAL_SUCCESS)
    {
        printf("Couldn't set video port format : error %d", status);
        mmal_component_destroy(camera);
        return false;
    }

    //setup still port format
    format = still_port->format;
    format->encoding = MMAL_ENCODING_OPAQUE;
    format->encoding_variant = MMAL_ENCODING_I420;
    format->es->video.width = Width;
    format->es->video.height = Height;
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = Width;
    format->es->video.crop.height = Height;
    format->es->video.frame_rate.num = 1;
    format->es->video.frame_rate.den = 1;
    status = mmal_port_format_commit(still_port);
    if (status != MMAL_SUCCESS)
    {
        printf("Couldn't set still port format : error %d", status);
        mmal_component_destroy(camera);
        return false;
    }

This is 3 almost identical bits of code - one for the preview port (which would be used for doing the full screen preview of the feed if we were using it), one for the video port (that's the one we're interested in) and one for the still port (presumably for capturing stills). If you read the code it's pretty much just plugging in the numbers provided to configure the camera. The most important part, highlighted in red is where we set the video port format to I420 encoding (the native format of the camera). By setting it correctly, this tells mmal that we will be providing a callback for the video output later, and it'll be wanting all the data thankyou very much! Otherwise it just passes in the buffer headers but no actual output... Point of note - I tried setting the format to ABGR, but the camera just output I420 data in a dodgy layout, so it's going to need converting.

Create a buffer pool for the video port to write to

    //setup video port buffer and a pool to hold them
    video_port->buffer_num = 3;
    video_port->buffer_size = video_port->buffer_size_recommended;
    MMAL_POOL_T* video_buffer_pool;
    printf("Creating video port pool with %d buffers of size %d\n", video_port->buffer_num, video_port->buffer_size);
    video_buffer_pool = mmal_port_pool_create(video_port, video_port->buffer_num, video_port->buffer_size);
    if (!video_buffer_pool)
    {
        printf("Couldn't create video buffer pool\n");
        mmal_component_destroy(camera);
        return false;    
    }

This little chunk is the first properly 'new' bit when compared to the raspivid. It creates a pool of buffers that we'll be providing to the video port to write the frames to. For now we're just creating it, but later we'll pass all the buffers to the video port and then begin capturing! The buffer_num is 3, as that gives you enough time to have the camera writing 1 buffer, while you read another, with an extra one in the middle for safety. The recommended buffer size comes from the format we specified earlier.

Enable stuff

    //enable the camera
    status = mmal_component_enable(camera);
    if (status != MMAL_SUCCESS)
    {
        printf("Couldn't enable camera\n");
        mmal_port_pool_destroy(video_port,video_buffer_pool);
        mmal_component_destroy(camera);
        return false;    
    }

    //apply all camera parameters
    raspicamcontrol_set_all_parameters(camera, &CameraParameters);

    //setup the video buffer callback
    status = mmal_port_enable(video_port, VideoBufferCallback);
    if (status != MMAL_SUCCESS)
    {
        printf("Failed to set video buffer callback\n");
        mmal_port_pool_destroy(video_port,video_buffer_pool);
        mmal_component_destroy(camera);
        return false;    
    }

This pretty simple code enables the camera, sends it a list of setup parameters using the cam control code, then enables the video port. Note the port enable call, which tells the video port about our VideoBufferCallback function, which we want calling for each frame received from the camera.

Give the buffers to the video port

    //send all the buffers in our pool to the video port ready for use
    {
        int num = mmal_queue_length(video_buffer_pool->queue);
        int q;
        for (q=0;q<num;q++)
        {
            MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(video_buffer_pool->queue);
            if (!buffer)
                printf("Unable to get a required buffer %d from pool queue", q);
            if (mmal_port_send_buffer(video_port, buffer)!= MMAL_SUCCESS)
                printf("Unable to send a buffer to encoder output port (%d)", q);
            printf("Sent buffer %d to video port\n");
        }
    }

OK, so this one looks a bit odd! The basic idea is that we created a pool of 3 buffers earlier, which is basically a queue of pointers to unused blocks of memory. This bit of code removes each buffer from the pool and sends it into the video port. In effect, we're handing the video port the blocks of memory it'll use to store frames in.

Begin capture and return SUCCESS!

    //begin capture
    if (mmal_port_parameter_set_boolean(video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS)
    {
        printf("Failed to start capture\n");
        mmal_port_pool_destroy(video_port,video_buffer_pool);
        mmal_component_destroy(camera);
        return false;    
    }

    //store created info
    CameraComponent = camera;
    BufferPool = video_buffer_pool;

    //return success
    printf("Camera successfully created\n");
    return true;

As our final trick, we set the 'capturing' setting to 1, and if all goes well that VideoBufferCallback function should start getting called.

The video callback

What also deserves a mention is the video callback:

void CCamera::OnVideoBufferCallback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
    //check if buffer has data in
    if(buffer->length)
    {
        //got data so lock the buffer, call the callback so the application can use it, then unlock
        mmal_buffer_header_mem_lock(buffer);
        Callback(this,buffer->data,buffer->length);
        mmal_buffer_header_mem_unlock(buffer);
    }
    
    // release buffer back to the pool
    mmal_buffer_header_release(buffer);

    // and send one back to the port (if still open)
    if (port->is_enabled)
    {
        MMAL_STATUS_T status;
        MMAL_BUFFER_HEADER_T *new_buffer;
        new_buffer = mmal_queue_get(BufferPool->queue);
        if (new_buffer)
            status = mmal_port_send_buffer(port, new_buffer);
        if (!new_buffer || status != MMAL_SUCCESS)
            printf("Unable to return a buffer to the video port\n");
    }
}

The first bit should be fairly simple - we check if the buffer has any data in (hopefully it always does!), and if so, lock it, call the users callback (remember they passed it into the Init function) so the application can have it's merry way with the frame data, then unlock it.

The next bit of code is a little more confusing. It's the second part of the buffer management stuff we saw earlier. Once the buffer is used, we first release it. This frees it up and effectively puts it back in the pool from whence it came! However, the video port is now down a buffer, so (if its still open), we pull the buffer back out of the pool and send it back into the video port ready for reuse.

What is interesting here is that we have control over when the buffer is returned to the video port. I can see down the line doing stuff with the gpu, where I extend the life time of a buffer over a frame so compute shaders can do stuff with it!

What's Next?

Well, I can now read data, at 1080p/30hz if I want but the next question is what to do with it! Currently it's in the funky I420 encoding (basically a common form where 1 channel represents the 'brightness' and the other 2 channels represent the colour). To be useful it'll need converting to rgb, and displaying on screen. I know from Pierre's work that opencv isn't ideal for this, so while I'll need it for proper image processing I think I'll have a look at faster ways to get from camera output -> me on the tv!

Pi Eyes Stage 1

I've got 2 new raspberry pi camera modules and 2 new rapsberry pis to go with them. Time to start hooking things up.

Setting up the Pis

First step I get my raspberry pis all setup how I like them. This is something I've done a few times now and should probably write down in detail for people, especially as the setup process these days is quite different to how it was with the earlier versions of the pi - maybe if enough people ask I will do :)

My ideal setup with the cameras included is:

• Raspberry pi
• Wireless usb dongle (TPLINK WN727N works out of the box with no additional power)
• Pi Camera Module
• Standard power supplies and cases
• Unpowered usb hub + cheap wired keyboard / mouse for the initial setup steps
• HDMI cable to plug into tv

Once everything is hooked up I proceed to:

• Boot the raspberry pis and use the nice 'N00B' interface to install Raspbian. The only modification I make in the config screen is to ensure camera support is on.
• Setup the wifi (or just plugin to network if using a wired connection)
• Assign a static ip address to each raspberry pi so my computer can find them easily

At this point I can disconnect the mouse and keyboard - everything else can be done from my pc via ssh. Now my setup looks like this:

I can connect to the pis using SSH (with the putty software). Now I proceed to install:

• TightVNCServer (for remote desktop access from pc)
• CMake (for compiling all sorts of things)
• Samba file sharing (so I can access the pi file system from pc)
• Synergy (handy if I want to run the pi on the tv but use mouse/keyboard from pc)

In the absence of any details from me on this, a few great web sites to look at are:

http://www.neil-black.co.uk/raspberry-pi-beginners-guide#.Umfbx_msh8E

http://simonthepiman.com/how_to_setup_windows_file_server.php

http://sonof8bits.com/running-synergy-on-raspberry-pi-raspbian-at-boot/2013/09

Testing The Cameras

It's pretty easy to test the pi cams as some software comes packaged to record videos / take stills and show the feed on screen. By typing into putty the command:

raspivid -t 30000 -vf

I tell the video feed to show on screen for 30s, vertically flipped as my cameras are hanging upside down!

And there's me taking a photo of cameras taking a photo of me taking a photo of the cameras....

Getting the camera in c++....

So far so good - both pis work, both cameras works and we're setup ready for development. Or are we? It turns out not really - current support for the cameras in terms of coding is extremely minimal. If what you want to do is write an app that regularly takes a snap shot and sends it somewhere then fine. I however am looking to read the camera feed in c++ and do some image processing with it and at the time of writing this is not an out-of-the-box task.

The core issue with the cameras in terms of coding is that they don't come with video4linux drivers, so no standard web cam reading software (opencv included) can just read from them. Clearly it's possible as the raspivid application does it, and the source code is available so we have somewhere to start. Fortunately a very clever and helpful chap called Pierre Raufast has already done a load of the digging, and his information is all up here:

http://thinkrpi.wordpress.com/2013/05/22/opencv-and-camera-board-csi/

As Pierre discovered, the raspivid application uses the mmal (multimedia abstraction layer) library to access the camera data and transfer the it to the screen or encode it as stills / videos. His steps (which I recommend at least reading) are in short:

• Install the raspberry pi camera module and get it going
• Download / build / install the userland code for raspberry pi - this includes all the latest source code for the raspivid application and libaries it needs. Can be found here: https://github.com/raspberrypi/userland
• Install opencv (and in his case the face recognistion library)
• Copy the raspivid code and create a new modified one that doesn't do any fancy stuff, and instead just grabs the data from the camera and shoves it into an opencv window

The key result of Pierre's work is in this file: http://raufast.org/download/camcv_vid0.c

Having gone through his steps and made a few tweaks, I eventually got the code running:

However it dies after a few seconds due to some unknown error (probably because the code is a little out of date) and doesn't do exactly what I need it to.

So my next plan - redo some of Pierre's work using the most recent raspivid application and see if I can come up with a nice tidy camera api.

OK, so after a bit of work I've...

• Stripped out everything to do with encoding from the latest raspvid
• Re-implemented some of Pierre's work to capture the memory from each frame

In other words, I've got a functioning program that can run the camera at 1080p, and access the memory for each video frame. Here's the very basic code (currently at 720p to speed up disk writing):

http://www.cheerfulprogrammer.com/downloads/camcv.c

This blog's got long enough for now, so I'll leave it there and write up my progress getting from this preliminary code into a nice camera api in the next installment.

Update on the pi bot simulator

It's been a little while and I've got a little further with my pi bot simulator. There's not a lot to describe on top my earlier plans. Basically I now have:

• A unity program that runs a simulation of a robot
• A unity plugin that runs a small tcp server. It stores some values such as 'motor speed', 'sensor value', 'servo position'.
• A python script that connects to the server to set values that unity reads for controlling the robot, or to retrieve values that unity stores from the 'sensors'.

My simulated robot now has 2 motors, 2 neck servos, 2 eye servos, front, back, left, right and bottom range finders and a couple of eye cameras. 

The server code is a bit of bog standard c++ use of sockets. It reads in a 4 byte message length followed by a text based command, interprets it and sends a response. The code is longer than I'd like in true c++ style, but the nice python interpreter is much more precise:

import socket
import struct

print("Running inputtest.py")

HOST = 'localhost'    # The remote host
PORT = 5152              # The same port as used by the server
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))

#handy recvall function to block until a fixed number of bytes are read
def recvall(sock,requested_bytes):
    total_data=bytes(0)
    while len(total_data) < requested_bytes:
        data = sock.recv(requested_bytes-len(total_data))
        total_data = total_data + data
    return total_data

#Recv a block of text as a msg with 4 byte length header
def RecvText():
    lenbytes = struct.unpack("<i",recvall(s,4))[0]
    data = recvall(s,lenbytes);
    return data.decode()

print("Begin client loop")
while True:
    #read in a message to send
    val = input("Please input something\n")
    if(val == "q"): #bail out if quit requested
        break;
    
    #get the number of bytes
    lenbytes = struct.pack("<i",len(val))

    #send the bytes and print out the response
    s.sendall(lenbytes)
    s.sendall(val.encode())
    print(RecvText())

s.close();

Using this little script I can send commands such as:

set motor0 0.5

This results in:

• The script posting the text "set motor0 0.5" to the server
• The servo decoding this and assigning 0.5 to the variable RequestedMotorPower[0]
• Unity querying the latest requested power for motor0 and assigning it to the motor joint in the simulation

Its the sort of thing that's much better described in video though, so here's a little demo:

Next up I'll getting those camera feeds through to python somehow, and get a slightly prettier python project going on with some proper modules to make the whole thing a bit more readable. In theory from there I should be able to run the very same scripts on the raspberry pi and have it controlling the simulation.

Pi Bot Simulator

I decided to begin a new robot recently, but first I've decided to simulate one. Why? Well a good few reasons....

• It's going to take some serious coding, and I wanted to make sure I was up to the task before spending time and money building it
• Making digital tweaks to the design is easier than making them once its built
• Programming on my nice shiny lap top is much easier than writing code and distributing it out to multiple raspberry pis while plugged into a robot!

So this'll divide up into a few areas which I'll detail here.

The Architecture

I'm writing the simulation in unity. Basically it involves:

• A basic physical model of the robot, in a simple physical world.
• Scripts to replicate the behaviors of the various components of the physical robot (i.e. a servo script that feeds input into a unity hinge joint).
• A central script that runs a tcp server, and takes commands from external programs to communicate with the robot components.

This basic model is designed to simulate how the raspberry pis work in the actual robot. Each Pi is an external program that communicates with a subset of the robot components (or other raspberry pis).

This diagram shows a simplified model of what I have in mind, with only the motor and central controllers (ignoring the vision and speech processors). Crucially, the controller programs will be cross platform applications that run the same code on pc or raspberry pi. The only exception will be that the communications layer on a raspberry pi will be talking to physical devices like an IO board, wheras on a pc it'll be sending commands over TCP to unity.

A unity example

This snippet from my code shows the servo logic. It has a current position and target position, and uses them to control a damped spring on a hinge joint. The result is similar to the real world servo scenario, in which you send it a signal to tell it which position to move to.

using UnityEngine;
using System.Collections;

public class Servo : MonoBehaviour {
    
    public float Position;
    public float TargetPosition;
    
    // Use this for initialization
    void Start () {
        Application.runInBackground = true;
    }
    
    // Update is called once per frame
    void Update () {
    
    }
    
    float ConvertToWithinPI(float angle)
    {
        while(angle < -180) angle += 360;
        while(angle > 180) angle -= 360;
        return angle;
    }
    
    void FixedUpdate()
    {    
        float a = ConvertToWithinPI(ConvertToWithinPI(TargetPosition) - ConvertToWithinPI(Position));
        if(a < -5) a = -5;
        if(a > 5) a = 5;
        Position += a;
        
        JointSpring spring = hingeJoint.spring;
        spring.targetPosition = Position;
        hingeJoint.spring = spring;
    }
}

I've written similar scripts for motors, and a central one to wire them all together in which I'll add the tcp communications layer.

It in action

So without further coding ado, here's a video of it in action:

This first version is just me tinkering with the numbers in the unity editor. Here you can see me fiddling with the eyes, neck joints and wheel motors.

Next Time...

Next up, I'll get it so that the inputs and outputs to the various simulated devices can be accessed via a network connection. Once I'm there I'll be in a position to write applications in any language I like to control the various aspects of the robot (probably python for the low power stuff, and c++ for things like image processing).

Introducing PiBot

Well I'm coming to the end of a project at work, which means I'll finally have some of that free time stuff I've heard so much about, and will need a project at home to use it all up. And so, here's the beginnings of my thoughts on my next robot...

The basic idea is to build upon my prototypes made with MmBot back in the day, the objective being to create an AI driven 'cute' robot. My experience making games has taught me it's not about making something intelligent, it's about making something appear intelligent, and that will be the goal. Something that is cute looking and makes an apparent emotional connection to it's surroundings.

Before I get all AI on anybody though, we need a basic system design. The plan is to network together a set of raspberry Pi's, talking a common language to each other via a battery powered Ethernet hub. Initial layout is as follows:

Layout of processing units in Raspberry Pi

The arrows here indicate data flow, not necessarily physical connections, as the connections between the raspberry pis (in green) are all done across the network. So in reality, all the Pis are connected to a central Ethernet hub. I'll also have a port spare in the hub for a PC to connect to for development purposes.

Crucial to the design of the system is the extendability of it. I have no idea right now as to the exact processing power required to achieve my goals, but by designing the system as a set of CPUs running in parallel, communicating over a network, I can add new modules as required.

Vision

It's crucial that this robot has a good awareness of it's surroundings. In order to make any kind of emotive connection it'll need to be identify people and make eye contact with them, or wander around a room or building trying to 'make friends'. As such a full 3D representation of the scene will be necessary. This'll be obtained by:

• 2 raspberry pi cameras
• A raspberry pi for each camera, doing initial 2D vision processing. This will be conversion of the images into Sobel transforms, converting them to useful formats for 3D processing, and performing 2D feature detection.
• The stereoscopic processor will receive data from the 2 vision processors, and combine it to form a 3D view of the scene, primarily through matching features from the 2 separate cameras and data from previous frames. It'll  also be responsible for feeding back requested eye movements to the central controller that are necessary to enhance knowledge of the scene layout.

It's my hope that these systems will provide the power needed to build and maintain an understanding of the environment the robot is in, however large parts of the image processing and stereoscopic work are parallelisable, so could be distributed across more machines if necessary. The raspberry pi has a tasty GPU though which could well be harnessed for this purpose.

For recognising things such as faces in the scene, I may offload additional work to an extra raspberry pi, to take data from the stereoscopic data and match it to historical records of people, or even image data retrieved from the internet!

Audio

This comes in 2 forms - input and output. I'd like the robot to be able to understand some very basic speech commands, but also respond to audio queues such as loud noises being scary. For feedback, I intend some sort of sim speak or emotive gobbledygook language (think R2D2), as you can achieve a lot with this kind of sound without it really having to make any sense!

Currently I'm expecting not to need too much power to achieve the audio goals, so 1 raspberry pi is reserved for both input and output. This could easily be farmed out to more CPUs if necessary though.

Additional IO

All extra IO will go via a raspberry pi 'Gert Board' - an extension available designed for driving motors and reading sensor input. 

The robot will be driven by 2 fairly powerful motors, each with a built in quadrature encoder (aka thing that measures how far the motors have turned), which will allow for precise control over its movement.

PiBot's head will be on a neck controlled by 3 servos to give a full range of motion, plus an additional servo to allow the eyes to move in unison (crucial for realistic and cute eye contact). I could see me needing additional servos for controlling eye brows or mounted sensors, but they aren't on the plan just yet.

I'll be adding various LEDs or light strips on PiBot to allow it communicate 'mood' with colour (think Ian M Banks culture robots), again as its simple to code but powerful in terms of emotive feedback. 

The only extra sensors I have planned are rangefinders (probably IR) mounted at various positions to give the robot some last minute warning systems against crashing into walls or rolling down stairs!

WiFi

I'd like the robot to be able to access the internet, to retrieve data from systems like face book in order to glean any extra info it can about the world around it or the people it's communicating with. In addition, a quick and easy way to connect to it with a PC will be handy so for this I'll be adding a WiFi network adapter connected to the central processor.

HDMI

Not sure what this is for yet, but I'm sure some kind of output to a display will be useful for debugging - or maybe playing back what it's been doing. Plug PiBot into the TV after its been wandering around for a day and see what it's been up to!

Summary

Well that's the basic plan - a set of networked raspberry pis all doing their own little jobs, with a central unit talking to them all to gather up sensor data and feedback to the output devices. Exciting stuff - just gotta ship Tearaway first!