major refactoring: working with KDL and working for the two camera scenarios
get full repo