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.

• 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.

• SLAM (optional) (rc_slam)

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

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.

Fig. 22 Coordinate frames for state estimation. The IMU coordinate frame is inside the rc_visard’s housing. The camera coordinate frame is in the exit pupil of the left camera (see also Coordinate frames)

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"
}