Lab: Examining Vision Results

From Tekkotsu Wiki

Jump to: navigation, search



Learning goal: This lab will show you how to get your robot to act on the shapes that it sees. The preceding lab showed how to tell the MapBuilder to look for shapes in the camera image. The MapBuilder puts them in the camera shape space, camShS, which you can manually examine using the SketchGUI. Now you will learn to automate this process by writing code to examine the shape space.

Shape Vectors

The NEW_SHAPEVEC macro creates a vector of shapes of a specified type, such as EllipseData or BlobData. We can extract these shapes from the camera shape space using the templated function select_type<T>, where T is the type of the shape we're looking for. This method takes a vector of ShapeRoot objects as an argument. ShapeRoot is the parent class for all shape objects. We can obtain this vector of shapes by writing camShS.allShapes(). However, Tekkotsu includes an implicit type coercion that automatically coerces a shape space to a vector of the shapes it contains, which allows you to just write camShS. Thus, to extract the ellipse shapes the MapBuilder has found, you would write:

NEW_SHAPEVEC(myEllipses, EllipseData, select_type<EllipseData>(camShS));

If Tekkotsu did not include this implicit type coercion feature, you would instead have to write:

NEW_SHAPEVEC(myEllipses, EllipseData, select_type<EllipseData>(camShS.allShapes()));

After executing this line, myEllipses will be a vector of type std::vector<Shape<EllipseData> >. You can reference the first ellipse by writing myEllipses[0]. But what if the MapBuilder didn't find any ellipses? Then myEllipses will be empty, and myEllipses[0] will generate a runtime error that crashes your program. Therefore you should always check that myEllipses.size() is positive before trying to access the first element.

Let's write a test program to look for ellipses and report the first one the MapBuilder finds. Note that the code to examine the shape space cannot be placed inside the MapBuilderNode. The job of the MapBuilderNode is to prepare a request to the MapBuilder and then submit that request. We must wait for the MapBuilder to finish its work before we examine the shape space for the results. Therefore, vision in Tekkotsu is done in two steps: first ask the MapBuilder to look for something, then examine the shape space to see what it found. Here is our test program:

#include "Behaviors/StateMachine.h"

