Debug bone length support for marker optimization, re-integrate old code