Fix/orientation optimizer

Corvin Navarro Eckerrequested to merge
fix/orientation_optimizer into master

Plotting of the test cases in orientation_optimizer_test.cpp can be easily enabled by commenting in 5 code sections marked by "//COMMENT IN FOR TEST CASE PLOTTING"

Edited by Tobias Gröger

Merge request reports