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)