Skip to content

Coordinate Systems

Jump to bottom Edit New page
dskyle edited this page Jul 10, 2018 · 24 revisions

Introduction

GAMS' coordinate system capabilities, contained within the gams::pose namespace, are designed to provide easy conversion between different kinds of coordinate reference frames, embedded within each other. For example, you can represent the Earth using a GPS reference frame, and embed Cartesian frames within it throughout the world, with origins at given latitudes, longitudes, and altidues. This would allow for manipulating positions on a scale of meters near that origin, automatically translating back to latitude and longitudes as needed, as well as seemlessly translating between points in the various embedded frames.

In addition, Cartesian frames can embed within each other, forming trees of reference frames, equivalent to transform trees in ROS.

The basic coordinate types, Position, Orientation, and Pose (which combines position and orientation information into one object) bind to a reference frame. GAMS provides a default Cartesian reference frame, suitable if your application does not require any conversion between reference frames.

Table of Contents

Reference Frames

ReferenceFrame objects represent the various coordinate systems coordinates can belong to. Reference frames can embed within other frames, allowing for transformations between those frames. ReferenceFrames are reference counted to ensure they live as long as any child frames, or coordinates using them.

A ReferenceFrame has an ID. This ID can be specified at time of creation; otherwise, it will receive a random unique ID. Either way, this ID should be unique across all KnowledgeBases a process might save ReferenceFrames to. Random IDs are generated lazily, as needed. This saves the cost of generating one for short lived temporary frames. Call the has_id() method to determine if a ReferenceFrame already has an ID; calling id() will generate one automatically.

GAMS supports two kinds of reference frames: Cartesian, and GPS. Cartesian is the default, and will be used if no other kind is specified.

The GPS frame is a simple spherical model, and provides position coordinates as Latitude, Longitude and Altitude.

ReferenceFrames have string ID associated with them. You can specify one at time of construction, as the first parameter of the constructor. If you omit the ID, a random GUID will be generated.

There are two default frames available, each accessed through a global function in the pose namespace: default_frame() and gps_frame(), a Cartesian and and GPS type respectively. The former is also the frame used if a coordinate is constructed without an explicit frame.

These frames are parentless, and may serve as roots of a frame tree. A frame may have a parent, by specifying an origin point within another frame. By creating frame objects with parents, coordinates can easily convert between any frames in the resulting tree. For example:

using namespace gams::pose;

// Latitude/Longitude frame representing planet Earth
const ReferenceFrame &earth_frame = gps_frame();

// Cartesian frame representing a house, at the given Latitude and Longitude
ReferenceFrame house_frame("House", Position(earth_frame, 40.4464481, -79.9500588));

// Cartesian frame representing a given room in the house, centered at
// 10 meters East and 20 meters North of the above point
ReferenceFrame room_frame("Room", Position(house_frame, 10, 20));

In this example, coordinates within room_frame may be converted to coordinates with earth_frame. Instead of using Position, Pose may be used instead to specify frames that are rotated with respect to another. Position objects keep a reference to their frame, so you must ensure that a frame object outlives any positions belonging to it.

There are additional reference frame related types, including ReferenceFrameVersion, ReferenceFrameIdentity, and ReferenceFrameType. These are implementation details, hidden behind the ReferenceFrame facade. Users should not reference these directly, except to compare the return value of ReferenceFrame::type() to either pose::Cartesian or pose::GPS.

Coordinate Types

GAMS supports nine coordinate types; three fixed vector: Position, Orientation, and Pose.

And two kinds of free vectors, with three levels of derivatives each: Displacement, Velocity, and Acceleration; Rotation, AngularVelocity, and AngularAcceleration.

All the above coordinates bind to a frame (the first argument of any constructor). If you construct a coordinate of any type omitting the frame argument, it will bind to the default reference frame, a Cartesian frame that has no parent.

There are frame-less versions of the above, which have the same name, but appended with Vector, e.g., PositionVector. If a framed coordinate is constructed from a frame-less coordinate, the frame-less one is treated as though it already belongs to the framed coordinate's own frame.

There a few base classes that the above types inherit from. These are implementation details, and are not relevant to most users of GAMS directly.

