Optimize integrators, Rviz plugin and add usage examples #700

Job	Run time
Define common variables	4s
Lint	45s
Build CI workspace container	1m 28s
Build docs	1m 38s
Build	1m 4s
Test	1m 33s
Catkin install	2m 35s
Clang tidy	7m 32s
Sanitize undefined behavior	8m 17s
Sanitize addressability and leaks	6m 15s
Valgrind memcheck	9m 48s
Coverage	0s
	40m 59s

Provide feedback