Sensor dynamics¶

The dynamics module is a base module which is available on every rc_visard and 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 (rc_slam)

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

Note

Using Stereo matching in parallel to the dynamics module may lead to decreased localization accuracy. See Visual odometry for how to avoid this.

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 focal point of the left camera.

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

Warning

The stereo INS module self-calibrates the IMU during its initialization. It is therefore required that the rc_visard is not moving and sufficient texture is visible during startup of the stereo INS module.

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 running, then it contains the loop-closure corrected estimates and is equivalent to the stream from Stereo INS when SLAM is not running.

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 SLAM and Stereo INS if the SLAM module is running. If SLAM is not running, 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 SLAM and Stereo INS when the SLAM module is running. If SLAM is not running, 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 SLAM and Stereo INS when the SLAM module is running. If SLAM is not running, 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 module, 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) 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.

Status values¶

This module reports the following status values:

Table 12 The `rc_dynamics` module’s status values¶
Name	Description
`state`	The current state of the rc_dynamics node

Services¶

The sensor dynamics module offers the following services for starting dynamics/motion estimation. All services return a numerical code of the entered state. The meaning of the returned state codes and names are given in Table 13.

Table 13 Possible states of the sensor dynamics module¶
State name	Description
IDLE	The module is ready, but idle
WAITING_FOR_INS	Waiting for stereo INS to start up
WAITING_FOR_INS_AND_SLAM	Waiting for stereo INS and SLAM to start up
RUNNING	The stereo INS module is running (SLAM is not running)
WAITING_FOR_SLAM	Waiting for SLAM to start up (stereo INS is running)
RUNNING_WITH_SLAM	Both stereo INS and SLAM are running
STOPPING	Transitional state when going to (or through IDLE)
FATAL	A fatal error has occurred (either in stereo INS or SLAM)

`start`¶

Starts the stereo INS module. Transitions from state IDLE through WAITING_FOR_INS to RUNNING.

This service has no arguments.

The definition for the response with corresponding datatypes is:
{
  "name": "start",
  "response": {
    "accepted": "bool",
    "current_state": "string"
  }
}

`start_slam`¶

Starts the SLAM and – if not yet started – the stereo INS module. From state IDLE: Transitions through WAITING_FOR_INS_AND_SLAM and WAITING_FOR_SLAM to RUNNING_WITH_SLAM. From state RUNNING: Transitions through WAITING_FOR_SLAM to RUNNING_WITH_SLAM.

This service has no arguments.

The definition for the response with corresponding datatypes is:
{
  "name": "start",
  "response": {
    "accepted": "bool",
    "current_state": "string"
  }
}

`stop`¶

Stops the stereo INS and – if running – the SLAM modules. The trajectory estimate of the SLAM module will still be available. Transitions from state RUNNING or RUNNING_WITH_SLAM through STOPPING to IDLE.

This service has no arguments.

The definition for the response with corresponding datatypes is:
{
  "name": "stop",
  "response": {
    "accepted": "bool",
    "current_state": "string"
  }
}

`stop_slam`¶

Stops the SLAM module. Stereo INS will continue to run. The trajectory estimate of the SLAM module will still be available. Transitions from state RUNNING_WITH_SLAM to RUNNING.

This service has no arguments.

The definition for the response with corresponding datatypes is:
{
  "name": "stop",
  "response": {
    "accepted": "bool",
    "current_state": "string"
  }
}

`restart`¶

Restarts to stereo INS. Equivalent to successive stop and start.

From state RUNNING or RUNNING_WITH_SLAM: Transitions through states STOPPING, IDLE and WAITING_FOR_INS to RUNNING.

This service has no arguments.

The definition for the response with corresponding datatypes is:
{
  "name": "restart",
  "response": {
    "accepted": "bool",
    "current_state": "string"
  }
}

`restart_slam`¶

Restarts to SLAM mode. Equivalent to successive stop and start_slam.

From state RUNNING or RUNNING_WITH_SLAM: Transitions through states STOPPING, IDLE, WAITING_FOR_INS_AND_SLAM, WAITING_FOR_SLAM to RUNNING_WITH_SLAM.

This service has no arguments.

The definition for the response with corresponding datatypes is:
{
  "name": "restart_slam",
  "response": {
    "accepted": "bool",
    "current_state": "string"
  }
}

The following diagram shows the main states and transitions. Intermediate states and the fatal error state are omitted for conceptual clarity.

_images/rc_dynamics_state_machine_simplified.png

Fig. 23 Simplified state and transition diagram

These services shall respond quickly. Therefore, for services that cause a state transition the value of the returned current_state in general is the first new (intermediate) state that was transitioned to, not the final state. E.g., for the start command the returned current_state will be WAITING_FOR_INS, not state RUNNING. If the transition does not take place within 0.1 seconds, the current state is returned. See Table 13 for the meaning of the returned state codes.

Note

The state FATAL can only be left by calling stop, which performs a transition to the state IDLE. The services restart and restart_slam internally use stop and will also work as expected. start and start_slam only work if the state is IDLE, and do nothing if the state is FATAL.

Note

The dynamics modules can also be started and stopped on the Dynamics page of the Web GUI.

`get_cam2imu_transform`¶

returns the transformation from camera to IMU coordinate frame. This is equivalent to the cam2imu_transform in the Dynamics message.

This service has no arguments.

The definition for the response with corresponding datatypes is:

{
  "name": "get_cam2imu_transform",
  "response": {
    "name": "string",
    "parent": "string",
    "pose": {
      "pose": {
        "orientation": {
          "w": "float64",
          "x": "float64",
          "y": "float64",
          "z": "float64"
        },
        "position": {
          "x": "float64",
          "y": "float64",
          "z": "float64"
        }
      },
      "timestamp": {
        "nsec": "int32",
        "sec": "int32"
      }
    },
    "return_code": {
      "message": "string",
      "value": "int16"
    }
  }
}

Sensor dynamics¶

Coordinate frames for state estimation¶

Available state estimates¶

Camera-pose streams (pose and pose_ins)¶

Real-time camera-pose streams (pose_rt and pose_rt_ins)¶

Real-time dynamics streams (dynamics and dynamics_ins)¶

IMU data stream (imu)¶

Status values¶

Services¶

start¶

start_slam¶

stop¶

stop_slam¶

restart¶

restart_slam¶

get_cam2imu_transform¶

Camera-pose streams (`pose` and `pose_ins`)¶

Real-time camera-pose streams (`pose_rt` and `pose_rt_ins`)¶

Real-time dynamics streams (`dynamics` and `dynamics_ins`)¶

IMU data stream (`imu`)¶

`start`¶

`start_slam`¶

`stop`¶

`stop_slam`¶

`restart`¶

`restart_slam`¶

`get_cam2imu_transform`¶