## page was renamed from world_modeling/Tutorials/Tuning the world model: object propagation models
## page was renamed from ros.org/wiki/world_modeling/Tutorials/Tuning the world model: object propagation models
## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0=[[ROS/Tutorials|ROS tutorials]]
## descriptive title for the tutorial
## title = Tuning the world model: object propagation models
## multi-line description to be displayed in search 
## description = The models used for object propagation determine the probability of associations between predicted object positions and measured object positions. This tutorial shows how changing the model influences the world state estimate.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= AdvancedCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>


== 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:

<<Youtube(ryQVijBuDe0)>>

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

<<Youtube(S9x3C7upt44)>>

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

<<Youtube(WRvchzmJ8QI)>>

== 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:

{{{
#!python block=objectmodels
<knowledge>

    <prior_new value="0.14" />
    <prior_existing value="0.14" />
    <prior_clutter value="0.72" />

    <object_class name="object"> 
    
        <behavior_model attribute="position" model="wire_state_estimators/PositionEstimator">
            <pnew type="uniform" dimensions="3" density="0.0001" />            
            <pclutter type="uniform" dimensions="3" density="0.0001" />

            <param name="max_acceleration" value="8" />                
            <param name="kalman_timeout" value="1" />                
            <param name="fixed_pdf_cov" value="0.008" />                
        </behavior_model>

        <behavior_model attribute="color" model="wire_state_estimators/DiscreteEstimator">
            <pnew type="discrete" domain_size="100" />            
            <pclutter type="discrete" domain_size="100" />
        </behavior_model> 

        <behavior_model attribute="class_label" model="wire_state_estimators/DiscreteEstimator">
            <pnew type="discrete" domain_size="100" />            
            <pclutter type="discrete" domain_size="100" />
        </behavior_model>

        <behavior_model attribute="shape" model="wire_state_estimators/DiscreteEstimator">
            <pnew type="discrete" domain_size="10" />            
            <pclutter type="discrete" domain_size="10" />
        </behavior_model>

    </object_class>
    
</knowledge>

}}}

=== The Models XML File Explained ===
Now, let's break the launch xml down.

<<CodeRef(objectmodels,9,16)>>
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 ([[attachment:demo04.bag]]) and decompress the file:
{{{
$ rosbag decompress demo04.bag
}}}

The bag file contains [[tf]]s, 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 [[Clock#Using_Simulation_Time_from_the_.2BAC8-clock_Topic|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:
{{{
#!python block=objectmodels_lowacc
<param name="fixed_pdf_cov" value="0.001" /> 
}}}

For reproducing the second result (switching hypotheses), increase the ''fixed_pdf_cov'' parameter:
{{{
#!python block=objectmodels_lowacc
<param name="fixed_pdf_cov" value="0.008" /> 
}}}

For the third result (incorrect association), increase the ''fixed_pdf_cov'' even further:
{{{
#!python block=objectmodels_lowacc
<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.

## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE