g2o 代码学习—— exp map and log map for SE(3), SIM(3)

我一般不咋用 g2o，我用 ceres-solver。有人 g2o 的代码看不懂，问我。所以我就帮忙看一下。

sim(3) 的 exponential map 是一个有意思的东西，推了一下。主要参考 [1]，辅助参考 [2]，也从 Wikipedia 拿了一些简单的结论来用。

1. g2o SE(3)

2. g2o SIM(3)

Reference

[1] Allen–Blanchette, Christine, Spyridon Leonardos, and Jean Gallier. "Motion interpolation in sim (3)." (2014).

[2] Barfoot, Timothy D. State Estimation for Robotics. Cambridge University Press, 2017.