Real-Time Robot and Rat Tracker

This is a real-time, depth-camera based, overhead-mounted tracking system for small robots and small animals. It is a spin-off from the Pirat project at UQ but somewhat streamlined and simplified.

Features

• Real-time tracking using single commodity depth camera.
• Low latency architecture.
• Interactive - enables operator to correct online.
• Track deformable objects using multiple templates.
• Built-in template capture and editing.
• Per-template semantic and position annotations for automatic coding and enhanced interaction.
• TCP/IP socket output.
• Streaming data recording and replay of depth-camera input.
• Comprehensive logging, in easy-to-read json format.

System Requirements

• Windows 7 or Windows 10 64-bit (code is portable, but Kinect2 dependent at present).
• Quad-core modern CPU (can get away with less, perhaps).
• Kinect2 for Windows depth camera.
• For deformable objects with many template poses: NVIDIA GPU, very roughly GTX960M or above.

Paper and videos

• IROS 2018 Paper describing the Pirat system, which this tracker grew out of. The tracker in this paper is a much earlier version with very very sketchy rat-tracking, IROS 2018 paper supplementary video. Still, it showcases the target environment for the system.
• Video showcasing the tracker recorded to go with a ICRA paper (which was not accepted). Provides an overview of the system.

Pre-compiled packages

The pre-compiled binaries are built with CUDA and Kinect2 support, but should work without CUDA installed as the code detects falls back to CPU if there is no CUDA driver or device present. The Kinect2 SDK has to be installed (see below), as the code was compiled with Kinect2 support enabled. Also, visual C++ 2015 runtimes must be present, ant they usually are nowadays.

• Binary only build: just bin folder containing executables, assumes you cloned or copied the repository. Unzip into the root of the repository and it will add a bin folder with executables that are ready to go.
• Minimal stand-alone executable: contains everything needed to run the tracker, including data-files etc in addition to the binaries as per the above.
• Full souce plus binary package: as above but also includes the source code from which it was built.

Development Environment

Compiler

Visual Studio 2015 or later (tested with 2015), it should be fine with any version (Pro/Community/Etc), but not Visual Studio Code. Be sure to select the C++ language support when installing. The tracker is built in C++ and was developed on Windows using Visual Studio 2015, targeting 64-bit Windows. However, it has at some point been compiled for linux (less the Kinect support), and is mostly written in portable C++. It has very few external dependencies.

Boost

Boost libraries need to be installed and added to the inlcude and lib paths. The easiest way is to download and install prebuilt Windows binaries. The key trick is to understand which version of the binaries you need, given that they are NOT named after the Visual Studio Version (but instead the underlying C++ compiler version, which is not part of the user interface...). As far as I can tell, Visual Studio 2015 corresponds to version 14.0, so download e.g., boost_1_69_0-msvc-14.0-64.exe, and for Visual Studio 2017 get boost_1_69_0-msvc-14.1-64.exe.

Next you need to set the paths, the recomended way to do this is to do it once in the user-wide properties in visual studio, and then forget about it. This is done by opening the 'Property Manager' side bar (not the property sheet or property editor(!)). If not open, click View->Property Manager (VS 2015) to open this side-bar. Next expand the Debug|x64 node for the project and then double-click on Microsoft.Cpp.x64.user. In the property pages that open now, set VC++ directories to point at boost, e.g., add c:\local\boost_1_69_0 to Include Directories and c:\local\boost_1_69_0\lib64-msvc-14.0 to Library Directories (assuming you installed boost to the default location).

Boost 1.61 and a few newer releases have been tested, and should work.

For GPU support

Download and install CUDA 9.0 with Visual Studio 2015 integration. There are really no reasons why later CUDA would not work. The code was developed with CUDA 9 and the project file is setup to auto-detect CUDA 9. To change the CUDA version that is auto-detected, you need to open the src/shared/cuda_detect.props as well as the .vcxproj in a text editor and change the paths to the CUDA build extensions to match the new version. E.g., (from src/tracker/tracker.vcxproj):

