Graph SLAM with python, the pose is represented by Lie algebra se3 and Lie group SE3. The expm and logm function contribute a lot of bottleneck of the algorithm. Reference: https://github.com/rsasaki0109/PoseOptimizationSLAM3D https://github.com/gaoxiang12/slambook
Result:
