ANYbotics/kindr

boxPlus for local perturbations

Opened this issue · 1 comments

hdino commented

Hi,
As far as I can see, kindr implements only a boxPlus operation for global perturbations, i.e.
Phi_2 = exp(v)*Phi_1

Do I overlook something or would it (maybe) make sense to add a boxPlusLocal operation? I.e.
Phi_2 = Phi_1*exp(v)

At the moment one has to create an auxiliary rotation that is applied manually afterwards.

You can emulate it using double inversion if you have the extra CPU time:
Phi_1*exp(v) = inv(inv(exp(v)) * inv(Phi_1)) = inv(exp(-v) * inv(Phi_1)) = inv(inv(Phi_1) BOXPLUS (-v))
Another way is to rotate v :
It should hold:
Phi_1 * exp(v)= exp(Phi_1(v)) Phi_1
Is that simpler than what you are currently doing?