g2o - what's difference between VertexSE3 and VertexSE3Expmap?

938 views Asked by At

I'm really new to g2o library and I'm struggling to add pose to vertex :(

I used the python version of g2o and this is the code I wrote.

The type error occus in this line: 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)

The error says

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

I found that I can add vertex when I use v_se3 = g2o.VertexSE3Expmap() instead of g2o.VertexSE3().

But I'm not sure what g2o.VertexSE3Expmap() exactly means.

So these are my questions,

  1. Why VertexSE3() is not working here?

  2. What's difference between g2o.VertexSE3 and g2o.VertexSE3Expmap?

  3. What should I do to add vertex with the quaternion (numpy array)?

Thanks

1

There are 1 answers

1
Jerry123 On

vertexse3 is represented as an Isometry3d, while vertexse3expmap is parameterized internally with a transformation matrix and externally with its exponential map .

So, if you represent pose as g2o.SE3Quat, you shold use vertexse3expmap.