Quite a bit of progress on re-writing kinfu with poses. All the setup is done, now I just need to write the callback function / main loop.