Concept

There exist various frames that are needed to obtain the robot's global pose (pose within in the global frame):

Name	Description
`global`	The "world" frame
`map`	During mapping, the robot does not have knowledge about the world but creates a local map (with `map` as root frame). All map features (e.g. point clouds, occupancy grids, ...) are linked to this frame.
`odom`	The odometry as an integration of the robot's velocity.
`robot`	The robot's root frame

frames

For each transformation between frames, there exist individual components:

Involved Frames	Description
`global->map`	Map registration: given a scene (objects with known poses, e.g. walls, ...), this component obtains a static transformation between a the scene and the map.
`map->odom`	Localization The localization component computes the transformation `map->robot` and performs map corrections by updating `map->odom`.
`odom->robot`	Odometry

Memory Design

Within the core segment, each robot has its own provider segment (here: Armar6). Within the provider segment, entities describe the transformations between the frames (Global,Map -> global->map).

Localization

Using the Localization Memory

See armarx::armem::robot_state::client::localization (include RobotAPI/libraries/armem_robot_state/client/localization/).

There exist two classes:

armarx::armem::robot_state::client::localization::TransformWriter to send transformations to memory
armarx::armem::robot_state::client::localization::TransformReader to obtain transformations between any of the frames; can also be used to obtain the "robot's global pose" (getGlobalPose(), also used by RobotReader)

For an example of a writer, see SelfLocalization in armarx/skills/localization_and_mapping.

Looking up transforms

A lookup (TransformReader::lookupTransform() with a TransformQuery, or getGlobalPose()) walks the chain of links from the query's parent frame to its frame, e.g. Global -> Map -> Odom -> root, and evaluates every link at the query time:

A pose exactly at that time is used as it is.
With interpolation, a link with poses right before and after that time is interpolated between them (position linearly, rotation by slerp).
Otherwise, the link uses its latest pose at or before that time. If the query time lies after the link's newest pose (typical for Clock::Now()), that newest pose is held and the lookup succeeds.
Static links (written with header.isStatic = true, and Global -> Map, which is treated as static) are valid until their next pose replaces them, and before their first pose. They are never interpolated or held, and no limit applies to them.

Besides the transform and a status (convertible to bool), the result has transform.header.timestamp, the time the result is valid for, and linkTimes: the oldest and newest time the links were evaluated at (spread() is the difference), the largest gap between the two poses a link was interpolated or chosen between (largestPoseGap), and whether any link was held (anyLinkHeld: the result is provisional, the same query can give a different answer once that link's next pose arrives) or interpolated (anyLinkInterpolated).

Settings

Setting	Where	Default	Effect
`interpolate`	`TransformQuery`; default per component: property `mem.robot_state.interpolateTransforms` (`TransformReader`, also for `getGlobalPose()` / `RobotReader`), `mem.loc.interpolateTransforms` (RobotStateMemory's own lookups)	off	Interpolate between the poses around the query time instead of using the latest pose at or before it.
`timePolicy`	`TransformQuery`	`QueryTime`	`QueryTime`: every link at the query time (slow links are held). `LatestCommonTime`: all links at the latest time every non-static link has data for, like ROS TF; consistent and final, but possibly older than the query time (the header timestamp says when).
`limits.maxExtrapolation`	`TransformQuery`; for `getGlobalPose()`: property `mem.robot_state.globalPoseMaxExtrapolationSeconds`	unset	Fail (`ErrorExtrapolationTooLarge`) if a link would be held longer than this (with `LatestCommonTime`: if the common time lies further before the query time). Unset: held without limit; a link held for more than 5 s is reported with a throttled warning.
`limits.maxSpread`	`TransformQuery`	unset	Fail (`ErrorTimeSpreadTooLarge`) if the links are further apart in time than this.
`limits.maxPoseGap`	`TransformQuery`	unset	Fail (`ErrorPoseGapTooLarge`) if the two poses of a link around the evaluation time are further apart than this, e.g. across a dropout of the localization.
`isStatic`	`TransformHeader`, when writing	false	Mark a fixed transform (e.g. a sensor mount); see above.

Which settings do I want?

The robot's current pose (navigation, visualization, getGlobalPose(..., Clock::Now())): the defaults. The slower links (e.g. map -> odom) are held at their newest pose. If the localization updates map -> odom continuously, set maxExtrapolation (or globalPoseMaxExtrapolationSeconds) to a few update periods, so a stopped localization makes the lookup fail instead of silently returning an outdated pose.

A pose at a time in the past, e.g. where a camera was when an image was taken: interpolate, and limit the pose gap so the lookup does not interpolate across a dropout.

namespace loc = armarx::armem::robot_state::localization;
 
const loc::TransformQuery query{.header = {.parentFrame = armarx::GlobalFrame,
                                           .frame = cameraFrame,
                                           .agent = robotName,
                                           .timestamp = image.timestamp},
                                .interpolate = true,
                                .limits = {.maxPoseGap = armarx::Duration::MilliSeconds(500)}};
 
const loc::TransformResult result = transformReader.lookupTransform(query);
if (not result)
{
    ARMARX_WARNING << result.errorMessage;
    return;
}
const Eigen::Isometry3f global_T_camera = result.transform.transform;

Consistent in time, like ROS TF: timePolicy = LatestCommonTime with maxExtrapolation. All links are evaluated at the same time; use result.transform.header.timestamp as the time of the pose.

The behaviour before interpolation was added: the defaults (interpolation is off). Each link then uses its latest pose at or before the query time, as before.

Fixed transforms (sensor mounts, calibrated offsets): write them with header.isStatic = true. A static link is valid before its first pose; a new static pose replaces it from its timestamp on. If a mount is calibrated at runtime, lookups for data from before the first calibration therefore get that calibration instead of failing.

Differences to ROS TF

ROS fails a lookup whose time lies past the newest data of a link (extrapolation error). Here, the newest pose is held by default and the lookup succeeds (check linkTimes.anyLinkHeld). For the ROS behaviour, use LatestCommonTime with maxExtrapolation.
ROS always interpolates; here, interpolation is opt-in.
A re-published static transform replaces the old one in ROS for all times; here, only from its timestamp on (earlier lookups keep the old pose). Before its first pose, it is valid in both.
Global -> Map counts as static even without the flag.

Writing transforms

TransformWriter stores a transform at header.timestamp, so set it to the time the transform is valid for (e.g. the time of the laser scan it was computed from), not to the time of writing. Write each link in time order; interpolation and lookups in the past rely on it. Timestamps far in the future or past are reported (properties mem.robot_state.maxTimestampFutureSeconds and mem.robot_state.maxTimestampAgeSeconds), as they usually indicate a reused header or a clock offset between PCs.