Sim(3) pose graph

[Philippe-Antoine Gohard]

Hello everyone, thanks for accepting me in the group.

I'm currently developing a visual graph-SLAM system (monocular for instance) using GTSAM for optimizing the graph. Everything works like a charm for instance (fast and precise !), with a little drift at the end of the estimated trajectory. However, following Starsdat paper and thesis , I would like to correct (distribute) the accumulated drift around all the poses, using a sim(3) pose graph.

I saw that there is a similarity Lie group which is in the unstable module of gtsam, but I don't think it implements enough function following this (no compose, no between) to be used inside a pose graph.

My question is what should I use to do a sim(3) pose graph ? There is no compact form for the jacobian (the derivative of  log(Sij * exp(e)* Si *Sj^-1) |e=0 is not trivial!), and Strasdat use the Campbell-Baker-Hausdorff formula to have an approximation, but it doesn't seems better than using numerical derivatives (I don't have a great expertise on the subject).

Should I define my own type and use numerical derivatives? Or use the unstable actual one and do something I ignore with the between factor ?

Thanks in advance for considering this, and thanks for the great work you provide !

PS: I don't try the automatic differentiation, as if I understand correctly will need partial derivatives, thus needing the "general" jacobian (not the one with epsilon tends to 0 ) of logmap that I don't have.
 

[Dellaert, Frank]

Hi Philippe-Antoine,

Similarity3.h derives from LieGroup, and hence it does​ have all of these things.

Please look in testSimilarity3 for a number of examples.

Best

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Philippe-Antoine Gohard <philippe-an...@easymile.com>
Sent: Monday, February 8, 2021 09:43
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Sim(3) pose graph

 

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/7c91ac6c-0456-4988-99f2-48cd270e5e0bn%40googlegroups.com.

[Philippe-Antoine Gohard]

Hi Frank,

Thank you very much for this quick answer ! Okay that's perfect then, I'll use Similarity3.h.

Best regards,
P-A