Visual Inertial Sensors
Visual inertial sensors are a popular sensor choice for SLAM due to a large number of advantages over other sensor setups
- Unlike monocular SLAM, the maps have an absolute scale.
- State estimation and feature tracking is more robust to motion blur and fast rotations than visual only systems.
- IMU predictions can be used to give near instant state-estimates at over 100 Hz.
- The setup is cheaper, smaller, lighter and lower power then lidar or stereo setups.
However, there are two large downsides
- Small errors in the relative timestamps of the IMU and camera can cause large errors or divergence in the state estimation
- The system must continually estimate and compensate for biases in the IMU readings.
Some odometry systems (e.g. VINS mono) will attempt to compensate for timing errors, while others (e.g. ROVIO) require that all timestamps are accurate.
The timing accuracy needed will depend on the application and the state-estimator, however from handheld experiments with ROVIO we have found that generally:
- Within 2ms: improving timing accuracy beyond this has minimal to no effect.
- Within 5ms: general movements can be tracked, but fast changes can cause loss of tracking / divergence.
- Within 20ms: slow careful movements can result in usable state estimates.
- Over 20ms: unusable, instant divergence.
Many sensors (including almost all cameras) simply timestamp their data on arrival at the computer. However there are a large number of delays that prevent this stamp from being accurate. These include:
- Camera exposure time
- Internal data processing
- Internal filtering (for IMUs)
- Data transfer
- The OS scheduler pausing the driver process
For most of the cameras we have tested these delays are usually in the range of 5 to 30ms. While some components of this delay are fairly constant and may be accounted for by a hardcoded offset, others may vary by 5ms plus per image. These unknown delays prevents any sensor that is timestamped on the host computer from providing accurate timing information to any visual-inertial estimator.
Every MEMS IMU has an unknown bias to its measurements. This bias is dependent on temperature, humidity and several other factors that cause it to slowly change over time. This causes issues for the state estimator as any motion the system undergoes could also be explained by the bias changing. The main difference between a $1 and $1000 IMU is not their sensitivity, but the amount of bias and its rate of change (often the expensive IMU's are basically just a cheap IMU that is measuring temperature and performing corrections). The slower the bias of a system changes, the more observable the motion is and the more robust the system becomes.
Only a small number of commercial solutions exist and we are currently unaware of any sensor that does not have a fatal flaw. Sensors and their issues:
- Skybotics VI-sensor (bundles IMU messages, never made it out of prototype and discontinued)
- Sky-aware VI-sensor (bundles IMU messages, never made it out of prototype and discontinued)
- Realsense ZR300 (gyro and accelerometer run at different rates, discontinued)
- Optor sensor (software timestamping)
- ZED-mini (still evaluating, but results so far are poor)
Due to the limited availability of quality VI sensors, most people who need tightly synchronized visual inertial data are going to have to put one together themselves. This guide will give a general overview of what is important in getting a sensor up and running, before giving specific instructions and code for a sensor using a pixracer and point-grey camera.
The idea behind the sensor is very simple. As a computers OS prevents sub-millisecond accurate timing, a micro-controller is used. The micro-controller gets data from an IMU and uses a trigger-line to control when a camera takes the images. It then transmits this information to the computer where it can be associated with the received images.
The IMU is connected directly to the micro-controller and periodically provides its readings with minimal delay. The micro-controller timestamps this data on arrival so each IMU message now has a timestamp in the micro-controller time (usually ticks since power on).
As long as the IMU rate is much greater than the camera frame rate, its exact value does not matter too much, we typically run our systems at 200 Hz, but have had sucess with anything in the range of 50 to 1000 Hz.
Almost all IMUs have the option of performing some on chip filtering. Keep in mind that any filtering applied will add a delay. Fortuantely these delays are fairly constant for most filters and so a simple offset can be applied to the timestamp to account for them. As a rule of thumb keep the filtering fairly minimal to reduce this issue.
However, in the case of sensors mounted on rotary wing systems, some filtering is always needed. As a rule of thumb the effective noise on the IMU will jump several orders of magnitude when flying due to the vibrations induced by the motors. Always keep this in mind and provide damping between the system frame and IMU.
While using an interrupt based strategy for getting new IMU data is preferable, the delay caused by a polling based approach is typically not significant.
As micro-controllers either have no OS or a very minimal low-latancy one, they can accurately timestamp data. Because of this, the micro-controller has three main tasks.
The first, as mentioned above is to give timestamps to the messages received from the IMU.
The second, is to periodically trigger the camera using a trigger-line (usually done by sending a pulse from a GPIO pin). Take note as to if the camera is triggered on the rising or falling edge and send the appropriate signal. Whenever the trigger occurs the micro-controller records both the timestamp at which the trigger occurred and the number of times it has activated the trigger.
The third task is to transmit the timestamped IMU message, the trigger timestamp and the number of triggers sent to the computer.
A fourth optional task is for the micro-controller to communicate with the connected computer in order to estimate the offset between the two clocks (using an approach such as ticsync). While it is not essential that the micro-controller sync its clock with the computers, if this is done it can output when the readings occurred in the computers time frame which allows the readings to also be used and fused with input from other systems.
The camera needs to support triggering off an external signal. To work with most visual-inertial pipelines it is also required that it has a global shutter. As the average position of objects inside an image occurs when half the exposure has elapsed, the length of the exposure must also be provided by the camera. Finally in order to ensure robustness to dropped frames the camera should be able to output the number of frames it has captured since power-up. As shown in the above diagram the camera should be triggered by the micro-controller and send this data to the computer.
The computer should have a custom camera driver. This driver will operate as follows:
- Read in trigger data and store in a lookup table.
- Read in the image, exposure length, and frame number.
- Use the frame-number to look up what time the frame was triggered at.
- Timestamp the image by adding half the exposure time to the trigger time.
- Add an offset to the timestamp to account for any small constant delays or filtering.
- Output the timestamped image.
The Camera-IMU calibration tool Kalibr can perform timing calibration. In our experience the results are accurate to within 1ms. A working system will usually still have at least a few milliseconds of constant delay introduced. Kalibr can be used to find this offset. Note to verify correctness this offset value should be the same over multiple datasets and under different lighting conditions.
A fairly minimal VI-sensor is shown below. It consists of
- Pixracer Autopilot: while designed for drones the autopilot provides a microcontroller and IMU in one plus comes with a ROS driver to retrieve the information.
- Point-grey BFS-U3-04S2M-CS: Machine vision camera with great low light performance.
- Focussafe Fisheye Lens: a cheap wide FOV lens
The Pixracer is rigidly attached (tape and zip-tied) to the camera. Pin 3 of the cameras GPIO is attached to servo pin 1 on the Pixracer (both these pin choices are fairly arbitrary, with other options possible) and the grounds are connected.
The pixracer is flashed with our version of the PX4 firmware. For our handheld setup we also went in and removed all the filtering in the MPU6000 driver (in this configuration delays are a smaller issue, but the IMU is too sensitive to vibrations to fly with). QGroundControl should be used to calibrate the internal IMU and setup camera triggering. The second internal IMU should then be disabled (controlled by CAL_ACC1_EN and CAL_GYRO1_EN)
IMU data and trigger information can be retrieved from the pixhawk using MAVROS. Make sure to enable timesyncing <param name="conn/timesync_rate" value="10.0"/> <param name="conn/system_time_rate" value="1.0"/> (or otherwise all timing information will be thrown away and replaced with arrival times)
Camera trigger time association is done with our modified Flir camera driver that listens for the trigger information published by MAVROS. Note, the constant offset induced by any internal delays and filtering is found by Kalibr and given as an input to this driver.
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="sensor_name" default="pixracer"/>
<arg name="namespace" default="$(arg sensor_name)" />
<group ns="$(arg namespace)" >
<node pkg="mavros" type="mavros_node" name="mavros" output="screen">
<param name="conn/timesync_rate" value="10.0"/>
<param name="conn/system_time_rate" value="1.0"/>
</node>
<node name="spinnaker_camera_node" pkg="spinnaker_camera_driver" type="camera_node" clear_params="true" output="screen" >
<param name="force_mavros_triggering" value="true" />
<param name="image_format_y_binning" value="1"/>
<param name="imu_time_offset_s" value="YOUR_DELAY_FOUND_BY_KALIBR_GOES_HERE" />
<remap from="/mavros/cam_imu_sync/cam_imu_stamp" to="/$(arg namespace)/mavros/cam_imu_sync/cam_imu_stamp"/>
</node>
<node name="rovio" pkg="rovio" type="rovio_node" output="screen">
<param name="filter_config" value="$(find mav_startup)/parameters/sensors/$(arg sensor_name)/rovio_filter.info" />
<param name="camera0_config" value="$(find mav_startup)/parameters/sensors/$(arg sensor_name)/rovio_cam0.yaml" />
<remap from="cam0/image_raw" to="image_raw"/>
<remap from="imu0" to="mavros/imu/data_raw"/>
<param name="world_frame" value="odom"/>
</node>
</group>
</launch>
The MPU6000 has very significant bias that can hurt robustness, because of this if you can afford it there are a lot of advantages to upgrading the IMU. The Pixhawk boards can be configured to work with an external IMU. For example we use the Analog Devices ADIS16448. Our fork of the PX4 firmware will automatically change to using this IMU if it is detected. However, it should be noted that the ADIS16448 communicates via SPI and the Pixracer does not expose this port. Because of this an alternative board in the pixhawk line is needed.