python - g2o - VertexSE3 和 VertexSE3Expmap 有什么区别？

Question

我对 g2o 库真的很陌生，我正在努力为顶点添加姿势:(

我使用了 g2o 的 python 版本，这是我编写的代码。

类型错误发生在这一行：v_se3.set_estimate(pose)

import g2o
import numpy as np

solver = g2o.BlockSolverSE3(g2o.LinearSolverEigenSE3())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
optimizer = g2o.SparseOptimizer()
optimizer.set_algorithm(solver)

trans = np.zeros(shape=[], dtype=np.float64)
quat = g2o.Quaternion(np.array([1., 0., 0., 0.], dtype=np.float64))
pose = g2o.SE3Quat(quat, trans)

v_se3 = g2o.VertexSE()        
v_se3.set_id(0)
v_se3.set_estimate(pose)         # <- ERROR OCCURS HERE!
optimizer.add_vertex(v_se3)

错误说

Type error: set_estimate(): incompatible function arguments. The following argument types are supported:
    1. (self:g2o.BaseVertex_6_Isometry3D, et:g2o.Isometry3d) -> None

我发现当我使用v_se3 = g2o.VertexSE3Expmap()而不是g2o.VertexSE3().

但我不确定究竟是什么g2o.VertexSE3Expmap()意思。

所以这些是我的问题，

1. 为什么VertexSE3()不在这里工作？

2. g2o.VertexSE3和 有什么区别g2o.VertexSE3Expmap？

3. 我应该怎么做才能用四元数（numpy数组）添加顶点？

谢谢

Answer 1

vertexse3 表示为 Isometry3d，而 vertexse3expmap 在内部使用变换矩阵进行参数化，在外部使用其指数映射进行参数化。

因此，如果您将姿势表示为 g2o.SE3Quat，则应使用 vertexse3expmap。