The ARPool 2.0 Re-write

After our successful but not stress free demonstration of ARPool at the Ontario Centers of Excellence Discovery Show I took it upon myself to make a rather large structural change to the ARPool program in an effort to avoid similar problems in the future. I wanted to change ARPool from an event driven program to what I refer to as a state machine, a technique I learned through my involvement in First Robotics.

If you read my last blog post on our experience at OCE you’ll know that one of our main problems with the system was difficulty understanding the order in which code was called and our lack of control over changing this process. The ARPool system relied on several timer events as well as signals and slots in an interrupt fashion to get things done in the right order. If things are not executed in the right order then the system simply has no chance – for example if the projector doesn’t illuminate the table before we try to detect the pool balls then it won’t be able to find them. We had many problems with the system becoming out of sync and the whole experience felt rather unstable even after the system was working.

Now don’t get me wrong it’s not that I think event driven programs are bad – they aren’t and in many applications it is the only possible way to make something work, it’s just that in the case of ARPool it doesn’t make any sense. ARPool is a program that only needs to do one thing at a time and it all needs to happen in the right order – so why bother with all the event driven stuff? The final point is that being a University lab we don’t have the resources to maintain a system that complicated and many people who have and will work on the system don’t have the required background to effectively work on such a system. ARPool needed an overhaul.

Introducing the state machine: It is really quite a simple idea - there is a main loop and inside this loop there is a single switch case block. The switch is executed on a “state counter” variable which for extra points should be made an enum with relevant state names. Inside each state (a case block) the program does its thing and then if appropriate changes the state counter to the next state or leaves the state variable as it is. Here is a simple example:

enum State
{
  detectBalls,
  trackCue
};

State state = detectBalls;

while(true)
{
  Switch(state)
  {
    Case(detectBalls):
    detectBalls();
    state = trackCue;
    break;

    Case(trackCue):
    // code
    break;

    // and so on…
  }
}

It’s probably not the first you thing you would think of but after you try it out it’s a pretty nice way to do things. I picked up on this approach earlier this year while working on programming the CRio device for First Robotics – see First Robotics has these built in safety functions around your robot class if your robot’s main function doesn’t finish executing at least several times a second it assumes that you’ve either lost communication or wrote an infinite loop. You’re basically forced into this state machine idea if you want to do anything complicated.

Anyways First’s API and safety functions are a topic of a different rant how did all of this turn out for ARPool? Well surprisingly well it only took me about 2.5 hours of straight coding to completely pull out the Qt gui, remove all the event driven code and re-write the main program to utilize the state machine concept. It compiled and the code was way cleaner, shorter and I could finally see where and in what order my vision functions were called. All in all a great victory – however it was time to return to my other projects and leave my shiny new ARPool program sitting there waiting to be tested on the real system.

*edit* 09/01/2012 I finally got my shot to try the new ARPool code, it actually worked! Every programmer has a few moments they are proud of and this was definitely one of mine, re-structuring an entire program (like major major stuff here) not being able to test it for months and it actually just worked! Not every day gets to be like this.

ARPool at OCE Discovery

ARPool (Augmented Reality Pool) was a project I was always very interested in getting involved with so when my supervisor mentioned he need a few of us to get the system ready for a demo I jumped on the opportunity. We were going to be taking ARPool to the Ontario Centers of Excellence Discovery Show – it is kind of a mix between a trade show and a conference poster session. The Queen’s marketing guys were also super excited because they felt Queen’s was out done the previous year and ARPool was our show stealer.

The first thing we had to do was update the travelling version of ARPool, what we needed was an enclosure to keep it dark ish around the system – if it is too bright then the augmented reality projections onto the table are tough to see. We also needed to build a new mount for the projector and camera, our supervisor wanted to try straight projection rather than off of a mirror so it needed to be higher up. We experimented with a few ideas and set up the system in Beamish Monroe Hall and ran a successful demo for Queen’s Admitted Students Day.

At this point I hadn’t had too much exposure to the code behind ARPool but it was already clear that the system was not what I would call rock solid. Near the end of the demonstration the ball detection algorithm was not finding all the balls on the table – fixing the ball detection algorithm would be my first contribution to ARPool. Having learned that testing new algorithms on a live system lacks the quantitative feedback that is necessary to get the best results I took a handful of test images and headed back upstairs to my lab to tackle the issue. A quick look at the old detection algorithm and it was easy to see how the system was suffering from having multiple inexperienced developers. The algorithm went through 4 iterations of finding contours, then drawing the contours to a new image albeit with some filtering each time (area, shape etc.) and then running find contours on this image. I would sincerely hope that the find contours algorithm can find the contours you just drew…

I rewrote the ball detection routine, tested it on my data set and then when I was satisfied brought it back downstairs, ball detection back online. Next I tackled the Background subtraction code – it was actually pretty good but I was able to speed up save and load times dramatically by switching to cv::FileStorage from plain text. We added a simple smoothing filter to the cue tracking just to make the system appear less jumpy to users. The system was working pretty well although there were a few design choices I didn’t care for I thought we were in good shape for OCE.

