Parental links

Authors:

jsola@laas.fr, nmansard@laas.fr

Introduction and philosophy

The hierarchical organization of objects in RTSLAM is defined family-tree-like. The family-tree differs from a regular tree in that an object can have more than one parent (ie. a father and a mother, please keep reading).

• The parents may have none, one or more children.
• The children have a backwards link to their parent.
• The Observations link Sensors to Landmarks and are the only objects to have both mother (eg. the Sensor) and father (eg. the Landmark).

The figure below illustrates the family-tree organization for the case of a map with 2 robots and 2 landmarks, one robot with one sensor and the other one with 2 sensors.

The simplified family tree hierarchical organization

This is actually a simplified view of RTSLAM. In order to generically manage different types of sensors and landmarks, some managers have been introduced:

• MapManager that manages the landmarks of some type in a map
• DataManager that manages a sensor and landmark type at the data processing level

The full family tree hierarchical organization

It is important to notice the following facts:

• An imaginary line going through the sensor and the observations separates the parts of the SLAM problem that correspond to the state space (Map, Robot, Sensor, Map Manager, Landmark) and the measurement space (Sensor, Data Manager, Observation).

Each Observation creates a loop of links. All of such loops are visible in the figure. For example, the loop highlighted in red: M1->R2->S2->DM2->O4->L2->MM1->M1.

To account for these relationships, the rtslam uses lists of pointers to children, and a single pointer to a parent. Yes, each link in the graph is stored twice. This allows systematic forward and backward referencing between objects. This figure shows the general implementation of some of these bi-directional links.

Bi-directional parental links. A parent has a list of pointers to children. A child has a pointer to his parent.

You will discover these triangular loops in the collaboration diagrams of this documentation. Check this one jafar::rtslam::MapAbstract for an example.

In our implementation, if we have a pointer mapPtr to the map at the base level, loops can be traversed clockwise or counter-clockwise. The parent->child links are to be traversed by means of iterators.

This code traverses the complete graph (extracted from demo_slam.cpp):

Taking arbitrary fractions of such loops allow us to access information at any part of the parental hierarchy. For example, from a pointer to observation 4, we can know which robot was observing which landmark:

  obs4ptr->sensor->robot->id()     // This is the ID of the landmark: 2 in this case (see figure)
  obs4Ptr->landmark->id()          // This is the ID of the robot   : 2 in this case

NOTE: The drawback or danger of this bidirectional linkage is that it allows access to any object of the system from any other object. Be careful.

Here is an example of the Robot class. Observe the maps of links to sensors and the link to the father map:

  class RobotAbstract
  {
    public:
      string                        name;           // Name of robot.
      string                        type;           // Type of robot.
      Control                       control;        // Control input.
      map<size_t, SensorAbstract*>  sensors;        // List of pointers to sensors.
      MapAbstract                   * map;          // Pointer to father Map.
      State                         state;          // State vector, pointing to the SLAM map.
      virtual void                  set_control();  // Set control input.
      virtual void                  move() = 0;     // Move the robot.
  };

Implementation using boost smart pointers

We use boost:: smart pointers to implement suck links.

• Links from father to child use shared_ptr. The sets of links are implemented with map as stated. Therefore we find map<type_t,shared_ptr<CHILD>>.
• Links from child to parent use weak_ptr.
• At the Observation level, we use downcasting to convert pointers of abstract types into specific type pointers.

Check out this figure:

Types of pointers used in the rtslam hierarchy

Traversing downwards: links PARENT -> CHILD.

Traversing downwards is straightforward:

  robotPtr->sensorsPtrSet[1]->pose.x()

is the pose of sensor 1, accessed from the robot that owns it. In this case, we used the sensor ID to access the element in the sensorsPtrSet map. Usually, we will iterate all sensors. The code would look then more like this:

  typedef boost::shared_ptr<SensorAbstract> sensor_ptr_t;
  typedef map<size_t, sensor_ptr_t> sensors_ptr_set_t;
  sensors_ptr_set_t sensorsPtrSet;
  /* ... */
  for (sensors_ptr_set_t::iterator senIter = sensorsPtrSet.begin(); senIter != sensorsPtrSet.end(); senIter++) { // loop sensors
    sensor_ptr_t senPtr = senIter->second;
    cout << "exploring sensor: " << senPtr->id() << endl;
    senPtr->doSomething();
  }

Traversing upwards: links CHILD -> PARENT.

Traversing upwards is a bit more tricky because weak_ptr needs to lock the operation of shared_ptr before accessing the object. We encapsulated the necessary code into a convenient function with the name of the pointer, such that the only difference you see is the aparition of a couple of empty brackets. For example:

  senPtr->robotPtr()->state.x()

is the state of the robot owning a particular sensor, accessed from that sensor.

Downcasting to access derived class parameters. Links DERIVED_OBS -> DERIVED_PARENT.

The observation class needs to access information in Sensor and Landmark that only exist in the derived versions. For example, observing an AHP point from a Pin-hole camera requires knowledge about the sensor's intrinsic parameters. But these parameters do not exist at the Abstract level.

We add a couple of pointers to each derived observation class, especially dedicated to each particular parent. These pointers are a downcast of the ones at the abstract level.

The downcast is done at construction time of the object ObservationPinHoleAnchoredHomogeneousPoint, with something like this:

  // Some type definitions
  typedef weak_ptr<SensorPinHole> pinhole_ptr_t;
  typedef weak_ptr<LandmarkAnchoredHomogeneousPoint> ahp_ptr_t;
  // Members of the derived observation class 
  pinhole_ptr_t pinholePtr;
  ahp_ptr_t ahpPtr;
  // At construction time of the derived observation object 
  pinholePtr = dynamic_pointer_cast<SensorPinHole> (sensorPtr);
  ahpPtr = dynamic_pointer_cast<LandmarkAnchoredHomogeneousPoint> (landmarkPtr);

With this downcast, we can now access the parameters of the sensor. For example, this ficticious function:

  vec3 p = ahpPtr->convertToEuclidean();
  vec2 u = ObsAHP.project(pinholePtr->intrinsic, p);

converts the AHP into an Euclidean point, and projects it into the pin-hole sensor.