# Sensor dynamics¶

The dynamics component provides estimates of the sensor state. These include pose, linear velocity, linear acceleration, and rotational rates. The module handles starting and stopping, and streaming of the estimates for individual subcomponents:

- Visual odometry (
`rc_stereovisodo`

) - estimates the camera’s motion from the motion of characteristic image points in the left camera images.

- Visual odometry (
- Stereo INS (
`rc_stereo_ins`

) - combines visual odometry measurements with readings from an inertial measurement unit (IMU) to provide accurate, high-frequency state estimates in real time.

- Stereo INS (
- SLAM (optional) (
`rc_slam`

) - performs simultaneous localization and mapping (SLAM) for correcting accumulated poses.

- SLAM (optional) (

## Coordinate frames for state estimation¶

The world coordinate frame for state estimation is defined as follows: The
coordinate frame’s z-axis points upward and is aligned with the gravity vector. The x-axis
is orthogonal to the z-axis and points in the *rc_visard*’s viewing direction at the
time when the pose estimation starts. The world
frame’s origin is located at the origin of the *rc_visard*’s IMU coordinate frame at the instant
when state estimation is switched on.

If pose estimation is switched on when the *rc_visard*’s viewing direction parallels
the gravity vector (with a tolerance range of 10 degrees), then the world coordinate frame’s y-axis
is aligned either with the IMU’s positive or negative x-axis. In this orientation, the
initial alignment of the world coordinate frame is no longer continuous. Thus, special care has to
be taken when pose estimation has to be started at such an orientation.

The transformation between the IMU coordinate frame and the camera/sensor frame is also
estimated and provided in the *real-time dynamics stream* over the rc_dynamics interface
(see Interfaces).

## Available state estimates¶

The *rc_visard* provides seven different kinds of timestamped state-estimate data streams via the rc_dynamics interface (see
The rc_dynamics interface):

Name | Frequency | Source | Description |
---|---|---|---|

pose | 25 Hz | best effort | Pose of camera frame, slightly delayed but most accurate |

pose_ins | 25 Hz | Stereo INS | Pose of camera frame, slightly delayed but most accurate |

pose_rt | 200 Hz | best effort | Pose of camera frame |

pose_rt_ins | 200 Hz | Stereo INS | Pose of camera frame |

dynamics | 200 Hz | best effort | Pose, velocity and acceleration in IMU frame |

dynamics_ins | 200 Hz | Stereo INS | Pose, velocity and acceleration in IMU frame |

imu | 200 Hz | Stereo INS | Raw IMU data |

*Best effort* here means that if SLAM is available, then it contains the
loop-closure corrected estimates and is equivalent to the stream from Stereo INS
when SLAM is unavailable.

### Camera-pose streams (`pose`

and `pose_ins`

)¶

The *camera-pose streams* called `pose`

and `pose_ins`

are provided at 25 Hz with
timestamps that correspond to image timestamps. The
former stream is the best-effort estimate, combining `rc_slam`

and `rc_stereo_ins`

if a license for the SLAM module is
available. If no license is available, then both data streams are equivalent. Pose
values are given in world coordinates, and also refer to the *rc_visard*’s camera
frame origin
(see Coordinate frames for state estimation).
They are the
most accurate estimates, taking all available *rc_visard* information into consideration.
They can be used in modeling applications, where camera images, depth images,
or point clouds have to be aligned highly accurately with each other. To ensure
the greatest possible accuracy, these pose values are delayed until a corresponding
visual odometry measurement is available.

### Real-time camera-pose streams (`pose_rt`

and `pose_rt_ins`

)¶

Two *real-time pose streams* called `pose_rt`

and `pose_rt_ins`

are provided at the IMU rate of 200 Hz. The
former stream is the best-effort estimate, combining `rc_slam`

and `rc_stereo_ins`

when a license for the SLAM module is
available. If no license is available, then both data streams are equivalent. They consist of the
pose estimates of the *rc_visard*’s camera frame origin (see Coordinate frames for state estimation)
in world coordinates. The
values given in these streams correspond to the values in the *real-time dynamics streams*,
but give the pose of the sensor/camera coordinate frame instead of that of the IMU coordinate frame.

### Real-time dynamics streams (`dynamics`

and `dynamics_ins`

)¶

Two *real-time dynamics streams* called `dynamics`

and `dynamics_ins`

are provided at the IMU rate of 200 Hz. The
former stream is the best-effort estimate, combining `rc_slam`

and `rc_stereo_ins`

when a license for the SLAM module is
available. If no license is available, then both data streams are equivalent. The
estimates can be used for real-time control of a robot. Since the values are provided
in real time and visual odometry computation requires some processing time,
the latest visual odometry estimate may not be included.
Therefore, these estimates are in general slightly less accurate than those in the
non-real-time *camera-pose streams* (see above), but are the best estimates
available at this instant. The provided dynamics streams contain the *rc_visard*’s

- translation \(\mathbf{p} = (x, y, z)^T\) in \(m\),

- rotation \(\mathbf{q} = (q_x, q_y, q_z, q_w)^T\) as unit quaternion,

- linear velocities \(\mathbf{v} = (v_x, v_y, v_z)^T\) in \(\frac{m}{s}\),

- angular velocities \(\mathbf{\omega} = (\omega_x, \omega_y, \omega_z)^T\) in \(\frac{rad}{s}\),

- gravity-compensated linear accelerations \(\mathbf{a} = (a_x, a_y, a_z)^T\) in \(\frac{m}{s^2}\), and

- transformation from camera to IMU coordinate frame as pose with frame name and parent frame name.

For each component, the stream also provides the name of the coordinate frame in which the values are given. Translation, rotation, and linear velocities are given in the world frame; angular velocities and accelerations are given in the IMU frame (see Coordinate frames for state estimation). All values refer to the IMU frame’s origin. That means, for example, that linear velocity is the velocity of the IMU frame’s origin in the world frame.

Lastly, the stream contains a `possible_jump`

flag, which is set to *true* whenever
the optional SLAM module (see SLAM (optional))
corrects the state estimation after finding a loop
closure. The state estimate can jump in this case, which should be considered when
the values are used in a control loop. If SLAM is not running, the jump flag can be
ignored and will stay *false*.

### IMU data stream (`imu`

)¶

The *IMU data stream* called `imu`

is provided at the IMU rate of 200 Hz. It consists of the
acceleration in x, y, z directions plus the angular velocities around these three axis.
The values are calibrated but not bias- and gravity-compensated, and are given in the IMU frame. The transformation
between IMU and sensor frame is provided in the *real-time dynamics stream*.

## Services¶

The sensor dynamics component offers the following services for starting, stopping, and restarting
dynamics/motion estimation.
It can also be switched on and off in the *Dynamics* area of the Web GUI’s *Overview* tab.

`start`

Starts the component.

This service requires no arguments.

This service returns the following response:

{ "accepted": "bool", "enteredState": "uint8" }

`stop`

Stops the component.

This service requires no arguments.

This service returns the following response:

{ "accepted": "bool", "enteredState": "uint8" }

`restart`

Restarts the component.

This service requires no arguments.

This service returns the following response:

{ "accepted": "bool", "enteredState": "uint8" }

`getstate`

Returns the component’s current state.

This service requires no arguments.

This service returns the following response:

{ "accepted": "bool", "enteredState": "uint8" }