<ImportGroup Label="ExtensionSettings" Condition="exists('$(VCTargetsPath)\BuildCustomizations\CUDA 9.0.props')">
  <Import Project="$(VCTargetsPath)\BuildCustomizations\CUDA 9.0.props" />
</ImportGroup>

and

<ImportGroup Label="ExtensionTargets" Condition="exists('$(VCTargetsPath)\BuildCustomizations\CUDA 9.0.props')">
  <Import Project="$(VCTargetsPath)\BuildCustomizations\CUDA 9.0.targets" />
</ImportGroup>

In src/shared/cuda_detect.props you also need to change (or add, depending) this same condition (look for Condition="exists('$(VCTargetsPath)\BuildCustomizations\CUDA 9.0.props')").

As a note, recall that the system works fine without CUDA but performance will be far lower.

Kinect Support

To compile the code to use the Kinect sensor you must download and install Kinect for Windows 2.0 SDK. The src/shared/kinect_detect.props should then automatically set the ENABLE_KINECT switch to 1. This works in the same way as the CUDA detection by using a condition to check for the SDK. The tracker can be built and used without the Kinect support, but can then only replay previously recorded files.

Usage

Sample data

There are a couple of pre-recorded sequences that can be donwloaded to test the system. These were used in examples in this document.

Running the tracker

Starting the tracker without any command line arguments will attempt to create Kinect2 input source and brings up the tracker main UI. To use a pre-recorded file, use -i <filename> if the file is in the ./recordings subfolder you can neglect specifying the path.

When the tracker starts and cannot find the kinect you will see a black screen like the below. Make sure you have powered the kinect properly, plugged it into an USB3 port, and that you are using a USB3 cable (the included one is, but if a USB2 cable is used, it will not work). Sometimes the Kinect does not like certain USB ports, even though they are supposed to be USB3 capable, it can take a few tries. To check if the Kinect is working you can run the Kinect Studio v2.0 that should have been installed with the SDK.

Root path

The project root path is by default set to be the grandparent of the executable path, e.g., running C:/blah/blah/bin/x64/tracker_Release.exe will use C:/blah/blah/ as the root path to load and store options etc. It is NOT based on the current working directory! This is possible to change with the --envRootPath,-r command line argument. The nice thing about the default is that it will work the same whether you start it from Visual Studio or just double click the executable.

Command line switches

  -h [ --help ]                         produce this help message.
  -i [ --input ] arg                    Name of the input file to load or
                                        'kinect2' to specify the depth camera.
  -t [ --optionsTag ] arg               Used to name specific options file to
                                        load, useful when many recordings from
                                        one setup need to be processed, default
                                        is to use the input name.
  -o [ --outputTag ] arg                Used to tag the output, e.g., the data
                                        log, the default is to use the input
                                        name.
  --defaultOptionsName arg              name of options file that will be
                                        loaded after the default.txt ones but
                                        before specific settings. Makes it
                                        easier to propagate a set of
                                        parameters.
  -d [ --depthOnly ]                    Turns off the Color stream from the
                                        input (the Kinect), this is needed in
                                        low light as the exposure time is
                                        increased, throttling the frame-rate.
  -c [ --forceCpuBackend ]              Makes the tracker use the CPU backend
                                        (even if a CUDA capable GPU is
                                        present).
  -p [ --pauseFrameCount ] arg (=-1)    Number of frames to run before pausing.
  -f [ --ffmpegPath ] arg (=C:\tools\ffmpeg\bin\ffmpeg.exe)
                                        Set the path to the ffmpeg executable,
                                        only needed to record video, not for
                                        tracking.
  -r [ --envRootPath ] arg (=C:/code\rt_robot_rat_tracker)
                                        Set the root path of the
                                        project/environment, defaults to
                                        grandparent of exe path.

Arena setup

The first time you start the tracker in a new location, or with a new input source, you need to set the arena ground plane and perimiter. Currently, only circular arenas are correctly identified. However, this is usually not a problem for other shapes, as the main thing is to get the ground plane setup. use the main UI to select an existing setup, or create a new setup.

