Lab Tutorial 5 - Using Vision to Estimate the Distance to an Object

In this tutorial you will learn how a simplified camera model can be used to estimate the distance between a robot's camera and an object.

This lab is provided in the language. You can find the project files for this lab at the following location under the Microsoft Robotics Developer Studio installation folder:

 Samples\Courseware\Introductory\Lab5

This tutorial teaches you how to:

  • Use the Color Segment Service.
  • Make a Distance Estimation Activity.
  • Connect the Activity with ColorSegment.
  • Set the ColorSegment manifest.
  • Generate C# Code.
  • Examine generated code.
  • Change the input a service can receive in C#.
  • Post the List of Color Areas.
  • Add configurable parameters in C#.
  • Code the distance calculation.
  • Configure your service.
  • Test your service.

See Also:

  • Camera Models
  • Pinhole Camera Model
  • Focal Length
  • Computing the distance to an object
  • Getting Started

Prerequisites

Hardware

This tutorial requires a video camera, such as a simple web-cam.

Software

This tutorial is designed for use with VPL and Microsoft Visual C#, it requires Robotics Developer Studio.

Camera Models

When a camera captures an image it is essentially producing a 2D representation of a 3D space. We will call the camera’s representation the image plane. A camera model simulates this process so that we can reason about the relationship between the image space and the 3D world.

Pinhole Camera Model

The pinhole camera model is one of the most widely used in robotic vision. In this model all the rays of light from the scene are assumed to pass through a pinhole. The pinhole is the single point from which the light rays from the scene are refracted and projected onto the image plane. The object in the image plane is inverted.

Pinhole Camera

Pinhole Camera - all the light rays pass through a single focal point and are projected onto the image plane.

Focal Length

The focal length is the distance between the pinhole (the camera’s lens) and the image plane. As shown in the following figure.

Pinhole Camera Model

Pinhole Camera Model - this diagram illustrates some important components in the 2D representation of the Pinhole Camera Model, including the Focal Length.

Computing the distance to an object

We can conceptually move the image plane to obtain the Inverted Pinhole Camera Model, where the image is no longer inverted. From the diagram below, it is easy to see a relationship between the size of the image on the image plane and the distance between the object and the pinhole. We know the equality shown in the diagram holds because the triangles are similar. Thus, if we know the height of an object, and the focal length, we can use this relationship to determine the distance between the camera and the object simply by calculating the height of the image in the image plane.

Inverted Pinhole Camera Model

Inverted Pinhole Camera Model - The equality shown holds because the small triangle and the large triangle containing it are similar.

In the image plane we measure the height of an image in terms of pixels. This means our measurement for the focal length must also be in terms of pixels. It is relatively easy to find out the focal length of the camera you are using in millimeters. This value is often given in the documentation that comes with the camera, or in the specifications that can be found online. Note however, that to use the equation we derived we need the focal length to be in terms of pixels. If it is not provided in the documentation of the camera, you can work this out if you know the pixel density. (There are online calculators that help you to compute the focal length and you can find the algorithms that they use in vision text books.)

Once you have an estimate for the focal length of your camera in pixels, you are ready to get started on this tutorial.

Getting Started

In order to get information about the scene viewed by the camera, we are going to utilize the ColorSegment service. We will setup the partnership between our service and this service in VPL. In a future tutorial you will learn how to setup partnerships between services in C# without the aid of VPL's code generation feature.

To get started open up VPL.

Step 1: Add the Color Segment Service to your diagram

From the Services panel, add the ColorSegment service to your diagram. You learned a bit about this service in the previous tutorial. From ColorSegment you can receive periodic notifications about what color blobs are seen by a robot's camera. ColorSegment can return a List of type ColorArea. A ColorArea contains information about an individual color blob. This information includes:

  • Area - number of pixels
  • Cordinates - MinX, MaxX, MinY, MaxY, CenterX, CenterY
  • Name - the name of the color

Step 2: Make the Distance Estimation Activity

Next, we are going to create a new activity which will become our simple distance estimation service. Add a new Activity from the Basic Activities pane. Name your activity in the properties pane something appropriate, we called ours SimpleEstimateDistanceToObject. Double click on your new activity to open it. We will now add an input and an output value. The input that we want to pass in is a List of type ColorArea. However, ColorArea is not a type in VPL, so we will just make an int input value called Field as a placeholder. The function of our SimpleEstimateDistanceToObject is to return the distance to an object in the image, so the output value should be a double which we will call Distance.