Position

Position represents position information. It can represent coordinates within any type of 3-dimensional coordinate system (there aren't separate classes for GPS vs Cartesian coordinates). Position provides methods for accessing those coordinates according to various naming conventions:

Position earth_loc(earth_frame, 40.4465197, -79.9494575);
earth_loc.lat(); // Latitude (40.4465197)
earth_loc.lng(); // Longitude (-79.9494575)
earth_loc.alt(); // altitude (0 meters above surface, the default)

Position room_loc(room_frame, 1, 2);
room_loc.x(); // x-coordinate (1 meter from origin)
room_loc.y(); // y-coordinate (2 meters from origin)
room_loc.z(); // z-coordinate (0 meters above origin, the default)

These different functions are for the sake of readable code; there is no underlying difference between, for example, earth_loc.lng() and earth_loc.x().

To set coordinates, simply pass the new value as an argument to the function for that coordinate; e.g., room_loc.z(2).

The Velocity and Acceleration classes expose exactly the same interface as Position.

Orientation

The Orientation class represents a rotation or orientation relative to its frame. There are two primary ways to construct an Orientation. The easiest is to specify one of the fundamental axes, and an Orientation (in degrees, counter clockwise if looking from above) around that axis:

Orientation simple_rot(room_frame, Orientation::Z_axis, 90);

The more expressive form represents the orientation in an angle-axis vector format. For this, the rx, ry, and rz parameters represent a 3-vector which points in the direction of the axis of rotation relative to the origin orientation of the frame (e.g., for GPSFrame, due north along the ground), and whose magnitude is the amount of rotation about that axis in radians. The above orientation could be specified instead as follows:

Orientation simple_rot(room_frame, 0, 0, M_PI / 2);

This notation can represent any possible orientation relative to the frame, while the first notation is limited to simple orientations.

The AngularVelocity and AngularAcceleration classes expose exactly the same interface as Orientation.

Pose

The Pose class combines both position and orientation information. It may be constructed as follows:

Pose room_pose(room_frame, 3, 1, 0, 0, 0, M_PI / 2);

Where the positional 3-vector is specified first, followed by the rotational 3-vector. A variety of other constructors are avialable (see the Doxygen documentation).

Stamped Coordinates

Each coordinate type has a timestamped version, prefixed with Stamped. These versions support the same constructors that the un-stamped versions have, but with an optional initial timestamp argument, a std::chrono::time_point, available as gams::pose::TimeValue. These types provide three getter/setter pairs, all operating on the same internal timestamp, but with different units:

  • time() gets the current TimeValue, time(TimeValue) sets it
  • nanos() gets the timestamp in nanoseconds, nanos(uint64_t) sets it
  • secs() gets the timestamp in seconds (as a double), secs(double) sets it

Coordinate Operations

GAMS provides the following methods on all coordinates (Position, Orientaion, and Pose, as well as the derivative coordinates):

transform_to creates a copy of a coordinate, but in a new reference frame. This new frame must somehow be related to the coordinate's current frame.

Pose gps_pose = room_pose.transform_to(earth_frame);

transform_this_to transforms the coordinate object, in place, to the new reference frame. Again, it must be related to the current frame.

room_pose.transform_to(earth_frame);

distance_to finds the distance between the two coordinates. For Position and Pose, this is the number of meters between them. For Orientation, this is the number of degrees between them. Note that the coordinates need not be in the same reference frame, but they must be transformable to a common reference frame. Orientation and Pose also provide angle_to. For Orientation, this is synonymous with distance_to. For Poses, this finds the degrees between the orientation parts of the Poses.

room_loc.distance_to(earth_loc); // returns sqrt(5)

These operations can raise exceptions. The most common is the unrelated_frames exception, which indicates that the coordinates you are operating on belong to frames which are not connected by a tree. exception, which indicates that the coordinates you are operating on belong to frames which are not connected by a tree. The other possible exception is undefined_transform, raised if the transform tree structure is not supported. In particular, the GPS frame should not be embedded within a Cartesian frame. This will be avoided as long as you always use gps_frame() and never construct your own GPS-type ReferenceFrame. Both exceptions derive from std::exception.

Platform Frames

In GAMS, every platform belongs to a reference frame. By default, this frame is the Earth GPS-based frame. In most cases, you will manipulate coordinates belonging to this frame, or to frames embedded in it. For example:

class MyAlgorithm : public BaseAlgorithm {
  ...
  /* Note: BaseAlgorithm provides:
     protected: BasePlatform *platform_; */

  void execute() {
    const ReferenceFrame &platform_frame = platform_->get_frame();

    Position platform_location = platform_->get_location();
    ReferenceFrame local_frame(platform_location) // Create Cartesian frame
                             // centered on the platforms current location

    Pose target(local_frame, 5, 0); // location 5 meters east of platform
    Pose target_gps(platform_frame, target); // convert to platform frame (GPS)

    platform_->move(target); // conversion to platform's frame is automatic;
                             // equivalent to platform_->move(target_gps);

    double distance = target.distance_to(platform_location);
    // distance == 5
  }
  ...
}

Additionally, platforms which target the VREP simulator (derived from VREPBase) provide a get_vrep_frame() method which returns a Cartesian type ReferenceFrame, embedded within the platform's GPSFrame, which corresponds to the reference frame inside VREP. This frame will be a child frame of the platform's frame.

In general, any coordinates you pass to platform methods must belong to that platform's frame, or to a frame that is a child of that frame (or grandchild, etc.).

When you implement a Platform, you can choose what frame you want the platform to view the world through, by overriding the get_frame() method. For example:

class MyPlatform : public BasePlatform {
  ...

  ReferenceFrame get_frame() const override {
    const KnowledgeBase *kb = get_knowledge_base();

    double origin_lat = kb.get(".building_origin_lat").to_double();
    double origin_lng = kb.get(".building_origin_lng").to_double();

    Position origin{gps_frame(), origin_lng, origin_lat, 0};
    return ReferenceFrame("BuildingFrame", origin);
  }

  ...
}

In this example, the platform is designed to operate within a given building, so it is natural to use a local cartesian frame centered on it as the base frame. Note that get_frame() should provide a global frame. It should not move along with the platform itself.

Alternatively, the above platform could simply return default_frame() instead of constructing a new Cartesian frame embedded in the GPS frame. This would lose the capability to convert local coordinates to GPS coordinates, or to transform to other local cartesian frames elsewhere in the world.

Frame Timestamping

Reference frames provide an additional field not yet discussed: a timestamp. Each frame can have a timestamp, which represents the moment in time when the frame was measured or interpolated to. It is the moment best estimated as when the transformation provided by the frame is accurate.

You can set the timestamp of a frame at construction, by passing an integer value after the origin pose. If none is given, a default of -1 is used, which causes the frame to be "eternal". It is treated as always accurate at all possible timestamps. This is appropriate for, for example, a building frame, which will presumably not change during the course of a mission.

Frame timestamps are not checked during transform operations. It is assumed that if your frame tree has inconsistent timestamps, they are within an appropriate tolerance. If you are using timestamps, you should use the load_tree method, described in the next section, to ensure you are operating on trees with fully consistent timestamps.

Frame Tree Storage/Buffering

When initially constructed, ReferenceFrame objects are strictly in-memory structures. They have no connection to a KnowledgeBase. You can use the save method to store a ReferenceFrame to the KnowledgeBase. The frame will be stored based on its ID and version for later lookup with load, which loads a given ReferenceFrame. For example:

// These functions not provided by GAMS
double get_sensed_x();
double get_sensed_x();
uint64_t get_timestamp();

...

double x = get_sensed_x(), y = get_sensed_y();
uint64_t time = get_timestamp();
ReferenceFrame sensor_frame("Sensor", Position(x, y), time);
sensor_frame.save(kb);

...

ReferenceFrame sensor_frame = ReferenceFrame::load(kb, "Sensor", time);

Usually, however, you'll want to load an entire consistent tree from the KnowledgeBase. For this, you can use the load_tree method. For example:

std::vector<std::string> ids = {"Building", "Sensor", "FrontCamera"};

std::vector<ReferenceFrame> frames = ReferenceFrame::load_tree(kb, ids, time);

std::vector<ReferenceFrame> latest_frames = ReferenceFrame::load_tree(kb, ids);

In this example, frames will contain the given frame IDs at the given time, load them into ReferenceFrame objects, interpolated as necessary, and return them (in the same order as the ID list given). latest_frames will contain the latest possible consistent set of frames.

load_tree does not only examine and load the specific IDs given. It will also consider any intermediate frames needed to join the IDs given into a tree, and load/interpolate those as well. It will not return those frames directly as part of the returned vector.

If load_tree cannot find a consistent tree which satisfies the timestamp given, or if the frames given are not part of the same frame tree (no path through parent frames can transform between them), or if any of the IDs given do not exist in the knowledge base, the returned vector will be empty.

Note that save only saves a single frame, not an entire tree. Ensure you are saving the entire ancestry of frames you need.

The gps_frame() and default_frame() frames are not saved by default. You will need to save them yourself, if they are in the ancestry of any frames you wish to save, to each KnowledgeBase where you wish to save such frames. This should typically be done at the start of your controller.

Frame Storage Location

By default, all frames are stored with a ".gams.frames" prefix in the KnowledgeBase. This keeps them in a well-known location for later loading. This prefix also implies that frames are kept local, and not sent to other agents. To override this prefix, each load and save function can take a prefix argument as final argument. You must pass the same prefix to save and load/load_tree. Load operations will only look at a single prefix, so each prefix must hold complete trees, including any default frames, to be useful. If you save frames without a "." at that start, the frame data will be sent to other agents, if the KnowledgeBase is configured with transports appropriately.

Frame Expiration

By default, all saved frames with all timestamps are stored in the KnowledgeBase indefinitely. By setting frame expiry, old frames will be cleaned out as new ones are saved. You can set expiry in three ways:

  1. Pass the expiry age as a second parameter to ReferenceFrame::save()
  2. Set the expiry age for a frame ID, by calling ReferenceFrame::expiry() on a frame of that ID.
  3. Set a global default expiry, by calling ReferenceFrame::default_expiry(). This global default will apply to all new frame IDs, but not those that already exist.

When an expiry applies to a save operation, from any of the above (precedence going to first on the list), any frames older than the given age relative to the frame being saved will be removed from the KnowledgeBase. An expiry of -1, the default, will not expire any frames. If a frame is saved with timestamp -1, and expiry other than -1, all other copies of that frame ID will be deleted.

Note that deleting entries from the KnowledgeBase can be dangerous. The ReferenceFrame interface provided by GAMS ensures safety for accesses through itself, but if you access the variables directly vi Madara, be sure no outstanding VariableReference or Madara containers exist that reference the underlying Madara variables of the expiring ReferenceFrame.

FrameStore

The FrameStore object encapsulates the information needed to load and store frames, providing a minimal interface, in-place of the direct KnowledgeBase used in the interface described above. It is constructed with a KnowledgeBase value, optional prefix, and optional expiry. Call load/load_tree/save on the FrameStore object to use the saved values.

Producing Frames

Some threads might only need to produce ReferenceFrame objects and store them to the KnowledgeBase, such as a sensor polling thread. Such threads do not need to load any frames from the KnowledgeBase. You still need to specify the correct parent frame when saving a frame, though, but this parent frame need only have the correct ID. For example, suppose an encoder for a wheel sensor produces the frame id "LeftFrontWheel", with parent frame "LeftFrontAxle":

// These functions not provided by GAMS
double get_left front encoder();
uint64_t get_timestamp();

...

double angle = get_left_front_encoder();
uint64_t time = get_timestamp();
ReferenceFrame parent("LeftFrontAxle", Pose()); // fake parent frame
ReferenceFrame me("LeftFrontWheel", Orientation(parent, 0, 0, angle), time);
me.save(kb);

It doesn't matter what the parent's frame actual ancestry is, or what its timestamp is, or its relationship to its parent, as long as you don't need to save the parent itself.

Add a custom footer
Add a custom sidebar

Clone this wiki locally