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.

Tuning the world model: object propagation models

Goal

The world state estimate generated by wire depends on the probabilistic models underlying the world model. This tutorial shows how the process noise associated with the Kalman filter motion model influences the world state estimate. The three videos below show the output for three different settings. First, the process noise if rather low:

If the noise if increased a bit, the 'trust' in the motion model is lower and the output changes to:

And finally, with a high process noise, the motion model is considered not reliable and the occlusion is sufficient to confuse the world model:

Approach

For each of the measurements entering the world modeling algorithm, all possible data association explanations are considered:

• The measurement originates from an object not ye present in the world model (new object)
• The measurement originates from any of the objects already present in the world model
• The measurement represents a false positive (clutter)

As a result, a hypothesis tree in which each leaf represents a hypothesis and each hypothesis represents a possible world state is obtained. Each hypothesis gets a probability of being correct and the most probable hypothesis is given as resulting world state estimate.

For the calculation of the probabilities, a Bayesian update law is used. The probabilistic models representing the probability of measurements being any of the above options are crucial for the quality of the output of the algorithm. The models are defined in the models/world_object_models.xml file in the wire_core package and are object class dependent. For this tutorial, the focus lies on the motion model for tracking object positions:

   1 <knowledge>
   2 
   3     <prior_new value="0.14" />
   4     <prior_existing value="0.14" />
   5     <prior_clutter value="0.72" />
   6 
   7     <object_class name="object"> 
   8     
   9         <behavior_model attribute="position" model="wire_state_estimators/PositionEstimator">
  10             <pnew type="uniform" dimensions="3" density="0.0001" />            
  11             <pclutter type="uniform" dimensions="3" density="0.0001" />
  12 
  13             <param name="max_acceleration" value="8" />                
  14             <param name="kalman_timeout" value="1" />                
  15             <param name="fixed_pdf_cov" value="0.008" />                
  16         </behavior_model>
  17 
  18         <behavior_model attribute="color" model="wire_state_estimators/DiscreteEstimator">
  19             <pnew type="discrete" domain_size="100" />            
  20             <pclutter type="discrete" domain_size="100" />
  21         </behavior_model> 
  22 
  23         <behavior_model attribute="class_label" model="wire_state_estimators/DiscreteEstimator">
  24             <pnew type="discrete" domain_size="100" />            
  25             <pclutter type="discrete" domain_size="100" />
  26         </behavior_model>
  27 
  28         <behavior_model attribute="shape" model="wire_state_estimators/DiscreteEstimator">
  29             <pnew type="discrete" domain_size="10" />            
  30             <pclutter type="discrete" domain_size="10" />
  31         </behavior_model>
  32 
  33     </object_class>
  34     
  35 </knowledge>

The Models XML File Explained

Now, let's break the launch xml down.

   9         <behavior_model attribute="position" model="wire_state_estimators/PositionEstimator">
  10             <pnew type="uniform" dimensions="3" density="0.0001" />            
  11             <pclutter type="uniform" dimensions="3" density="0.0001" />
  12 
  13             <param name="max_acceleration" value="8" />                
  14             <param name="kalman_timeout" value="1" />                
  15             <param name="fixed_pdf_cov" value="0.008" />                
  16         </behavior_model>

The position will be estimated using a position estimator. This estimator, defined in the package [wire_state_estimators] is a multiple model estimator that combines (i) a Kalman filter with a constant velocity motion model with (ii) a fixed state with fixed uncertainty.

If updates follow each other relatively quickly, the Kalman filter is used to estimate the position. However, if no updates are received for kalman_timeout seconds, the Kalman filter makes place for a fixed state and uncertainty. This fixed state is defined by a Gaussian, the mean of which is based on the last estimated position, and the covariance is chosen to be fixed_pdf_cov.

In this tutorial, the fixed_pdf_cov parameter will be varied. By increasing this value, it is indicated that the position uncertainty of the object is larger, i.e., that the object may have moved a more significant amount. As a result, measurements far away from the the estimated position are more easily associated with the object. If the position uncertainty fixed_pdf_cov is chosen to be low, then object detections far away from the estimated state are less likely to be associated.

Data

In order to be able to reproduce the result shown in the video above, make sure that you have downloaded and compiled the wire packages:

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

Download the bag-file containing the data (demo04.bag) and decompress the file:

$ rosbag decompress demo04.bag

The bag file contains tfs, object detections and both rgb and depth images. The images are only included for ease of interpretation and inspection. These are not used by wire.

Reproducing the result

Start a ROS core:

$ roscore

and set the use_sim_time parameter to true:

$ rosparam set use_sim_time true

Then make sure the world_object_models.xml file in the models folder of the world_model package is configured correctly. For reproducing the first result, set the following fixed_pdf_cov in the behavior model for the position attribute for object_class object in line 16 above:

   1 <param name="fixed_pdf_cov" value="0.001" /> 

For reproducing the second result (switching hypotheses), increase the fixed_pdf_cov parameter:

   1 <param name="fixed_pdf_cov" value="0.008" /> 

For the third result (incorrect association), increase the fixed_pdf_cov even further:

   1 <param name="fixed_pdf_cov" value="0.01" /> 

To test the different parameter settings, start WIRE as follows (you will have to restart the wire_core every time you change the parameter since the parameters are loaded during start-up):

Launch the wire_core:

$ roslaunch wire_core start.launch

In a second terminal, launch the visualization:

$ roslaunch wire_tutorials rviz_wire_kinetic.launch

Finally, play back the data using rosbag_video:

$ rosbag play demo04.bag --clock

and inspect the results in RViz.