Leader: @jsola

Gradually substitute math implementations into using manif library