Note: This tutorial assumes that you have completed the previous tutorials: ROS tutorials.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Using the estimated world state

Description: This tutorial explains how to interpret and access the world model output by means of a minimal example.

Tutorial Level: INTERMEDIATE

Goal

This tutorial will show how the world model can be fed with evidence.

Approach

Wire generates a world state estimate represemted by the message type wire_msgs/WorldState. Converting the world state to other formats can easily be done using problib in at most a few lines of code. A detailed example present in the source folder of the wire_tutorials package shows how this can be done.

   1 #include "ros/ros.h"
   2 #include "wire_msgs/ObjectState.h"
   3 #include "wire_msgs/WorldState.h"
   4 
   5 #include "problib/conversions.h"
   6 
   7 #include <vector>
   8 
   9 using namespace std;
  10 
  11 
  12 // Function that extract the Gaussian with the highest weight from a mixture of Gaussians and/or a uniform distribution
  13 const pbl::Gaussian* getBestGaussianFromMixture(const pbl::Mixture& mix, double min_weight = 0) {
  14         const pbl::Gaussian* G_best = 0;
  15         double w_best = min_weight;
  16 
  17         // Iterate over all components
  18         for(int i = 0; i < mix.components(); ++i) {
  19                 const pbl::PDF& pdf = mix.getComponent(i);
  20                 const pbl::Gaussian* G = pbl::PDFtoGaussian(pdf);
  21                 double w = mix.getWeight(i);
  22 
  23                 // Get Gaussian with the highest associated weight
  24                 if (G && w > w_best) {
  25                         G_best = G;
  26                         w_best = w;
  27                 }
  28         }
  29         return G_best;
  30 }
  31 
  32 
  33 
  34 void worldStateCallback(const wire_msgs::WorldState::ConstPtr& msg) {
  35 
  36         ROS_INFO("Received world state with %d objects", msg->objects.size());
  37 
  38 
  39         // Iterate over world model objects
  40         for (vector<wire_msgs::ObjectState>::const_iterator it_obj = msg->objects.begin(); it_obj != msg->objects.end(); ++it_obj) {
  41 
  42                 // Get object from the object array
  43                 wire_msgs::ObjectState obj = *it_obj;
  44                 ROS_INFO(" Object:");
  45 
  46                 // Iterate over all properties
  47                 for (vector<wire_msgs::Property>::const_iterator it_prop = obj.properties.begin(); it_prop != obj.properties.end(); ++it_prop) {
  48 
  49                         // Get the pdf of the current property
  50                         pbl::PDF* pdf = pbl::msgToPDF(it_prop->pdf);
  51 
  52                         if (pdf) {
  53 
  54                                 //// Check the attribute
  55 
  56                                 // Object class
  57                                 if (it_prop->attribute == "class_label") {
  58                                         string label = "";
  59                                         pdf->getExpectedValue(label);
  60 
  61                                         // Get probability
  62                                         const pbl::PMF* pmf = pbl::PDFtoPMF(*pdf);
  63                                         double p = pmf->getProbability(label);
  64                                         ROS_INFO(" - class %s with probability %f", label.c_str(), p);
  65                                 }
  66                                 // Position
  67                                 else if (it_prop->attribute == "position") {
  68                                         const pbl::Mixture* pos_pdf = pbl::PDFtoMixture(*pdf);
  69                                         if (pos_pdf) {
  70                                                 const pbl::Gaussian* pos_gauss = getBestGaussianFromMixture(*pos_pdf);
  71                                                 if (pos_gauss) {
  72                                                         const pbl::Vector& pos = pos_gauss->getMean();
  73                                                         ROS_INFO(" - position: (%f,%f,%f)", pos(0), pos(1), pos(2));
  74                                                         ROS_INFO(" - diagonal position cov: (%f,%f,%f)",
  75                                                                         pos_gauss->getCovariance()(0, 0), pos_gauss->getCovariance()(1, 1), pos_gauss->getCovariance()(2, 2));
  76                                                 }
  77                                         } else {
  78                                                 ROS_INFO(" - position: object position unknown (uniform distribution)");
  79                                         }
  80                                 }
  81                                 // Orientation
  82                                 else if (it_prop->attribute == "orientation") {
  83                                         const pbl::Mixture* orientation_pdf = pbl::PDFtoMixture(*pdf);
  84                                         if (orientation_pdf) {
  85                                                 const pbl::Gaussian* orientation_gauss = getBestGaussianFromMixture(*orientation_pdf);
  86                                                 if (orientation_gauss) {
  87                                                         const pbl::Vector& ori = orientation_gauss->getMean();
  88                                                         ROS_INFO(" - orientation: (%f,%f,%f,%f)", ori(0), ori(1), ori(2), ori(3));
  89                                                 }
  90                                         } else {
  91                                                 ROS_INFO(" - orientation: object orientation unknown (uniform distribution)");
  92                                         }
  93                                 }
  94                                 // Color
  95                                 else if (it_prop->attribute == "color") {
  96                                         string color = "";
  97                                         pdf->getExpectedValue(color);
  98                                         ROS_INFO(" - color: %s", color.c_str());
  99                                 }
 100 
 101                                 delete pdf;
 102                         }
 103 
 104                 }
 105 
 106         }
 107 
 108 }
 109 
 110 int main(int argc, char **argv) {
 111         ros::init(argc, argv, "get_world_state");
 112         ros::NodeHandle nh;
 113 
 114         // Subscribe to the world model
 115         ros::Subscriber sub_obj = nh.subscribe("/world_state", 1000, &worldStateCallback);
 116 
 117         ros::spin();
 118         return 0;
 119 }