Click <Arena Setup->Create/Modify> to begin setting setting up the arena.

Then click three locations on the arena floor in the depth camera view (2D). The program now estimates the arena centre and extents by recursively adding points that are neighbours of the clicked locations and in the same plane. The parameter onPlaneTolerance can be used to tweak how far away from the plane a point may be and still be included in the arena floor.

Using the debug renderer you can show the arena bounding box as well as the points that were included (Press <F4> to toggle the debug renderer). You can adjust the radius manually.

When happy you can save the arena setup, which is then stored in a file in the folder ./arena_setups. Once the arena setup has been set for a given input source it will be remembered in the options file.

Run-time options

The options file is based on the name of the source, that is, the file name if the source is a file or 'kinect2' if the kinect is the source. When processing recordings from one setup, the -t option can be used to override the source name to specify the options, e.g., -t mySetup will load/store options in ./options/mySetup.txt.

There are a large number of tweakable options in the program, most of which you can probably ignore for most scenarios. You can edit them using the tweak var UI toggled with <F2>:

Template libraries

Objects are tracked based on sparse disc-based 3D templates that are collected in template libraries. One template library represents multiple poses of a single object. In the case of a rigid object, one is typically sufficient, but a real object like a rat can require more than a hundred to achieve accurate tracking. Template libraries are stored in folders with one index file and a numnber of pose files. All template libraries are in the folder ./template_libs.

Creating a new template library

The current simplest way to do this is to edit tracked_object_types.json as per below, a new library name e.g., bob will automatically be saved on exit in ./template_libs/bob. Then to actually use it, you must also edit or create a tracked object setup (see below) to reference the new object type. Somewhat cumbersome, but hopefully not something you'd do often. Another way is to just copy the folder containing an existing library and renaming the folder, then of course you have to modify the setup files as above. To see how to capture new templates see below Capturing Template Poses

Importing from CAD

You can also run the tool obj_to_template_lib which will sample the mesh of an input wavefront OBJ model. It is not very sophisticated and the part that creates the simplified convex hull is not finished. So the recomendation is to give it a hull-like mesh (does not have to be convex), that has been cleaned up of unneeded detail before-hand.

Tracked object types

The file tracked_object_types.json stores definitions of 'types' of objects that may be tracked. This allows connecting the template library to a more readable name (and changing the template library) as well as providing an object class for the type of object. The class, which is a string, is passed to the client along with location and pose information. The tracked_object_types.json file is loaded at startup, and any changes to it requires a restart of the program. If a template library that does not exist is named, it will be created and saved when the tracker exits. Example of tracked_object_types.json:

{
    "pirat" : {

        "template_library_name": "pirat",
        "object_class": "irat"
    },
    "irat-old" : {
        "template_library_name": "irat-old",
        "object_class": "irat"
    },
    "real-rat"  : {
        "template_library_name": "lib_a0",
        "object_class": "rat"
    }
}

Tracked Object Setups

The folder ./tracked_object_setups contains files that define the currently active objects. The setup may be changed at runtime, and will then cause a reinitialization of the tracker. The below example defines two objects of the type 'pirat', which is defined above. Each instance is given a name and a colour. The name is sent along to the client with each tracking update.

[
    {
        "name": "pirat_1",
        "type_id": "pirat",
        "display_colour": {"x" : 1.0, "y" : 0.0, "z" : 0.0}
    },
    {
        "name": "pirat_2",
        "type_id": "pirat",
        "display_colour": {"x" : 0.0, "y" : 1.0, "z" : 0.0}
    }
]

Capturing Template Poses

If you have created a new template library, or want to add more poses to an existing one, you can do this using the tracker. As a rule, you should perform this on pre-captured data, as you may need to restart the tracker several times, and be able to go backwards and forwards to ensure good matches.

WARNING: Template libraries are always saved on exit, the only way to avoid this is to kill the program by closing the console window (neat, I know). This leads to two pieces of advice: 1. When you have made some additions you want to keep, exit to save them and re-start. 2. Keep the template library under version control, so you can revert any unwanted changes.

To capture poses, you typically want to have the tracker paused, then:

