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,
Why
VertexSE3()
is not working here?What's difference between
g2o.VertexSE3
andg2o.VertexSE3Expmap
?What should I do to add vertex with the quaternion (numpy array)?
Thanks
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.