The code explained

   1 #include "ros/ros.h"
   2 #include "wire_msgs/ObjectState.h"
   3 #include "wire_msgs/WorldState.h"
   4 
   5 #include "problib/conversions.h"
   6 

Include the appropriate header files.

  12 // Function that extract the Gaussian with the highest weight from a mixture of Gaussians and/or a uniform distribution
  13 const pbl::Gaussian* getBestGaussianFromMixture(const pbl::Mixture& mix, double min_weight = 0) {
  14         const pbl::Gaussian* G_best = 0;
  15         double w_best = min_weight;
  16 
  17         // Iterate over all components
  18         for(int i = 0; i < mix.components(); ++i) {
  19                 const pbl::PDF& pdf = mix.getComponent(i);
  20                 const pbl::Gaussian* G = pbl::PDFtoGaussian(pdf);
  21                 double w = mix.getWeight(i);
  22 
  23                 // Get Gaussian with the highest associated weight
  24                 if (G && w > w_best) {
  25                         G_best = G;
  26                         w_best = w;
  27                 }
  28         }
  29         return G_best;
  30 }

This function gets a mixture of densities as an input and returns the Gaussian distribution associated with the highest weight (both defined in problib). Mixtures are commonly provided by the world model and a mixture can, besides Gaussian distributions, contain a uniform distribution that represents 'object is lost' (we know it must be somewhere, we just do not know where). If the mixture only contains a uniform distribution, G_best will be 0 and the object indeed is lost. This may happen as a result of a high number of propagation steps without intermediate updates combined with a Kalman filter motion model that is associated with a significant amount of uncertainty (process model noise covariance matrix).

  39         // Iterate over world model objects
  40         for (vector<wire_msgs::ObjectState>::const_iterator it_obj = msg->objects.begin(); it_obj != msg->objects.end(); ++it_obj) {
  41 
  42                 // Get object from the object array
  43                 wire_msgs::ObjectState obj = *it_obj;

Iterate over all objects in the world state estimate and get the current object.

  47                 for (vector<wire_msgs::Property>::const_iterator it_prop = obj.properties.begin(); it_prop != obj.properties.end(); ++it_prop) {
  48 
  49                         // Get the pdf of the current property
  50                         pbl::PDF* pdf = pbl::msgToPDF(it_prop->pdf);

Iterate over all properties associated with this object and get the probability density function representing the current property. This probability density function can either be continuous or discrete.

  52                         if (pdf) {

Only proceed if a valid probability density function is obtained.

  57                                 if (it_prop->attribute == "class_label") {

If the current property represents the discrete attribute 'class_label'.

  58                                         string label = "";
  59                                         pdf->getExpectedValue(label);

Get the most probable value from the probability mass function representing the possible class labels.

  62                                         const pbl::PMF* pmf = pbl::PDFtoPMF(*pdf);
  63                                         double p = pmf->getProbability(label);
  64                                         ROS_INFO(" - class %s with probability %f", label.c_str(), p);

Get the probability associated with the most probable class label in the probability mass function representing the class label and print the result.

  67                                 else if (it_prop->attribute == "position") {

If the current property represents the (continuous) attribute position.

  68                                         const pbl::Mixture* pos_pdf = pbl::PDFtoMixture(*pdf);

Get the mixture representing the current position.

  70                                                 const pbl::Gaussian* pos_gauss = getBestGaussianFromMixture(*pos_pdf);

Get the most probable Gaussian distribution from the mixture.

  71                                                 if (pos_gauss) {
  72                                                         const pbl::Vector& pos = pos_gauss->getMean();
  73                                                         ROS_INFO(" - position: (%f,%f,%f)", pos(0), pos(1), pos(2));
  74                                                         ROS_INFO(" - diagonal position cov: (%f,%f,%f)",
  75                                                                         pos_gauss->getCovariance()(0, 0), pos_gauss->getCovariance()(1, 1), pos_gauss->getCovariance()(2, 2));
  76                                                 }

If a Gaussian distribution is found, get the mean and the covariance matrix and print the result.

  77                                         } else {
  78                                                 ROS_INFO(" - position: object position unknown (uniform distribution)");
  79                                         }

If no Gaussian can be extracted from the mixture, the object is lost and the position is represented by a uniform distribution.

  81                                 // Orientation
  82                                 else if (it_prop->attribute == "orientation") {
  83                                         const pbl::Mixture* orientation_pdf = pbl::PDFtoMixture(*pdf);
  84                                         if (orientation_pdf) {
  85                                                 const pbl::Gaussian* orientation_gauss = getBestGaussianFromMixture(*orientation_pdf);
  86                                                 if (orientation_gauss) {
  87                                                         const pbl::Vector& ori = orientation_gauss->getMean();
  88                                                         ROS_INFO(" - orientation: (%f,%f,%f,%f)", ori(0), ori(1), ori(2), ori(3));
  89                                                 }
  90                                         } else {
  91                                                 ROS_INFO(" - orientation: object orientation unknown (uniform distribution)");
  92                                         }
  93                                 }

If the current property is the (continuous) property orientation, get the mean and covariance matrix of the most probable orientation in the mixture and print the result.

  94                                 // Color
  95                                 else if (it_prop->attribute == "color") {
  96                                         string color = "";
  97                                         pdf->getExpectedValue(color);
  98                                         ROS_INFO(" - color: %s", color.c_str());
  99                                 }

If the current property is the discrete property color, get the most probable value and print the result.

Reproducing the result

Make sure that you have downloaded and compiled the wire packages:

$ git clone https://github.com/tue-robotics/wire.git
$ catkin_make

Start a ros core

$ roscore

and launch the wire_core:

$ roslaunch wire_core start.launch

Now start the node created in this tutorial:

$ rosrun wire_tutorials generate_evidence

which creates dummy detections and start the node explained above:

$ rosrun wire_tutorials get_world_state

The following output should appear:

[ INFO] [1359389964.402146962]: Received world state with 0 objects
[ INFO] [1359389964.452492126]: Received world state with 1 objects
[ INFO] [1359389964.452530330]:  Object:
[ INFO] [1359389964.452567764]:  - position: object position unknown (uniform distribution)
[ INFO] [1359389964.452616793]:  - color: red
[ INFO] [1359389964.452654996]:  - class mug with probability 0.998148
[ INFO] [1359389964.502084040]: Received world state with 1 objects
[ INFO] [1359389964.502122104]:  Object:
[ INFO] [1359389964.502146060]:  - position: object position unknown (uniform distribution)
[ INFO] [1359389964.502168548]:  - color: red
[ INFO] [1359389964.502193342]:  - class mug with probability 0.998148

Wiki: wire/Tutorials/Using the estimated world state (last edited 2017-05-17 18:48:14 by JosElfring)