Step 1:

Select the tracked object with the template library you wish to modify, and place it on the target in the tracker. If the library is an empty one, you will see the default ellipsoid template. If the default template is active, use the slider 'defaultEllipsoidTemplateRadius' to modify the size until it sits stably on the target.

.

Step 2:

Next, in the main UI, Templates section, check the box 'Enable Pose Capture'. This ensures the best options for capturing poses are set. Adjust template size if needed after this.

Step 3:

Now if the default (or previous) template is in a good position (which means it is aligned with the axis you want to be 'forwards'), press the button 'Capture Pose' or (the short-cut key 'c') to capture the current pose. This should be reflected immediately as this now should be the best match (as a rule).

Step 4:

Since the capturing process is a large heap of ad-hoc measures and heuristics, the captured template is often somewhat noisy. So to clean it up, press F9 to view the template editor. Here you can select and remove discs, as well as add annotations, or indeed delete the pose and try again.

Step 5:

When you are satisfied with the pose, you can start playback until you see the need to create another template (typically when the pose of the target has changed sufficiently). Then repeat the process from Step 3. In the figures below, the plush rat has been changed to rear up, and a new pose is captured.

When Pose Capture mode is enabled, the result of the pose capturing process can also be visualized using the debug renderer. This can help explain if it is not behaving as expected. There exists a large number of tweakable variables that control different aspects of this process. Tweakable variables of all sorts can be edited in the Tweak var UI . Pose capture mode thus has rather high overhead and this is indicated in the HUD. It should never be on during normal tracking operations.

Recording tracking data

To start recording data simply check the 'dumpToFile' flag in the main UI (short-cut key 'f'). This starts a new ".bdsx" file in the ./recordings directory with a name along the lines of kinect2_20190219-01_35_46.bdsx. The HUD will indicate that a recording is ongoing, and also shows the amount of data that is currently buffered ('Bytes buffered'). The data is written asynchronously, and therefore, if the disk is slower than the kinect, the data may start accumulating. If the buffer becomes too large, it will cause swapping, which will further deterioate the performance (if it is the same disk). A normal external USB3 drive should have not problem sustaining the required throughput, but the safest bet is always to run the tracker on an internal SSD and then copy each recording to some other storage later.

Choice of Depth Camera

The primary motivation to use a depth-based system in the first place comes from the fact that rats come in light and dark varieties, as do backgrounds, and are most comfortable in dark environments. At the same time they are not able to percieve red light (or infra-red). Depth cameras make use of their own IR light in one way or another, and are therefore robust against lighting conditions (except sun-light, which is unlikely in a lab). Furthermore, a 3D point-cloud enables tracking the full pose, making more complex analysis and (robot) interaction possible.