Step 3: Edit the Activity

We are going to add some code to the activity. We won't actually use this code later, but it will be instructive to see where it ends up when we run the code generation. Inside SimpleEstimateDistanceToObject add a varible of type int called TestVar. Next we will add a Calculate box connected to the activity input. One way to do this is to right click ont he activity input and then drag the cursor to where you would like the box to appear. When you let go of the mouse-button a drop down menu will appear. Select Calculate from this menu. Inside the Calculate box enter Field to get the input to the activity. Now set TestVar to the value of Field.

Finally, add a Data block containing the value 0.0 and connect it to the output of the Variable block and the output (square) of the activity. Make sure you set the Distance returned by the activity to be the value coming out of the Data block. To double check you have set this correctly, you can select the link between the data block and the output, and then look at the properties for the link, which will be displayed on the right-hand side.

Activity

Activity - your activity should now look like this.

Step 4: Connect the Activity with ColorSegment

We want to act on the UpdateColorAreas notification from ColorSegment. In particular, every time we get this notification we would like to pass the List of ColorAreas to SimpleEstimateDistanceToObject. As we already mentioned, we can't do this in VPL, so instead we will pass the size of this list. This will make the generated code easy for us to change later.

Connect the notification port of ColorSegment to the input of SimpleEstimateDistanceToObject and select UpdateColorAreas as the notification. Then in the Data Connections properties make the Valuevalue.Areas.Count - this is the size of the List of ColorAreas (note, typing the shortcut Areas.Count is equivalent).

We also need to get the Distance returned by the SimpleEstimateDistanceToObject activity. If we set a variable to this value we will be able to easily examine the changing value of the variable in a web-browser when we run our program. This will allow us to easily test our SimpleEstimateDistanceToObject service.

Attach a Calculate block to the output of SimpleEstimateDistanceToObject to get the Distance returned. Use the value calculated to set a new Variable block in your diagram of type double called Distance.

Diagram

Diagram - Your main diagram should now look like this.

Step 5: Select the manifest for ColorSegment

To set the manifest for ColorSegment click on it so you can edit its properties. Under Configuration select Use a manifest and import the ColorSegment.manifest.xml form samples\config.

Step 6: Generate C# Code

It is now time to generate C# code from your diagram. Go to the Build menu and select Compile as a Service. You will be asked to select a directory for your files. It is convenient to choose the same directory as where your diagram is stored.

Step 7: Get a feel for the Generated Code

Go to the directory where you put the generated code and open the project file (extension .csproj) in Visual Studio. You will notice four .cs files have been generated.

  • DiagramService.cs
  • DiagramTypes.cs
  • SimpleEstimateDistanceToObjectService.cs
  • SimpleEstimateDistanceToObjectTypes.cs