Raw background subtraction output
Filtered binary image using contour extraction
detected pool balls overlaid on the input image

We packed it all into a single cube van which was seriously pushing it space wise and took off for Toronto. We spent most of the first day schlepping everything from the parking lot to the conference hall and then setting up the structure and pool table etc. We started into the calibration process and while it wasn’t smooth it wasn’t much worse than usual. Calibration finished enter the big problems… We load up the version of the code we had finished with at Queen’s and it isn’t working – wait what this code just worked!? Things are happening out of sync and we can’t figure out why, this is where my frustration with the way the system is set up begins. We eventually load up the code from before we fixed all the various algorithms and it is at least running in the right order… so we slowly add back one feature at a time until the system is working as we left it. I don’t like this – we never figure out the problem and this seems unstable. Oh and by the way it is now the next day at 1:00 the event is starting we got it running just in time. The system runs great for 4 hours straight we’re all a little surprised frankly. We demo to a lot of interested people mostly other demonstrators at the event (I’m still not totally sure I really understand the point of this event). We had back to the hotel after the reception which by the way was pretty fun oh and did I mention the Queen’s put us up at the Royal York (it’s pretty swanky). After dinner we went to a Jay’s game to relax for the evening.

The next morning (and this is the more important day of the show) we arrive and boot up ARPool – aaaannnd it’s not working today seriously this system is moody we ran it 4 hours straight no issues yesterday and now it’s deciding not to work again. I don’t remember exactly how but I do remember that I was the one who fixed it – I added or changed some piece of code that was completely trivial and then the system started working again – hmmm whatever I just want to get through this day now.

Got the system up and running and the day was a success – pics or it didn’t happen!

Our demo booth
A conference attendee playing ARPool

So what was the issue? Well ARPool is an event driven program written in Qt using signals, slots and the works including several timer events. Our best guess is that sometimes they would get our of sync. We had worked really hard on improving the algorithms but what really got us in trouble was the system layout and how tough it was to see and control the order code was executed. The higher level stuff was really poorly organized and documented for example there were 2 functions that were very key to the operation one called “run” the other “run_again” uh yeah that is not how we do things.

All in all a successful but certainly not stress free trip. I vowed to fix the system layout of AR Pool before our next demo because I did not want a repeat of OCE - that and I’m an engineer and I have the problem solving bug and this was a problem screaming to be solved and I already had an idea how…

FRC Rebound Rumble Vision System

One and a half weeks until we were heading to the First Robotics World Championship in St. Louis and our vision targeting system needed some serious TLC. At our last regional we swapped out our old system which was running on board the CRio with a new vision system that I had written using the smart dashboard which allowed us to offload the vision system to the driver station. The new vision system was way better for a number of reasons but admittedly the performance of the vision system was about the same.

In addition to changing where the vision processing was done I also totally changed the algorithm to use higher level OpenCV functions as opposed to the old system which was a crude stride detector operating on raw pixels implemented by another mentor. For worlds there was some debate over which approach was better and frankly despite what many people may have thought we really had no idea how well or not well either approach actually worked.

I’ve come to notice a common pitfall when dealing the so called “real” systems people tend to make small changes and then watch it work once and think they have fixed or improved it. I understand why there is a ton of temptation to simply try your new idea and this isn’t wrong just sometimes we need to take a step back and do it properly. This was one of those times and it presented a great opportunity to teach the students about proper testing, ground truth and of course explain this pitfall.

So I headed to the warehouse where our field was setup and using a simply extension to the smart dashboard that I wrote I collected 1000 images of the vision targets at various orientations and angles. The vision target for reference:

You wouldn’t think that finding a giant glowing green rectangle would present much of a problem, I mean I wish all computer vision problems were this simple, but none the less there are some details which make it tricky. The net likes to obstruct the rectangle causing a break in it – this can be easily fixed with morphological operators but this can get you into trouble by joining the target stadium lights from above. TL;DR yeah it’s easy but still not trivial.

So what am I going to do with all these data it isn’t labelled - oh wait don’t I mentor a team of 40 high school students? commence image labelling sweat shop! Seriously though we had all hands on deck for this one, I made a simple procedure to follow and then everyone helped label the data.

Truth Labelling is fun!

Fun times! Check out the full blog post from KBotics for more pics:

http://kbotics.ca/2012/04/05/image-processing-party/

I took the labelled data and created a program to test the algorithms on – first run it was pretty clear my new algorithm found the target in ~850 of the images while the old approach found 3… After some parameter tweaking I was able to get my algorithm to detect 997 of the 1000 - notbad.jpg!

Here is a link to the github repo for our final vision system: github.com/pickle27/2809_vision2012.git

Next KBotics meeting I presented my findings and taught all the students about how our vision system works, I like to think they learned a thing or two ;)