The system supports using the Kinect 2 for Windows, as this is a cheap depth camera with decent input latency (see (Kinect 2 Latency)[#kinect-2-latency)). The depth resolution is very high as the technique used is time-of-flight. We also looked at the new Intel Realsense D435, as it, on paper, has superior specifications. In practice the D435 yielded a patchy, mess that is either over-blurred or so noisy as to be useless.

The key recomendation here is to use the Kinect2 and before commiting to implementing support for another camera, inspect the output carefully. It seems likely that time-of-flight is the most suitable technology as it provides much more local depth detail than stereo or pattern-projection based cameras.

Kinect 2 Latency

To measure the kinect input latency we created a simple LED circuit wired to an Arduino Uno and which is controlled using serial commands over the USB connection. The roundtrip latency to the arduino was established to be at-most 4 ms, but from experiments it is clear that the output latency is typically much lower.

By monitoring a region of pixels in the tracker, we were able to detect when the LED turned on, and thus directly measure the time from sending the serial command to detecting the change. We repeated this proces a large number of times, the below image shows the results:

The latency falls more or less uniformly in the interval [33,67] milliseconds. This fits quite well with expectations of a uniform variability of approximately 33ms, matching length of time the shutter is expected to be open during a frame, possibly plus some normally distributed latency variation owing to the rest of the system (a few ms). The base input latency of the Kinect itself, and perhaps the driver stack / USB, is fairly clearly approximately around 33ms.

A caveat for this result is that the latency was measured using the RBG input, as the LED light does not affect the depth camera at all. There is thus the possibility that the depth camera might behave differently if the RGB image is turned off. It might be possible to repeat the experiment using an IR-LED but the ones we had available at the time were not in the right wavelength band to interfere with the time-of-flight (or possibly just not bright enough). The IR range of the Kinect2 is reportedly around 850nm.

The code used to perform these measurements is not included in the repository as it was developed in another branch of the project where we also measured the latency to a Inilabs DAVIS 240B event-based camera. As we did collect data for this sensor also, it is included here in the hope that someone finds it intersting. It also supports the claim that the output latency of the arduino setup is substantially less than the worst-case roundtrip suggested.

Examples

Network Client Test (C++)

This project in ./src/network_client_test is a very simple network client that connects to the tracker and prints any tracking messages it receives. The intentionis to show how a developer can use the output of the tracker.

Python Tracking Client (Python)

In ./scripts/python_tracking_client. Very similar to the C++ tracking client but written in python. Contains enough code to fully unpack the binary message structure and print the contents.

Depth Data Reader (Python)

In ./scripts/depth_data_reader. Contains a class for parsing the data format used to store the data output from the tracker. Uses matplotlib to display the color and depth images. This provides a good starting point for any re-processing needs. For example extracting data in conjunction with the logs from the tracker.

Data Plots (Python)

In ./scripts/data_plots. Simple example showing how the json-based data log from the tracker can be used to plot the tracked locations. The data log also contains the name of the source file, as well as the frame number. This combined with the depth data reader, makes it possible to fully reconstruct the point cloud at any point. This simple example only plots the 2D arena-space locations, but provides a starting point for more complex analysis.

Software System Overview

The tracking system is designed for low-latency operation. To achieve this it is is essential to ensure the tracker starts processing the next frame as early as possible. To this end, the system uses several threads, each fulfilling different tasks asynchronously. The most common method of communication is using thread-safe queues.

1. Main thread (green boxes): performs all user interface rendering and input processing. The tracking frame data is rendered from this thread also and is sent along with pose updates from the tracking thread.
2. Tracking thread (red): Waits (using a blocking read) until tracking data is available from the input source and then performs the tracking system update. The resulting pose updates are sent to the main thread as well as output server.
3. Input source (yellow): Uses it's own thread only when instantiated with a Kinnect source, and the file data is read on demand. When reading from the Kinect it is essential that no frames are missed (for data recording purposes) and thus the use of a dedicated thread means that it can wait for then next frame after a minimal delay to post the data to the tracking thread as well as the (optional) output streamer.
4. Output Streamer (orange): The output streamer is responsible for writing binary depth frame data to disc, such that it can be replayed later. To avoid disc activity blocking other systems, the output streamer uses a queue to buffer frames that are waiting to be written to disk. The queue status is shown in the UI.
5. Output Server (blue): Creates and listens to a TCP/IP socket to send tracking update messages to. Again, since writing to a socket could potentially be a blocking operation, this uses a separate thread. In this case a single variable storing the pose of all tracked object and a condition variable is used instead of a queue. The reasoning is that we only ever want to send the most up-to-date pose anyhow.

Notes

Debugging

Debug mode does not work very well together with the live tracking, as the code is so slow as to fail to read the data reliably. It is therefore almost always needed to debug with a pre-recorded data set.

Recording video

The image stream output (using ffmpeg or just images) output works with relatively modest performance impact but can typically not keep up with the tracker (maybe on a super-duper machine). Since it is fed from the main thread (not the tracking thread) this will lead to frames being skipped as the tracking thread rushes ahead. This is in particular a problem when using recorded data, as this can often be played back at more than real-time speed. To produce videos that contain all frames the current workaround is to build the tracker with the tracking thread disabled (USE_TRACKING_THREAD in ./src/tracker/Config.h). This will force the rendering (in the main thread) to run in lock-step with the tracker.