####################################
##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=
## descriptive title for the tutorial
## title = Advanced Parameter Tuning
## multi-line description to be displayed in search
## description = This tutorial tells you which parameter to change to improve performances
## 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= IntermediateCategory
## keywords = parameters
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>
<<TOC(4)>>
== Introduction ==
This tutorial is aimed at helping people using rtabmap in a more advanced way.
Only after you have successfully installed rtabmap and rtabmap_ros, you shall start this tutorial.
== Change Parameters ==
To know what parameters you can change, and what do they mean, you can use rtabmap gui and check the Preferences dialog or you can do:

{{{
rosrun rtabmap_ros rtabmap --params
rosrun rtabmap_ros rgbd_odometry --params
}}}

If you want to know a specific parameter, for example about g2o
{{{
$ rosrun rtabmap_ros rtabmap --params | grep g2o
Param: Optimizer/Robust = "false" [Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with "RGBD/OptimizeMaxError" if enabled.]
Param: Optimizer/Strategy = "2"   [Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.]
Param: g2o/Optimizer = "0"        [0=Levenberg 1=GaussNewton]
Param: g2o/PixelVariance = "1"    [Pixel variance used for SBA.]
Param: g2o/Solver = "0"           [0=csparse 1=pcg 2=cholmod]
}}}

There are generally three way of changing parameters.

1) Use a config.ini file.

The default config.ini should be automaticlly generated once you launch `rtabmap` through terminal by `"$ rtabmap"` command. Its location is `~/Documents/RTAB-Map/config.ini`.
You can copy this `config.ini` file into your own package and change your launch file `cfg` parameter into the corresponding location.

For example, you can set `Odom\Strategy=1` in `config.ini`, then in a package called `arti_vision`, you can put the `config.ini` in the config folder and renamed it into `rtabmap.ini`. Calling `rtabmap.launch` with the `cfg` argument:
{{{
<arg name="rtabmap_args" default="$(find arti_vision)/config/rtabmap.ini" />
}}}

2) Use <param> tag

This is the normal ROS way of reading parameters. For exmaple, you can put this under `rgbd_odometry` node
{{{
<param name="Odom/Strategy" value="0"/>
}}}

3) Add parameters in arguments of the node.

If you just want to temporarily play with some parameters, you can quickly put it under the `rtabmap_args` argument:
{{{
<arg name="rtabmap_args" default="--Odom/Strategy 0"/> 
}}}

All three way of changing parameters have the same effects. Choose whatever you want, though the second approach is the standard way to set parameters in ROS.

== Visual Odometry ==
Tuning visual odometry parameter is very important, it directly affects how good RTAB-Map generally performs. 
=== Increase Speed ===
If you have a very limited computing power, you probably want to play with the following parameters, so that you can increase the odometry frequency, and thus, the robot won't get lost.
Here are some parameters you should try:
==== Change the Visual Odometry Stategy into Frame to Frame ====
{{{
<!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) -->
<param name="Odom/Strategy" value="1"/>
}}}
==== Change the Matching Correspondences into Optical Flow ====

Optical flow may give more matches, but less robust correspondences:
{{{
<!-- Correspondences: 0=Features Matching, 1=Optical Flow -->
<param name="Vis/CorType" value="1"/>
}}}

==== Reduce the Maximum Features ====
{{{
<!-- maximum features map size, default 2000 -->
<param name="OdomF2M/MaxSize" type="string" value="1000"/> 
<!-- maximum features extracted by image, default 1000 -->
<param name="Vis/MaxFeatures" type="string" value="600"/>
}}}

==== Increase the Frame Rate of Camera and Reduce the Resolution ====
Those two aspects are very effective, increase your camera's frame rate directly affects how fast the VO can track your robot. Reduce resolution can greatly increase the speed of processing but may reduce the accuracy. You need to make a good balance based on your hardware and use case.

==== Change Feature Distance ====
If your images are 720p or more, you may want to increase GFTT distance (minimum distance between extracted features) so that you have more distributed features in the images:
{{{
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry">
	<param name="GFTT/MinDistance" type="string" value="10"/> <!-- default 5 pixels -->
</node>
}}}

=== Odometry Auto-Reset ===
When calling reset_odom service, rtabmap will start a new map, hiding the old one until a loop closure is found with the previous map. If you don't want rtabmap to start a new map when odometry is reset and wait until a first loop closure is found, you can set `Rtabmap/StartNewMapOnLoopClosure` to `true`.
If parameter `Odom/ResetCountdown` is set to `1` (default `0=disabled`), odometry will automatically reset one frame after being lost, i.e., it has the same effect than calling `reset_odom` service.
{{{
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry">
	<param name="Odom/ResetCountdown"              value="1" />
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
	<param name="Rtabmap/StartNewMapOnLoopClosure" value="true"/>
</node>
}}}

== Mapping ==
=== Three DOF mapping ===
For most of UGV, the vehicle only runs on a flat ground, in this way, you can force the visual odometry to track the vehicle in only 3DOF (x,y,theta) and increase the robustness of the map. For rtabmap, we can also constraint to 3 DoF loop closure detection and graph optimization:
{{{
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry">
	<param name="Reg/Force3DoF"    value="true" />
</node>

<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
	<param name="Reg/Force3DoF"    value="true" />
	<param name="Optimizer/Slam2D" value="true" />
</node>
}}}

=== Change inital costmap size for move_base ===
You can initialize the minimum size of the map with `grid_size` parameter:
{{{
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
   <param name="grid_size" type="double" value="50"/> <!-- 50 meters wide -->
</node>
}}}

=== Reduce Point Cloud Noises ===
{{{
<param name="cloud_noise_filtering_radius" value="0.05"/>
<param name="cloud_noise_filtering_min_neighbors" value="2"/>
}}}
Reference: [[http://docs.pointclouds.org/1.7.0/classpcl_1_1_radius_outlier_removal.html|pcl::RadiusOutlierRemoval()]]

=== Change Projected Occupancy Grid Characteristic ===
`proj_max_ground_angle` means mapping maximum angle between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.

Increasing `proj_max_ground_angle` will make the algorithm include points with normal's angle farther from z+ axis as ground.
{{{
<param name="proj_max_ground_angle" value="45"/>
}}}

`proj_max_ground_height` means mapping maximum height of points used for projection. Used for `proj_map` published topic.

Increasing the `proj_max_ground_height` will make the algorithm ignore points that are under this threshold while projection. All points below this threshold will be ignored:
{{{
<param name="proj_max_ground_height" value="0.1"/>
}}}

`proj_max_height` means mapping maximum height of points used for projection:

{{{
<param name="proj_max_height" value="2.0"/>
}}}

`proj_min_cluster_size` means mapping minimum cluster size to project the points. The distance between clusters is defined by `2*grid_cell_size`:
{{{
<param name="proj_min_cluster_size" value="20"/>
}}}

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