If you take a look at DiagramService.cs you will notice that it sets up the partnerships with ColorSegment and SimpleEstimateDistanceToObject. In the Start method it subscribes to the notifications from ColorSegment which it will pass onto SimpleEstimateDistanceToObject. Now expand the code regions - Notification Message Handlers and OnColorSegmentUpdateColorAreasHandler class. Examine the code to see where Areas.Count gets extracted from the incoming port and posted onto the SimpleEstimateDistanceToObject (note that the class JoinAlpha plays a part in this. Also look at how the Distance response is handled.

In DiagramTypes.cs you can see where the variable Distance that you defined in your diagram is declared inside the DiagramState class. If you wanted to declare more variables in this service you could follow this pattern.

Next, take a look at SimpleEstimateDistanceToObjectService.cs. Expand the ActionMessageHandler class region. Inside the method RunHandler is where the variable you created TestVar is assigned to the Field input. The Data value is posted by the line _joinAlphaPorts[0].Post(0);. The decimal point was removed by the code generation. If you look down further at the JoinAlpha class, you can see the definition of the output which we called Distance.

Finally, open SimpleEstimateDistanceToObjectTypes.cs and look at the definition of TestVar. When we write the code for our service we will be declaring a number of variables (as C# properties) in the same way that TestVar is declared. Expand the Custom Message Types region to see definition of the input, Field, and the output, Distance.

Step 8: Change input SimpleEstimateDistanceToObject can receive

We are going to start by changing the input the SimpleEstimateDistanceToObjectService can receive from being an int to being a List of type ColorArea. Recall that the input for SimpleEstimateDistanceToObjectService is defined in SimpleEstimateDistanceToObjectTypes.cs. Start by adding the following using directive so the type ColorArea is available: using colorsegment = Microsoft.Robotics.Services.Sample.ColorSegment.Proxy;

Next, carefully replace the Field input by a variable of our desired type. We called ours RecognizedAreas. Your code should appear as follows.

 List<colorsegment.ColorArea> _recognizedAreas;
[dssa.DataMember]
public List<colorsegment.ColorArea> RecognizedAreas
{
    get { return _recognizedAreas; }
    set { _recognizedAreas = value; }
}

Step 9: Post the List of Color Areas instead of the List Count

In DiagramService.cs in the method RunHandler instead of posting Areas.Count post Areas instead.

 public IEnumerator<ccr.ITask> RunHandler(colorsegment.UpdateColorAreas message)
{
    Increment();

    Increment();
    _joinAlphaPorts[0].Post(message.Body.Areas);

    Decrement();

    yield return WaitUntilComplete();
}

To do this we also need to edit the JoinAlpha class in DiagramService.cs. Instead of using the int Field, you need to change it to use a List of type ColorArea.

 class JoinAlpha
{
    public List<colorsegment.ColorArea> RecognizedAreas;

    public JoinAlpha()
    {
    }

    public JoinAlpha(object[] args)
    {
        RecognizedAreas = (List<colorsegment.ColorArea>)args[0];
    }

    public static explicit operator SimpleEstimateDistanceToObject.ActionRequest(JoinAlpha join)
    {
        SimpleEstimateDistanceToObject.ActionRequest message = new SimpleEstimateDistanceToObject.ActionRequest();

        message.RecognizedAreas = join.RecognizedAreas;
        return message;
    }
}

Step 10: Declare the parameters used by your service

We are now going to define all the initial parameters the distance estimator service needs to do its calculation. If we declare these as properties like TestVar using the [dssa.DataMember] attribute in the SimpleEstimateDistanceToObjectState users of our service will be able to set them to the appropriate values using an xml configuration file. We need to declare variables for the following parameters.

  • Object height - the height of the object in mm.
  • Focal length - the focal length of the camera in pixels.
  • Color - the name of the color of the object (the ColorSegment service we are using needs to be configured to detect this color).
  • Ignore constant - if a ColorArea has less than this number of pixels we should ignore it.

We can declare these variables according to the same pattern that generator used to declare TestVar in SimpleEstimateDistanceToObjectTypes.cs. Let's start by making the following declaration: double _objectHeight;. Once you have done this, you can get Visual Studio to generate the get and set for you, by highlighting the declaration, right-clicking, and selecting Refactor and then Encapsulate Field. From here if you just accept all the options Visual Studio will generate the code. Now all you need to do is add the attribute line: [dssa.DataMember]. Follow this pattern for each variable, to produce the code below.

 [dssa.DataContract]
public class SimpleEstimateDistanceToObjectState
{
    int _testVar;
    [dssa.DataMember]
    public int TestVar
    {
        get { return _testVar; }
        set { _testVar = value; }
    }

    //ball height in mm
    double _objectHeight;
    [dssa.DataMember]
    public double ObjectHeight
    {
        get { return _objectHeight; }
        set { _objectHeight = value; }
    }

    //focal length in mm
    double _focalLength;
    [dssa.DataMember]
    public double FocalLength
    {
        get { return _focalLength; }
        set { _focalLength = value; }
    }

    //number of pixels to ignore
    int _ignoreNum;
    [dssa.DataMember]
    public int IgnoreNum
    {
        get { return _ignoreNum; }
        set { _ignoreNum = value; }
    }

    //name of the color area of the object
    //the colorsegement service must be
    //trained to recognize a color with this name
    String _objectColorName;
    [dssa.DataMember]
    public String ObjectColorName
    {
        get { return _objectColorName; }
        set { _objectColorName = value; }
    }

}

Note that when you no longer need the TestVar example, you can delete it from your code.

Step 11: Write the code to calculate the distance to the object

It is now time to write the code to make the distance calculation. Recall from your examination of the generated code that this code should go inside the RunHandler method in SimpleEstimateDistanceToObjectService.cs. You can start by adding the using directive for colorsegment at the top of the file and removing the code that deals with TestVar and Field in RunHandler. Remember that we renamed the input RecognizedAreas and changed its type. If you now type in message. to Visual Studio the code-completion feature will show you that this variable is available.

One important thing to know about the List of type ColorArea, returned by the ColorSegment service, is that it is ordered according to the size of the color areas (in terms of the number of pixels contributing to the area). We will make the simplifying assumption that the object we are looking for is the largest object of the appropriate color in the image.

You should now have enough information to use what you learnt in the first part of this tutorial to write the distance calculation code. Remember that if you are unsure what variables or methods are available you can use Visual Studio's code completion feature.

When you finish, your code may be similar to the code following.

 public IEnumerator<ccr.ITask> RunHandler(ActionRequest message, dssp.DsspResponsePort<ActionResponse> responsePort)
{
    _responsePort = responsePort;
    Increment();

    ActionResponse response = new ActionResponse();

    colorsegment.ColorArea objectArea = null;
    for (int i = 0; i < message.RecognizedAreas.Count; i++)
    {
        if (message.RecognizedAreas[i].Name.Equals(State.ObjectColorName))
        {
            objectArea = message.RecognizedAreas[i];
            break;
        }
    }

    //no area of the correct color, output -1 to represent no object
    if (objectArea == null)
    {
        response.Distance = -1;
    }
    else
    {

        //test if area greater than IgnoreNum, output -1 to represent
        //no object otherwise
        int boundingBoxArea = objectArea.Area;

        if (boundingBoxArea < State.IgnoreNum)
        {
            response.Distance = -1;
        }
        else
        {
            //compute distance from camera - note that depending on how camera is attached to the robot
            //you may have to do an additional calculation to convert this to the distance from the robot
            response.Distance = State.ObjectHeight * (State.FocalLength / (objectArea.MaxY - objectArea.MinY));
        }
    }



    _responsePort.Post(response);

    Decrement();

    yield return WaitUntilComplete();
}

Step 12: Configure your service

We will now use the Dss Manifest Editor to make a configuration file for your distance estimation service, so you can set the parameters such as the focal length of the camera.

Open up the Dss Manifest Editor, select, Open and open the manifest for your project. Since we called our VPL diagram Lab5 our manifest is called Lab5.manifest.xml. Under Diagram click on SimpleEstimateDistanceToObject and then in the pane on the right, click on the button Create initial state. You can now enter values for all the parameters. Once you are done, hit save, and close the editor.

Manifest Editor

Manifest Editor - create an initial configuration like this one.

If you now navigate to your project directory you will find an initial configuration file - SimpleEstimateDistanceToObject.config.xml Open this file and take a look at it in the web browser. You can see the definition of all the variables. If you want to change one of these variables in the future, all you need to do is edit this file in a text editor.

Config File

Config File - your configuration file will look similar to this.

Step 13: Test your service

It is now time to build and run your service! You can build and run your program from Visual Studio by pressing the green arrow button, or hitting F5. This will start a dss host in the console. Look at the output in the console and point a web browser to the DSS control panel which will be running on one of your comptuer's ports. The web address will look like https://hostname:portnumber. From the control panel go to the Service Directory to see the services that are running.

Service Directory

Service Directory - the list of services currently running.

If you click on colorsegment you can configure the colorsegment service to recognize the color of your object from a variety of distances. You may need to give colorsegment examples of color from multiple differences because the lighting can change quite dramatically as the object moves away from the camera depending on the conditions and the quality of the camera.

Once you have configured colorsegment, go back to the Control Panel and click on lab5. This will display an xml representation of your service's state and you will be able to see the value of the Distance variable. Experiment with moving the object closer and further away from the camera and refreshing the web-page to test out your service.

Lab5

Lab5 - displays the value of the Distance variable.

Conclusion

Note that we have taken a simplified approach to estimating the distance to an object in this tutorial. Those interested in computer vision, can learn much more about camera models, camera calibration and translating between object space and image space, by reading one of the many text books on the subject.

A simple extension you might like to try is to handle the case where the object of interest may not be the largest in the image of the specified color. If the object has some other characteristics however you can still detect it. Examples of characteristics may include: the shape of the object and the colors or shapes of objects it is normally near to.

Summary

In this tutorial, you learned how to:

  • Use the Color Segment Service.
  • Make a Distance Estimation Activity.
  • Connect the Activity with ColorSegment.
  • Set the ColorSegment manifest.
  • Generate C# Code.
  • Examine generated code.
  • Change the input a service can receive in C#.
  • Post the List of Color Areas.
  • Add configurable parameters in C#.
  • Code the distance calculation.
  • Configure your service.
  • Test your service.

 

 

© 2008 Microsoft Corporation. All Rights Reserved.