$nodeclass ShapeTest1 : VisualRoutinesStateNode {

  $nodeclass LookForEllipses : MapBuilderNode : doStart {
    mapreq.addObjectColor(ellipseDataType, "red");
    mapreq.addObjectColor(ellipseDataType, "green");

  $nodeclass ExamineResults : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(myEllipses, EllipseData, select_type<EllipseData>(camShS));
    if ( myEllipses.size() > 0 )
      cout << "The first ellipse found is " << myEllipses[0]
           << " at " << myEllipses[0]->getCentroid() << endl;
      cout << "Found no ellipses!" << endl;

    LookForEllipses =C=> ExamineResults




  1. Compile the above behavior on your robot.
  2. Set out some round things that the robot can see as ellipses, or use Mirage with a world containing some round things.
  3. Run the ShapeTest1 behavior and see what results you get.
  4. Do the coordinates reported by your program match what you see in the camera SketchGUI?
  5. Hide all the ellipses (or turn the robot) and run the behavior again. What do you expect will happen?
  6. What determines which is the "first" ellipse in the vector? Do some experiments to figure this out.
  7. The select_type function is found in the file Tekkotsu/DualCoding/ShapeFuns.h. What similar functions are defined in that file?
  8. Remove the safety test from ShapeTest1 and run it with no ellipses visible. What does the crash look like? Show how you can tell where the error occurred by reading the crash dump.

Finding A Shape

You can use the find_if function to find a shape that meets certain criteria. There are several forms of find_if, some of which take a shape type as a template argument. For example, to find a single ellipse you could write:

Shape<EllipseData> someEllipse = find_if<EllipseData>(camShS);

Another version of find_if accepts a predicate functor and returns the first item in the shape vector for which the predicate returns true. To find the first green ellipse, we could write:

Shape<EllipseData> greenEllipse = find_if<EllipseData>(camShS, IsColor("green"));

Yet another form of find_it accepts a vector of Shape<T> as argument, instead of a vector of ShapeRoot. This version doesn't require an explicit template argument because it gets the template information from the input shape vector. If you've extracted a set of ellipses with select_type, you can use this version of find_if to search the results for a particular ellipse:

NEW_SHAPEVEC(myEllipses, EllipseData, select_type<EllipseData>(camShS));
Shape<EllipseData> redEllipse = find_if(myEllipses, IsColor("red"));


  1. The IsColor functor is defined in Tekkotsu/DualCoding/ShapeFuns.h. What other predicate functors are defined in that file?
  2. Define an IsRed() class whose instances return true if their argument is red, so that you can say find_if(myEllipses, IsRed()). The IsRed class should inherit from IsColor.

Invalid Shapes

The find_if functions always return a Shape<T>. But what happens if find_if doesn't find what it's looking for? In that case it returns an invalid shape, which is similar to a null pointer. If you try to dereference an invalid shape, a runtime error occurs and your program will crash. Therefore you must use the isValid() method on the result of find_if to check whether it did find something.

The example program below speaks the shape id number of the first green ellipse it finds. If there are no green ellipses, it recognizes that find_if returned an invalid shape and reports that no green ellipses were found.

#include "Behaviors/StateMachine.h"

$nodeclass ShapeTest2 : VisualRoutinesStateNode {

  $nodeclass LookForEllipses : MapBuilderNode : doStart {
    mapreq.addObjectColor(ellipseDataType, "green");

  $nodeclass SpeakResults : SpeechNode {
    NEW_SHAPE(firstGreen, EllipseData, find_if<EllipseData>(camShS, IsColor("green")));
    cout << "firstGreen is " << firstGreen << endl;
    if ( firstGreen.isValid() )
      textstream << "First green ellipse has id " << firstGreen->getId();
      textstream << "No green ellipses found";

    LookForEllipses =C=> SpeakResults




  1. Run the ShapeTest2 behavior and test the cases where a green ellipse is visible, and where no green ellipse is visible.
  2. From where is the getId() method inherited?
  3. From where is the isValid() method inherited? (Hint: it's a method of the Shape<EllipseData> object itself, not a method of EllipseData. There is no EllipseData object when a shape is invalid.)
  4. Remove the isValid() test from ShapeTest2 and try it with no ellipses visible. What do you expect to happen?
  5. If you know how to debug code using gdb, run your behavior under gdb and, when the crash occurs, print out the value of firstGreen. What does it look like? (To learn how to use gdb, see Lab: Debugging with gdb.
  6. Tekkotsu's find_if function was inspired by the find_if function in the STL (Standard Template Library). What type of value does the STL's find_if return? What does it return if it can't find an object that satisfies the predicate?

Collections of Shapes

Frequently you'll want to iterate over an entire vector of shapes instead of just processing the first element. You can do this with the usual STL iterator methods if you're familiar with them. But Tekkotsu provides some syntactic sugar for this common operation that eliminates the need to deal with iterators explicity. It's called the SHAPEVEC_ITERATE macro. Here's an example that prints the semimajor axis length of each ellipse the MapBuilder found:

NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));

SHAPEVEC_ITERATE(ellipses, EllipseData, e) {
  cout << "Ellipse " << e->getID() << " has semimajor axis length " << e->getSemiMajor() << endl;

Many algorithms require finding a distinguished element of a collection, such as the longest line or the ellipse with the greatest area. An easy way to do this is to sort the collection, then take the first element of the result. Tekkotsu provides a stable_sort method, inspired by the STL method of the same name. But Tekkotsu's version operates on vectors of shapes and takes a binary shape predicate as its second argument. For ellipses there is a predicate functor called AreaLessThan that will do what we want. Here is example code:

Shape<EllipseData> findBiggestEllipse() {
  NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
  NEW_SHAPEVEC(sortedEllipses, EllipseData, stable_sort(ellipses, not2(EllipseData::AreaLessThan())));
  Shape<EllipseData> biggestEllipse;
  if ( sortedEllipses.size() > 0 ) {
    biggestEllipse = sortedEllipses[0];
  return biggestEllipse;

There are several things to note in this code.

  • In order to sort the ellipses in descending order by area we had to negate the AreaLessThan predicate, which we did using not2().
  • Instead of using NEW_SHAPE to declare biggestEllipse, we used a simple Shape<EllipseData> declaration. This initializes biggestEllipse to an invalid shape. Then, if sortedEllipses is not empty, we assign sortedEllises[0] to biggestEllipse, replacing the invalid shape with a valid one.
  • Ellipse shapes created by the MapBuilder have a default name of "EllipseData". We use a call to setName to change the name of the selected ellipse to "biggestEllipse", so that we can see it in the SketchGUI. The NEW_SHAPE macro makes this setName call automatically if the shape is valid. Thus, another way to achieve the same effect as the last 5 lines of code would be to write:
NEW_SHAPE(biggestEllipse, EllipseData,
          sortedEllipses.size() == 0 ? Shape<EllipseData>() : sortedEllipses[0]);

Another handy function for dealing with collections of shapes is subset. Like find_if, subset is modeled after the STL function of the same name. The subset function is similar to find_if except instead of returning the first element that satisfies a predicate, it returns a result vector containing all the elements that satisfy the predicate. Here is how to find all the green ellipses:

NEW_SHAPE(greenEllipses, EllipseData, subset(ellipses, IsColor("green")));


  1. Write a behavior to select the two longest lines the robot can see and report the difference in their lengths. Check the documentation for LineData to find the appropriate predicate to use for sorting.
  2. Define a predicate functor for ellipses to compare the lengths of their semimajor axes.
  3. Write a behavior to find the longest line the robot can see, and then find the longest line that is parallel to that line. Rename the two lines to "longest" and "parallel", respectively.

Heterogeneous Collections of Shapes

ShapeRoot is the parent class for all the shape types, such as Shape<LineData> and Shape<EllipseData>. If you want a function to be able to return any type of shape, it must return a ShapeRoot. Similarly, if you want a vector of shapes of mixed type, it must be defined as a std::vector<ShapeRoot>. This is in fact what camShS.allShapes() returns.

Tekkotsu provides macros called NEW_SHAPEROOTVEC and SHAPEROOTVEC_ITERATE for dealing with vectors of ShapeRoots. These macros don't require a shape type argument because they know that the type is ShapeRoot. An example will be given shortly.

Most of the interesting shape methods are shape-specific, so if you want to compute with a ShapeRoot you'll need a way of checking its type and coercing it to its proper subtype. Checking the type can be done with the isType method or the IsType functor. Macros called ShapeRootType and ShapeRootTypeConst can be used for the coercion; the latter produces a const Shape<T> instead of a Shape<T>. Here is an example of computing with ShapeRoots:

NEW_SHAPEROOTVEC(myShapes, camShS.allShapes());
  cout << "I see a shape: " << s << " with length ";
  if ( s->isType(lineDataType) )
    cout << ShapeRootTypeConst(s,LineData)->getLength();
  else if ( s->isType(ellipseDataType) )
    cout << 2 * ShapeRootTypeConst(s,EllipseData)->getSemiMajor();
   cout << "unknown";
  cout << endl;


  1. Write a behavior to find all the shapes (lines or ellipses) to the left of the largest red ellipse. You will need the IsLeftOfThis predicate defined in Tekkotsu/DualCoding/ShapeFuns.h.
  2. Define a node class that finds the longest line in the camera shape space and then reports all shapes (of any type) in the shape space that appear to the left of that line.