ANYbotics / kindr

Kinematics and Dynamics for Robotics
BSD 3-Clause "New" or "Revised" License
552 stars 193 forks source link

boxPlus for local perturbations #81

Open hdino opened 8 years ago

hdino commented 8 years ago

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.

HannesSommer commented 8 years ago

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?