Pilotguru is a project aiming to make it possible, with just a smartphone and zero hardware work, to gather all the necessary data to train your own computer vision based car autopilot/ADAS:

Data recording: Android phone with Marshmallow (6.0) or more recent version.

1. Clone this repo.

   git clone https://github.com/waiwnf/pilotguru.git
   cd pilotguru
   

2. Build the docker image (this will take a while).

   ./build-docker.sh
   

3. Build everything (inside a docker container).

   ./run-docker.sh
   mkdir build
   cd build
   cmake ..
   make -j
   

Everything below should be run from inside the docker container (i.e. after executing ./run-docker.sh from pilotguru root directory).

Velocity data comes from two separate sources with very distinct characteristics:

• GPS readings.
  • Pros: Distinct measurements have independent errors. This is an extremely valuable property, it means the errors do not compound over time.
  • Cons: Relatively coarse-grained, both in time (typically comes at around 1 update per second) and space (can be off by a few meters). May not be able to capture well important events like starting hard braking when an obstacle suddenly appears.
• Inertial measurement unit (IMU) readings: accelerometer and gyroscope.
  • Pros: Can be very fine-grained (e.g. up to 500Hz measurement rate on a Nexus 5X), so very promising for precise annotation on video frames level.
  • Cons: Inertial measurements use the immediate previous state (pose + velocity) of the device as a reference point. To get the device trajectory one needs to integrate the accelerations over time, so the measurement errors accumulate over time: acceleration measurement error on step 1 "pollutes" velocity estimates on each subsequent step. Even worse is the travel distance computation, where we need to integrate accelerations twice and the errors grow quadratically with time.

We will fuse the two data sources to combine the advantages and cancel out the drawbacks. GPS data will provide "anchor points" and let us eliminate most of the compounding errors from integrating the IMU data.

The fusion is done by fit_motion binary with JSON files produced by pilotguru recorder as inputs:

    ./fit_motion \
        --rotations_json /path/to/rotations.json \
        --accelerations_json /path/to/accelerations.json \
        --locations_json /path/to/locations.json \
        --velocities_out_json /path/to/output/velocities.json \
        --steering_out_json /path/to/output/steering.json

The outputs are two files:

Render a video with steering and velocity infdormation overlayed with the original input video:

./render_motion \
    --in_video /path/to/video.mp4 \
    --frames_json /path/to/frames.json \
    --velocities_json /path/to/output/velocities.json \
    --steering_json /path/to/output/steering.json \
    --steering_wheel /opt/pilotguru/img/steering_wheel_image.jpg \
    --out_video /path/to/output/rendered-motion.mp4

We aim to combine GPS data (coarse grained, but no systematic drift over time) with inertial measurements unit data (fine-grained, but prone to systematic drift) to combine the advantages of the two data sources and get fine-grained precise velocity data.

Before diving into details, notice that GPS data by itself is actually quite precise, at least with a decent view of clear sky. Here is an example track:

There are a couple of areas where we can count on improving the accuracy somewhat:

• Velocity jumps between the 30 second and 40 second marks are suspicious.
• GPS-derived velocity by its nature lags behind the true velocity changes, since we need to wait until the next measurement to compute the estimated velocity over the ~1 second interval. So during significant acceleration or braking there may be substantial difference between the GPS reported velocity (averaged over the past 1 second) and the true instantaneous velocity.

After incorporating the IMU data, we indeed see improvements on both counts. Noisy jumps are smoothed out and sustained sharp velocity changes are registered earlier:

Some of the accelerometer processing challenges can be seen by simply looking at the raw accelerometer sensor readings from a phone lying flat on the table:

One can note that

Formalizing the above section, define the calibration problem for the IMU unit to find the optimal

• g: accelerometer bias in the fixed reference frame (this is roughly gravity).
• h: accelerometer bias in the device-local reference frame (this is the bias introduced by the sensor itself).
• v0: Initial velocity (3D) at the beginning of the recording sequence to support recordings that start when the car is already in motion.

The optimization objective is helpfully provided by the coarse grained "mostly correct" GPS data: for every interval between two neighbnoring GPS measurements we will aim to match the travelled distance of the calibrated IMU trajectory with the distance from GPS data. Using L2 loss function, we get

where

• i is the index of a GPS measurement.
• k_i ... k_(i+1) are the range of indices for IMU measurements that fall in time between GPS measurements i and i+1.
• is the time of the j-th IMU measurement.

Next, unroll the integrated IMU velocity in terms of measured raw accelerations and calibration parameters. Denote and .

where R_j is the rotation matrix from the device-local reference frame to the fixed reference time at time step j. The rotation matrix can itself be computed by integrating the rotational velocities measured by the gyroscope. Denoting W to be the derivative of the rotation matrix coming from the gyroscope, in the first approximation we have

... aka error accumulation strikes back.

Output data is a JSON file with timestamped absolute velocity magnitudes (no direction information) in m/s:

{
  "velocities": [
    {
      "speed_m_s": 0.031
      "time_usec": 567751960167
    },
    {
      "speed_m_s": 0.030
      "time_usec": 567751962707
    },
    ...
  ]
}

Unlike accelerometer, gyroscope data does not suffer from noticeable systematic bias on our timescales (tens of minutes). The main problem with the raw data is that

• There is no notion of the road surface plane.
• The vehicle can rotate around all 3 possible axes: in addition to vertical axis (yaw, steering), rotation is possible around both the lateral axis (pitch, e.g. when transitioning from uphill to downhill road, or coming across a speed bump) and around longitudinal axis (roll, e.g. when driving into a pothole on one side of the car).

So the goal with rotation data is to infer the vertical axis in the vehicle reference frame. Then projecting instantaneous rotational velocity (in 3D) on the vertical axis yields the steering related rotation. Fortunately, the Earth locally is almost flat, so both pitch and roll rotations magnitude will be much smaller than steering-related yaw rotation. So we can well approximate the vertical axis as the one around which most of the rotation happens.

Output data is a JSON file with timestamped angular velocity around the inferred vehicle vertical axis:

{
  "steering": [
    {
      "angular_velocity": 0.02
      "time_usec": 567751960134
    },
    {
      "angular_velocity": -0.01
      "time_usec": 567751962709
    },
    ...
  ]
}